Controls - Speed Limit Controller - Override Method

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

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;

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;

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")) {

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;