From 72fb829c47bf7d3eeeb75c011f74195fb6a1a3f9 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 16 Jul 2024 23:02:42 -0700
Subject: [PATCH] Controls - Quality of Life - Force Standstill State - Only
 For Stop Lights/Stop Signs

---
 .../frogpilot/controls/frogpilot_planner.py     | 17 ++++++++++++++++-
 .../lib/conditional_experimental_mode.py        |  2 +-
 selfdrive/ui/qt/onroad/annotated_camera.cc      |  1 +
 3 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index 7d7c30727..7351a14cb 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/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]))
diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
index c03c51b60..49f75099f 100644
--- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+++ b/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
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
index 583f9afa8..ee8df985d 100644
--- a/selfdrive/ui/qt/onroad/annotated_camera.cc
+++ b/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) {