Controls - Quality of Life - Force Standstill State - Only For Stop Lights/Stop Signs

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -59,6 +59,7 @@ class FrogPilotPlanner:
     self.mtsc_target = 0
     self.road_curvature = 0
     self.speed_jerk = 0
+    self.tracked_model_length = 0
     self.v_cruise = 0
 
     self.tracking_lead_mac = MovingAverageCalculator()
@@ -84,7 +85,7 @@ class FrogPilotPlanner:
     lead_distance = self.lead_one.dRel - distance_offset
     stopping_distance = STOP_DISTANCE + distance_offset
 
-    run_cem = frogpilot_toggles.conditional_experimental_mode
+    run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops
     if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear:
       self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles)
 
@@ -98,7 +99,9 @@ class FrogPilotPlanner:
 
     self.model_length = modelData.position.x[MODEL_LENGTH - 1]
     self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
+    self.model_stopped |= self.forcing_stop
     self.override_force_stop |= carState.gasPressed
+    self.override_force_stop |= frogpilot_toggles.force_stops and carState.standstill and self.tracking_lead
     self.override_force_stop |= frogpilotCarControl.resumePressed
     self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill and driving_gear else 1
 
@@ -215,8 +218,20 @@ class FrogPilotPlanner:
       self.forcing_stop = True
       self.v_cruise = -1
 
+    elif frogpilot_toggles.force_stops and self.cem.stop_light_detected and not self.override_force_stop and controlsState.enabled:
+      if self.tracked_model_length == 0:
+        self.tracked_model_length = self.model_length
+
+      self.forcing_stop = True
+      self.tracked_model_length -= v_ego * DT_MDL
+      self.v_cruise = min((self.tracked_model_length / PLANNER_TIME) - 1, v_cruise)
+
     else:
+      if not self.cem.stop_light_detected:
+        self.override_force_stop = False
+
       self.forcing_stop = False
+      self.tracked_model_length = 0
 
       targets = [self.mtsc_target]
       self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -59,7 +59,7 @@ class ConditionalExperimentalMode:
       return True
 
     if frogpilot_toggles.conditional_stop_lights and self.stop_light_detected:
-      self.status_value = 15
+      self.status_value = 15 if not self.frogpilot_planner.forcing_stop else 16
       return True
 
     return False

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -585,6 +585,7 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
     {13, tr("Experimental Mode activated for stopped lead")},
     {14, tr("Experimental Mode activated for slower lead")},
     {15, tr("Experimental Mode activated for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))},
+    {16, tr("Experimental Mode forced on for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))},
   };
 
   if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) {