Keep radar lead response predictable under ACC

selfdrive/controls/lib/longitudinal_planner.py

@@ -179,7 +179,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       if output_a_target < output_a_target_mpc:
         self.mpc.source = LongitudinalPlanSource.e2e
     else:
-      output_a_target = output_a_target_mpc
+      output_a_target = min(output_a_target_mpc, stop_constraint.accel_ceiling_mps2)
       self.output_should_stop = output_should_stop_mpc
 
     self.output_should_stop |= stop_constraint.hold

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

@@ -19,10 +19,6 @@ class WMACConstants:
   RADAR_LEAD_ACC_EXIT_PROB = 0.4
   RADAR_LEAD_ACC_RISE_RATE = 1.0
   RADAR_LEAD_ACC_FALL_RATE = 0.25
-  RADAR_LEAD_ACC_MAX_DREL = 80.0
-  RADAR_LEAD_ACC_MAX_TTC = 6.0
-  RADAR_LEAD_ACC_MIN_CLOSING_SPEED = -0.5
-
   SLOW_DOWN_PROB = 0.5
   SLOW_DOWN_EXIT_PROB = 0.4
   SLOW_DOWN_RISE_RATE = 0.65

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

@@ -237,16 +237,8 @@ class DynamicExperimentalController:
     self._urgency = self._slow_down_tracker.value
 
   def _radar_acc_lead_score(self, lead_one) -> float:
-    if not lead_one.status:
-      return 0.0
-
-    d_rel = float(getattr(lead_one, 'dRel', float('inf')))
-    v_rel = float(getattr(lead_one, 'vRel', 0.0))
-    if d_rel <= WMACConstants.RADAR_LEAD_ACC_MAX_DREL:
-      return 1.0
-    if v_rel <= WMACConstants.RADAR_LEAD_ACC_MIN_CLOSING_SPEED and d_rel / max(-v_rel, 0.1) <= WMACConstants.RADAR_LEAD_ACC_MAX_TTC:
-      return 1.0
-    return 0.0
+    radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
+    return float(lead_one.status and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
 
   def _model_action_urgency(self, md) -> float:
     action = getattr(md, 'action', None)
@@ -276,15 +268,15 @@ class DynamicExperimentalController:
     return urgency
 
   def _desired_mode(self) -> tuple[ModeType, bool]:
+    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
+      return 'acc', True
+
     if self._stop_constraint.state == StopIntentState.suppressed:
       return 'acc', True
 
     if self._stop_constraint.active:
       return 'blended', True
 
-    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
-      return 'acc', False
-
     if self._has_mpc_fcw:
       return 'blended', True
 
@@ -328,7 +320,7 @@ class DynamicExperimentalController:
 
     mode, immediate = self._desired_mode()
     self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
-                                    cancel_hold=self._has_radar_acc_lead)
+                                    cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead)
     self._mode_manager.update()
 
     self._frame += 1

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

@@ -5,15 +5,17 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.stop_intent import StopInte
 
 
 class MockLeadOne:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1):
     self.status = status
     self.dRel = dRel
     self.vRel = vRel
+    self.radar = radar
+    self.radarTrackId = radarTrackId
 
 
 class MockRadarState:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
-    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel)
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1):
+    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId)
 
 
 class MockCarState:
@@ -70,7 +72,7 @@ def default_sm():
   sm = MockSubMaster({
     'carControl': MockCarControl(),
     'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(status=1.0),
+    'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7),
     'modelV2': MockModelData(valid=True),
     'selfdriveState': MockSelfDriveState(experimentalMode=True),
   })
@@ -173,7 +175,7 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo
 
 def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
 
   for _ in range(3):
@@ -184,36 +186,31 @@ def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm)
   assert controller.mode() == "acc"
 
 
-def test_far_radar_lead_allows_blended_until_acc_relevant(mock_cp, mock_mpc, default_sm):
+def test_far_radar_lead_always_uses_acc(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0, radar=True, radarTrackId=7)
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
 
   controller.update(default_sm)
 
   assert controller._has_lead_filtered
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-
-def test_relevant_radar_lead_smoothly_returns_to_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=45.0, vRel=0.0)
-  for _ in range(20):
-    controller.update(default_sm)
-
   assert controller._has_radar_acc_lead
   assert controller.mode() == "acc"
 
 
+def test_vision_only_far_lead_can_use_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
 def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0, radar=True, radarTrackId=7)
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
 
   for _ in range(20):
@@ -225,7 +222,7 @@ def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
 
 def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
   default_sm['carState'].standstill = True
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True)
   mock_mpc.crash_cnt = 1
@@ -240,7 +237,7 @@ def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default
 
 def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
   controller.update(default_sm)
 
   default_sm['radarState'] = MockRadarState(status=0.0)
@@ -251,7 +248,7 @@ def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, defau
   assert controller.mode() == "acc"
 
 
-def test_consistent_stop_intent_commits_and_survives_radar_acquisition(mock_cp, mock_mpc, default_sm):
+def test_radar_acquisition_preserves_committed_stop_and_forces_acc(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
   default_sm['radarState'] = MockRadarState(status=0.0)
 
@@ -263,10 +260,16 @@ def test_consistent_stop_intent_commits_and_survives_radar_acquisition(mock_cp,
   assert controller.stop_constraint().state == StopIntentState.approach
   assert controller.mode() == "blended"
 
-  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
   controller.update(default_sm)
-  assert controller.stop_constraint().active
-  assert controller.mode() == "blended"
+  assert controller.stop_constraint().state == StopIntentState.approach
+  assert controller.mode() == "acc"
+
+  default_sm['carState'].vEgo = 0.0
+  default_sm['carState'].standstill = True
+  controller.update(default_sm)
+  assert controller.stop_constraint().state == StopIntentState.hold
+  assert controller.mode() == "acc"
 
 
 def test_gas_suppresses_committed_stop_and_returns_to_acc(mock_cp, mock_mpc, default_sm):