From 0f5ab2f6f638308ec2a1cfc855e90d916788a9f9 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Sat, 2 May 2026 09:57:49 -0500
Subject: [PATCH] cem light latch

---
 .../test_conditional_experimental_mode.py     | 24 +++++++++++++++++--
 .../lib/conditional_experimental_mode.py      |  6 ++++-
 2 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py
index 005d3b022..690adfb8b 100644
--- a/selfdrive/controls/tests/test_conditional_experimental_mode.py
+++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py
@@ -28,14 +28,14 @@ class FakeParams:
 def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False,
              lead_status: bool = False, lead_d_rel: float = float("inf"),
              lead_v_lead: float = 0.0, lead_model_prob: float = 0.0, lead_radar: bool = False,
-             stop_sign_confirmed: bool = False):
+             stop_sign_confirmed: bool = False, forcing_stop: bool = False):
   planner = SimpleNamespace(
     params=FakeParams(),
     params_memory=FakeParams(),
     model_length=model_length,
     model_stopped=model_stopped,
     tracking_lead=tracking_lead,
-    starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=stop_sign_confirmed),
+    starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=stop_sign_confirmed, forcing_stop=forcing_stop),
     starpilot_following=SimpleNamespace(slower_lead=False, following_lead=False),
     lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel, vLead=lead_v_lead,
                              modelProb=lead_model_prob, radar=lead_radar),
@@ -239,6 +239,26 @@ def test_standstill_dashboard_stop_sign_keeps_exp_on(monkeypatch):
   assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
 
 
+def test_standstill_force_stop_keeps_exp_on_even_if_red_light_latch_clears(monkeypatch):
+  cem = make_cem(model_length=80.0, model_stopped=False, forcing_stop=True)
+  toggles = make_update_toggles()
+  sm = make_update_sm(standstill=True)
+
+  cem.stop_light_detected = True
+  cem.experimental_mode = True
+
+  def clear_red_light(*args, **kwargs):
+    cem.stop_light_detected = False
+    cem.stop_light_model_detected = False
+
+  monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light)
+
+  cem.update(0.0, sm, toggles)
+
+  assert cem.experimental_mode
+  assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
+
+
 def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead():
   v_ego = 35 * CV.MPH_TO_MS
   cem = make_cem(
diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py
index 383409752..fab55711f 100644
--- a/starpilot/controls/lib/conditional_experimental_mode.py
+++ b/starpilot/controls/lib/conditional_experimental_mode.py
@@ -123,7 +123,11 @@ class ConditionalExperimentalMode:
       # light / stop sign, but can immediately release to CHILL when the model
       # clears the stop (green light / open path).
       self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time)
-      standstill_stop_hold = self.stop_light_detected or getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)
+      standstill_stop_hold = (
+        self.stop_light_detected or
+        getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False) or
+        getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False)
+      )
 
       self.experimental_mode = standstill_stop_hold
       self.prev_experimental_mode = self.experimental_mode