From 36a87e664c5461ba380b4b550933118f9fcb336a Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 19 Jul 2024 21:28:37 -0700 Subject: [PATCH] Controls - Speed Limit Controller - Override Method --- .../frogpilot/controls/frogpilot_planner.py | 18 ++++++++++++++++++ selfdrive/ui/qt/onroad/annotated_camera.cc | 11 ++++++++--- selfdrive/ui/qt/onroad/annotated_camera.h | 1 + selfdrive/ui/ui.cc | 2 ++ selfdrive/ui/ui.h | 2 ++ 5 files changed, 31 insertions(+), 3 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 445383564..ce200c589 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -51,6 +51,7 @@ class FrogPilotPlanner: self.forcing_stop = False self.model_stopped = False self.override_force_stop = False + self.override_slc = False self.slower_lead = False self.tracking_lead = False @@ -58,6 +59,7 @@ class FrogPilotPlanner: self.danger_jerk = 0 self.model_length = 0 self.mtsc_target = 0 + self.overridden_speed = 0 self.road_curvature = 0 self.slc_target = 0 self.speed_jerk = 0 @@ -245,6 +247,20 @@ class FrogPilotPlanner: self.params_memory.put_bool("SLCConfirmed", False) else: self.slc_target = unconfirmed_slc_target + + self.override_slc = self.overridden_speed > self.slc_target + self.override_slc |= carState.gasPressed and v_ego > self.slc_target + self.override_slc &= controlsState.enabled + + if self.override_slc: + if frogpilot_toggles.speed_limit_controller_override_manual: + if carState.gasPressed: + self.overridden_speed = v_ego + v_ego_diff + self.overridden_speed = clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff) + elif frogpilot_toggles.speed_limit_controller_override_set_speed: + self.overridden_speed = v_cruise + v_cruise_diff + else: + self.overridden_speed = 0 else: self.slc_target = 0 @@ -294,6 +310,8 @@ class FrogPilotPlanner: frogpilotPlan.maxAcceleration = float(self.max_accel) frogpilotPlan.minAcceleration = float(self.min_accel) + frogpilotPlan.slcOverridden = bool(self.override_slc) + frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed) frogpilotPlan.slcSpeedLimit = self.slc_target frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 7d987130e..f7035190e 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -54,9 +54,9 @@ 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 = speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; + speedLimit = slcOverridden ? s.scene.speed_limit_overridden_speed : speedLimitController ? s.scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); - if (speedLimitController) { + if (speedLimitController && !slcOverridden) { speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0); } @@ -162,6 +162,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr); const QRect sign_rect = set_speed_rect.adjusted(sign_margin, default_size.height(), -sign_margin, -sign_margin); + p.save(); // US/Canada (MUTCD style) sign if (has_us_speed_limit) { p.setPen(Qt::NoPen); @@ -170,7 +171,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.setPen(QPen(blackColor(), 6)); p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16); - if (speedLimitController && showSLCOffset) { + 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)); @@ -194,6 +196,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.setPen(QPen(Qt::red, 20)); p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16)); + p.setOpacity(slcOverridden ? 0.25 : 1.0); p.setPen(blackColor()); if (showSLCOffset) { p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); @@ -205,6 +208,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr); } } + p.restore(); // current speed p.setFont(InterFont(176, QFont::Bold)); @@ -571,6 +575,7 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce speedLimitController = scene.speed_limit_controller; showSLCOffset = speedLimitController && scene.show_slc_offset; + slcOverridden = speedLimitController && scene.speed_limit_overridden; slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); speedLimitChanged = speedLimitController && scene.speed_limit_changed; unconfirmedSpeedLimit = speedLimitController ? scene.unconfirmed_speed_limit : 0; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 3e8edcfaa..15b8dae99 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -63,6 +63,7 @@ private: bool showAlwaysOnLateralStatusBar; bool showConditionalExperimentalStatusBar; bool showSLCOffset; + bool slcOverridden; bool speedLimitChanged; bool speedLimitController; bool trafficModeActive; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index eb569345f..a1df454cd 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -240,6 +240,8 @@ static void update_state(UIState *s) { scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); 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(); } if (sm.updated("liveLocationKalman")) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 017b0e1da..96b35a222 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -143,6 +143,7 @@ typedef struct UIScene { bool show_slc_offset_ui; bool speed_limit_changed; bool speed_limit_controller; + bool speed_limit_overridden; bool tethering_enabled; bool traffic_mode; bool traffic_mode_active; @@ -153,6 +154,7 @@ typedef struct UIScene { float lead_detection_threshold; float speed_limit; float speed_limit_offset; + float speed_limit_overridden_speed; float unconfirmed_speed_limit; int alert_size;