dec: don't let curve override a saturated decel

openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -25,6 +25,7 @@ _DECEL_INV_T = 1.0 / _T_IDXS[_DECEL_IDX]
 DECEL_INTENT_A_HINT = 0.35
 DECEL_INTENT_A_FULL = 1.30
 DECEL_INTENT_TRIGGER = 0.5
+DECEL_INTENT_CURVE_OVERRIDE = 0.9
 
 CURVE_Y_MAX = 5.0
 
@@ -54,7 +55,8 @@ class DecSignals:
 
 def should_blend(s: DecSignals) -> bool:
   degraded = s.model_trust < MODEL_TRUST_MIN
-  slowdown_detected = not degraded and s.decel_intent >= DECEL_INTENT_TRIGGER and not s.curve_detected
+  curve_gate = s.decel_intent >= DECEL_INTENT_CURVE_OVERRIDE or not s.curve_detected
+  slowdown_detected = not degraded and s.decel_intent >= DECEL_INTENT_TRIGGER and curve_gate
   return slowdown_detected or s.creeping
 
 

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -111,11 +111,18 @@ class TestDynamicExperimentalController(OpenpilotTestCase):
 
   def test_curve_exclusion_prevents_false_blend(self):
     controller = make_controller()
-    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), position_y=[6.0] * len(T_IDXS))
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0), position_y=[6.0] * len(T_IDXS))
     for _ in range(30):
       controller.update(sm)
     assert controller.mode() == "acc"
 
+  def test_curve_does_not_override_saturated_decel_intent(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), position_y=[6.0] * len(T_IDXS))
+    for _ in range(10):
+      controller.update(sm)
+    assert controller.mode() == "blended"
+
   def test_any_lead_forces_acc_even_with_strong_model_signal(self):
     for lead_radar in (True, False):
       controller = make_controller()
@@ -275,7 +282,10 @@ class TestShouldBlend(OpenpilotTestCase):
     assert not should_blend(DecSignals(decel_intent=0.0))
 
   def test_curve_exclusion_suppresses_slowdown(self):
-    assert not should_blend(DecSignals(decel_intent=1.0, curve_detected=True))
+    assert not should_blend(DecSignals(decel_intent=0.7, curve_detected=True))
+
+  def test_curve_exclusion_does_not_override_saturated_decel_intent(self):
+    assert should_blend(DecSignals(decel_intent=1.0, curve_detected=True))
 
   def test_degraded_model_suppresses_model_based_reasons(self):
     s = DecSignals(decel_intent=1.0, model_trust=0.0)

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_dec_maneuvers.py

@@ -162,12 +162,19 @@ class TestDecManeuvers(OpenpilotTestCase):
     assert all(r["dec_mode"] == "acc" for r in results)
 
   def test_s9_curve_exclusion_prevents_false_blend_on_a_bend(self):
-    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-2.5),
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-1.0),
                      position_y_fn=lambda _t: [6.0] * len(T_IDXS))
     results, _ = _run(plant, steps=30, v_cruise=20.0)
 
     assert all(r["dec_mode"] == "acc" for r in results)
 
+  def test_s11_curve_does_not_interrupt_an_active_hard_stop(self):
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-2.5),
+                     position_y_fn=lambda _t: [6.0] * len(T_IDXS))
+    results, _ = _run(plant, steps=10, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "blended" for r in results[ENTER_FRAMES - 1:])
+
   def test_s10_hard_brake_override_inert_while_lead_present(self):
     def hard_brake_meta(_current_time):
       return [0.0] * 5, True, 0.0