Controls - Speed Limit Controller - Override Method

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -56,11 +56,13 @@ class FrogPilotPlanner:
     self.lead_one = Lead()
     self.mtsc = MapTurnSpeedController()
 
+    self.override_slc = False
     self.slower_lead = False
 
     self.acceleration_jerk = 0
     self.frame = 0
     self.mtsc_target = 0
+    self.overridden_speed = 0
     self.road_curvature = 0
     self.slc_target = 0
     self.speed_jerk = 0
@@ -214,10 +216,24 @@ 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 == 1:
+          if carState.gasPressed:
+            self.overridden_speed = v_ego + v_ego_diff
+          self.overridden_speed = np.clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff)
+        elif frogpilot_toggles.speed_limit_controller_override == 2:
+          self.overridden_speed = v_cruise + v_cruise_diff
+      else:
+        self.overridden_speed = 0
     else:
       self.slc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
 
-    targets = [self.mtsc_target, self.slc_target - v_ego_diff]
+    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff]
     filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets]
 
     return min(filtered_targets)
@@ -240,6 +256,8 @@ class FrogPilotPlanner:
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.minAcceleration = 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.cc

@@ -376,9 +376,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 ? scene.speed_limit_overridden_speed : speedLimitController ? 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);
   }
 
@@ -496,8 +496,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
 
     p.save();
-    p.setOpacity(1.0);
+    p.setOpacity(slcOverridden ? 0.25 : 1.0);
-    if (speedLimitController && showSLCOffset) {
+    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));
@@ -523,7 +523,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
 
     p.save();
-    p.setOpacity(1.0);
+    p.setOpacity(slcOverridden ? 0.25 : 1.0);
     p.setPen(blackColor());
     if (showSLCOffset) {
       p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
@@ -876,6 +876,7 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   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);
   useViennaSLCSign = scene.use_vienna_slc_sign;
 

selfdrive/ui/qt/onroad.h

@@ -154,6 +154,7 @@ private:
   bool showAlwaysOnLateralStatusBar;
   bool showConditionalExperimentalStatusBar;
   bool showSLCOffset;
+  bool slcOverridden;
   bool speedLimitController;
   bool trafficModeActive;
   bool useViennaSLCSign;

selfdrive/ui/ui.cc

@@ -241,6 +241,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

@@ -205,6 +205,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;
@@ -215,6 +216,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;