brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Speed limit controller

Browse Source 
Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself.

Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
Co-Authored-By: Efini <19368997+efini@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
Co-Authored-By: Jason Wen <haibin.wen3@gmail.com>
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent 6907410750
commit 3a926ddfbe
23 changed files with 703 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
selfdrive/ui/qt/onroad.cc
View File

@@ -128,7 +128,18 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
QRect hideSpeedRect(rect().center().x() - 175, 50, 350, 350);
bool isSpeedClicked = hideSpeedRect.contains(e->pos());
if (isMaxSpeedClicked || isSpeedClicked) {
// Speed limit confirmation buttons
QSize size = this->size();
QRect leftRect(0, 0, size.width() / 2, size.height());
QRect rightRect = leftRect.translated(size.width() / 2, 0);
bool isLeftSideClicked = leftRect.contains(e->pos()) && scene.speed_limit_changed;
bool isRightSideClicked = rightRect.contains(e->pos()) && scene.speed_limit_changed;
// Speed limit offset button
QRect speedLimitRect(7, 250, 225, 225);
bool isSpeedLimitClicked = speedLimitRect.contains(e->pos());
if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) {
if (isMaxSpeedClicked && scene.reverse_cruise_ui) {
bool currentReverseCruise = scene.reverse_cruise;
@@ -140,8 +151,19 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
uiState()->scene.hide_speed = !currentHideSpeed;
params.putBoolNonBlocking("HideSpeed", !currentHideSpeed);
} else if (isSpeedLimitClicked && scene.show_slc_offset_ui) {
bool currentShowSLCOffset = scene.show_slc_offset;
uiState()->scene.show_slc_offset = !currentShowSLCOffset;
params.putBoolNonBlocking("ShowSLCOffset", !currentShowSLCOffset);
}
widgetClicked = true;
} else if (isLeftSideClicked || isRightSideClicked) {
bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive;
paramsMemory.putBool("SLCConfirmed", slcConfirmed);
paramsMemory.putBool("SLCConfirmedPressed", true);
widgetClicked = true;
// If the click wasn't for anything specific, change the value of "ExperimentalMode"
} else if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) {
@@ -490,11 +512,14 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit = slcOverridden ? scene.speed_limit_overridden_speed : speedLimitController ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
if (speedLimitController && !slcOverridden) {
speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
}
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign);
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) || (speedLimitController && useViennaSLCSign);
is_metric = s.scene.is_metric;
speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph");
hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE || customSignals && (turnSignalLeft || turnSignalRight)) || fullMapOpen || showDriverCamera;
@@ -527,6 +552,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
QString speedStr = QString::number(std::nearbyint(speed));
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
@@ -600,11 +626,23 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.setPen(QPen(blackColor(), 6));
p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
p.save();
p.setOpacity(slcOverridden ? 0.25 : 1.0);
if (speedLimitController && showSLCOffset && !slcOverridden) {
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
p.setFont(InterFont(50, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
} else {
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
}
p.restore();
}
// EU (Vienna style) sign
@@ -615,10 +653,21 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.setPen(QPen(Qt::red, 20));
p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
p.save();
p.setOpacity(slcOverridden ? 0.25 : 1.0);
p.setPen(blackColor());
p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
if (showSLCOffset) {
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr);
p.setFont(InterFont(40, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
} else {
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
}
p.restore();
}
p.restore();
}
// current speed
@@ -1206,10 +1255,19 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
pedalsOnUI = scene.pedals_on_ui;
rightHandDrive = scene.right_hand_drive;
roadNameUI = scene.road_name_ui;
showDriverCamera = scene.show_driver_camera;
speedLimitController = scene.speed_limit_controller;
showSLCOffset = speedLimitController && scene.show_slc_offset;
slcOverridden = speedLimitController && scene.speed_limit_overridden;
slcSpeedLimit = scene.speed_limit;
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
useViennaSLCSign = scene.use_vienna_slc_sign;
turnSignalLeft = scene.turn_signal_left;
turnSignalRight = scene.turn_signal_right;
@@ -1230,6 +1288,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
} else if (animationTimer->isActive()) {
animationTimer->stop();
}
if (scene.speed_limit_changed) {
drawSLCConfirmation(p);
}
}
bool enableCompass = compass && !hideBottomIcons;
@@ -1561,6 +1623,39 @@ void PedalIcons::paintEvent(QPaintEvent *event) {
p.drawPixmap(gasX, (height() - img_size) / 2, gas_pedal_img);
}
void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) {
p.save();
// Get screen size
QSize size = this->size();
// Configure the screen halves
QRect leftRect(0, 0, size.width() / 2, size.height());
QRect rightRect = leftRect.translated(size.width() / 2, 0);
// Fill in the screen halves
p.setOpacity(0.5);
p.fillRect(leftRect, rightHandDrive ? redColor() : greenColor());
p.fillRect(rightRect, rightHandDrive ? greenColor() : redColor());
p.setOpacity(1.0);
// Configure the font
p.setFont(InterFont(75, QFont::Bold));
p.setPen(Qt::white);
// Configure the text
QString unitText = is_metric ? "kph" : "mph";
QString speedText = QString::number(std::nearbyint(scene.unconfirmed_speed_limit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText;
QString confirmText = "Confirm speed limit\n" + speedText;
QString ignoreText = "Ignore speed limit\n" + speedText;
// Draw the text
p.drawText(leftRect, Qt::AlignCenter | Qt::AlignTop, rightHandDrive ? ignoreText : confirmText);
p.drawText(rightRect, Qt::AlignCenter | Qt::AlignTop, rightHandDrive ? confirmText : ignoreText);
p.restore();
}
void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
p.save();

8
selfdrive/ui/qt/onroad.h
View File

@@ -158,6 +158,7 @@ private:
void updateFrogPilotWidgets(QPainter &p);
void drawLeadInfo(QPainter &p);
void drawSLCConfirmation(QPainter &p);
void drawStatusBar(QPainter &p);
void drawTurnSignals(QPainter &p);
@@ -183,15 +184,22 @@ private:
bool leadInfo;
bool mapOpen;
bool pedalsOnUI;
bool rightHandDrive;
bool roadNameUI;
bool showDriverCamera;
bool showSLCOffset;
bool slcOverridden;
bool speedLimitController;
bool turnSignalLeft;
bool turnSignalRight;
bool useViennaSLCSign;
float cruiseAdjustment;
float distanceConversion;
float laneWidthLeft;
float laneWidthRight;
float slcSpeedLimit;
float slcSpeedLimitOffset;
float speedConversion;
int availableImages;

19
selfdrive/ui/ui.cc
View File

@@ -247,11 +247,18 @@ static void update_state(UIState *s) {
scene.enabled = controlsState.getEnabled();
scene.experimental_mode = controlsState.getExperimentalMode();
}
if (sm.updated("driverMonitoringState")) {
auto driverMonitoringState = sm["driverMonitoringState"].getDriverMonitoringState();
scene.right_hand_drive = driverMonitoringState.getIsRHD();
}
if (sm.updated("frogpilotCarControl")) {
auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
if (scene.always_on_lateral) {
scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
}
if (scene.speed_limit_controller) {
scene.speed_limit_changed = frogpilotCarControl.getSpeedLimitChanged();
}
}
if (sm.updated("frogpilotPlan")) {
auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
@@ -265,6 +272,13 @@ static void update_state(UIState *s) {
scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock();
scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor();
}
if (scene.speed_limit_controller) {
scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed();
scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
}
scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
}
if (sm.updated("liveLocationKalman")) {
@@ -368,6 +382,11 @@ void ui_update_frogpilot_params(UIState *s) {
scene.screen_timeout = screen_management ? params.getInt("ScreenTimeout") : 30;
scene.screen_timeout_onroad = screen_management ? params.getInt("ScreenTimeoutOnroad") : 10;
scene.standby_mode = screen_management && params.getBool("StandbyMode");
scene.speed_limit_controller = params.getBool("SpeedLimitController");
scene.show_slc_offset = scene.speed_limit_controller && params.getBool("ShowSLCOffset");
scene.show_slc_offset_ui = scene.speed_limit_controller && params.getBool("ShowSLCOffsetUI");
scene.use_vienna_slc_sign = scene.speed_limit_controller && params.getBool("UseVienna");
}
void UIState::updateStatus() {

11
selfdrive/ui/ui.h
View File

@@ -212,10 +212,16 @@ typedef struct UIScene {
bool pedals_on_ui;
bool reverse_cruise;
bool reverse_cruise_ui;
bool right_hand_drive;
bool road_name_ui;
bool rotating_wheel;
bool screen_recorder;
bool show_driver_camera;
bool show_slc_offset;
bool show_slc_offset_ui;
bool speed_limit_changed;
bool speed_limit_controller;
bool speed_limit_overridden;
bool standby_mode;
bool standstill;
bool status_changed;
@@ -224,6 +230,7 @@ typedef struct UIScene {
bool turn_signal_right;
bool unlimited_road_ui_length;
bool use_si;
bool use_vienna_slc_sign;
float acceleration;
float adjusted_cruise;
@@ -233,6 +240,10 @@ typedef struct UIScene {
float path_edge_width;
float path_width;
float road_edge_width;
float speed_limit;
float speed_limit_offset;
float speed_limit_overridden_speed;
float unconfirmed_speed_limit;
int bearing_deg;
int camera_view;