oscillation

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -100,7 +100,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
     throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
-    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+    stock_allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+    self.allow_throttle = self.accel_controller.update_allow_throttle(
+      stock_allow_throttle, throttle_prob, force_allow=v_ego <= MIN_ALLOW_THROTTLE_SPEED,
+    )
 
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
 
@@ -141,7 +144,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
     is_e2e = self.is_e2e(sm)
-    output_a_target_e2e = self.select_model_accel(
+    output_a_target_model = self.select_model_accel(
       output_a_target_mpc, output_a_target_e2e, blended=is_e2e,
       should_stop=output_should_stop_e2e or output_should_stop_mpc, fcw=self.fcw, reset=reset_state,
     )
@@ -154,7 +157,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
     if is_e2e:
-      candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+      candidates.append((output_a_target_model, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+    elif self.model_accel_transition.active:
+      candidates[0] = (output_a_target_model, self.mpc.source, output_should_stop_mpc)
 
     candidates = self.update_accel_controller(sm, candidates)
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])

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

@@ -7,8 +7,8 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot import get_sanitize_int_param
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
   COMFORT_DECEL, EARLY_DECEL_EPSILON, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_RESPONSE_TIME,
-  EARLY_DECEL_SPEED_DEADBAND, EARLY_DECEL_TIGHTEN_RATE, PARAM_READ_INTERVAL, VEGO_NOISE_TOLERANCE,
-  AccelProfile, profile_accel_max, sanitize_profile,
+  EARLY_DECEL_SPEED_DEADBAND, EARLY_DECEL_TIGHTEN_RATE, PARAM_READ_INTERVAL, THROTTLE_REENABLE_PROB,
+  VEGO_NOISE_TOLERANCE, AccelProfile, profile_accel_max, sanitize_profile,
 )
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan
 
@@ -45,6 +45,7 @@ class AccelController:
     self.selected_lead = -1
     self.selected_lead_track_id = -1
     self.required_decel = 0.0
+    self._allow_throttle = True
 
   @property
   def is_enabled(self) -> bool:
@@ -58,6 +59,7 @@ class AccelController:
 
   def reset(self) -> None:
     self._early_decel = None
+    self._allow_throttle = True
     self.is_active = False
     self.cruise_accel_max = None
     self.early_decel = None
@@ -66,6 +68,15 @@ class AccelController:
     self.selected_lead_track_id = -1
     self.required_decel = 0.0
 
+  def update_allow_throttle(self, stock_allowed: bool, throttle_prob: float, *, force_allow: bool = False) -> bool:
+    if not self.is_enabled or force_allow:
+      self._allow_throttle = bool(stock_allowed)
+    elif self._allow_throttle:
+      self._allow_throttle = bool(stock_allowed)
+    elif stock_allowed and math.isfinite(throttle_prob) and throttle_prob > THROTTLE_REENABLE_PROB:
+      self._allow_throttle = True
+    return self._allow_throttle
+
   def _valid_context(self, *, v_ego: float, a_ego: float, v_cruise: float, stock_accel_max: float,
                      engaged: bool, cruise_initialized: bool) -> bool:
     values = (v_ego, a_ego, v_cruise, stock_accel_max, self.delay)
@@ -106,7 +117,8 @@ class AccelController:
 
   def update(self, radar_state, *, v_ego: float, a_ego: float, v_cruise: float, follow_personality,
              engaged: bool, cruise_initialized: bool, acc_selected: bool, stock_accel_max: float,
-             radar_fresh: bool = True, force_decel: bool = False, previous_plan_accel: float = 0.0) -> AccelDecision:
+             radar_fresh: bool = True, radar_healthy: bool = True, force_decel: bool = False,
+             previous_plan_accel: float = 0.0) -> AccelDecision:
     self.profile = sanitize_profile(self.profile)
     valid_context = self._valid_context(
       v_ego=v_ego, a_ego=a_ego, v_cruise=v_cruise, stock_accel_max=stock_accel_max,
@@ -120,19 +132,23 @@ class AccelController:
     positive_stock_max = max(float(stock_accel_max), 0.0)
     self.cruise_accel_max = profile_accel_max(self.profile, sanitized_v_ego, positive_stock_max)
 
-    lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
-    if radar_fresh and radar_state is not None:
-      try:
-        lead_plan = calculate_lead_plan(
-          radar_state, sanitized_v_ego, float(a_ego), self.delay, self.profile, follow_personality,
-        )
-      except (AttributeError, TypeError, ValueError):
-        lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
+    if radar_healthy and not radar_fresh:
+      if self._early_decel is not None:
+        self.state = AccelControllerState.hold
+    else:
+      lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
+      if radar_healthy and radar_state is not None:
+        try:
+          lead_plan = calculate_lead_plan(
+            radar_state, sanitized_v_ego, float(a_ego), self.delay, self.profile, follow_personality,
+          )
+        except (AttributeError, TypeError, ValueError):
+          lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
 
-    self.selected_lead = lead_plan.selected_lead
-    self.selected_lead_track_id = lead_plan.selected_lead_track_id
-    self.required_decel = lead_plan.required_decel
-    self._update_early_decel(self._raw_early_decel(lead_plan), previous_plan_accel)
+      self.selected_lead = lead_plan.selected_lead
+      self.selected_lead_track_id = lead_plan.selected_lead_track_id
+      self.required_decel = lead_plan.required_decel
+      self._update_early_decel(self._raw_early_decel(lead_plan), previous_plan_accel)
 
     profile_binding = self.cruise_accel_max < positive_stock_max - EARLY_DECEL_EPSILON
     self.is_active = profile_binding or self.early_decel is not None

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py

@@ -11,8 +11,8 @@ ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())
 # Scale the stock cruise candidate; stock turn and throttle limits stay authoritative.
 ACCEL_SCALE_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
 ACCEL_SCALE_V = {
-  AccelProfile.eco: [0.78, 0.72, 0.60, 0.50, 0.40],
-  AccelProfile.normal: [0.90, 0.86, 0.80, 0.72, 0.60],
+  AccelProfile.eco: [0.82, 0.76, 0.63, 0.53, 0.42],
+  AccelProfile.normal: [0.95, 0.90, 0.84, 0.76, 0.63],
   AccelProfile.sport: [1.00, 1.00, 1.00, 1.00, 1.00],
 }
 
@@ -34,6 +34,7 @@ MAX_LEAD_ACCEL_TAU = 10.0
 MIN_LEAD_SPEED = -1.0
 VEGO_NOISE_TOLERANCE = 0.10
 PARAM_READ_INTERVAL = 0.25
+THROTTLE_REENABLE_PROB = 0.45
 
 
 def sanitize_profile(profile: int) -> int:

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py

@@ -9,7 +9,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controll
   AccelController, AccelControllerState, AccelDecision,
 )
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  ACCEL_PROFILES, ACCEL_SCALE_BP, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_TIGHTEN_RATE,
+  ACCEL_PROFILES, ACCEL_SCALE_BP, ACCEL_SCALE_V, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_TIGHTEN_RATE,
   AccelProfile, profile_accel_max, profile_accel_scale, sanitize_profile,
 )
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import calculate_lead_plan
@@ -44,6 +44,7 @@ def update(instance, radar_state=None, **overrides):
     "acc_selected": True,
     "stock_accel_max": 1.0,
     "radar_fresh": True,
+    "radar_healthy": True,
     "force_decel": False,
   }
   arguments.update(overrides)
@@ -55,6 +56,11 @@ def restrictive_radar():
 
 
 class TestProfiles(OpenpilotTestCase):
+  def test_profiles_are_faster_without_exceeding_stock(self):
+    self.assertEqual(ACCEL_SCALE_V[AccelProfile.eco], [0.82, 0.76, 0.63, 0.53, 0.42])
+    self.assertEqual(ACCEL_SCALE_V[AccelProfile.normal], [0.95, 0.90, 0.84, 0.76, 0.63])
+    self.assertEqual(ACCEL_SCALE_V[AccelProfile.sport], [1.0] * len(ACCEL_SCALE_BP))
+
   def test_profile_order_and_stock_scaling(self):
     for speed in (*ACCEL_SCALE_BP, 17.0, 50.0):
       with self.subTest(speed=speed):
@@ -108,6 +114,33 @@ class TestAccelDecision(OpenpilotTestCase):
         self.assertFalse(instance.is_active)
         self.assertEqual(instance.state, AccelControllerState.inactive)
 
+  def test_allow_throttle_hysteresis_filters_route_chatter(self):
+    instance = controller()
+    outputs = []
+    for probability in (0.32, 0.41, 0.36, 0.42, 0.31, 0.44, 0.35):
+      stock_allowed = probability > 0.4
+      outputs.append(instance.update_allow_throttle(stock_allowed, probability))
+
+    self.assertEqual(outputs, [False] * len(outputs))
+    self.assertTrue(instance.update_allow_throttle(True, 0.46))
+
+  def test_allow_throttle_uses_stock_when_disabled_and_low_speed_override(self):
+    instance = controller(enabled=False)
+    self.assertFalse(instance.update_allow_throttle(False, 0.39))
+    self.assertTrue(instance.update_allow_throttle(True, 0.0, force_allow=True))
+
+    instance.enabled = True
+    self.assertFalse(instance.update_allow_throttle(False, 0.0))
+    self.assertTrue(instance.update_allow_throttle(True, 0.0, force_allow=True))
+
+  def test_allow_throttle_hysteresis_resets_with_controller(self):
+    instance = controller()
+    self.assertFalse(instance.update_allow_throttle(False, 0.39))
+
+    instance.reset()
+
+    self.assertTrue(instance.update_allow_throttle(True, 0.41))
+
   def test_profile_limit_is_only_active_when_binding(self):
     for profile in ACCEL_PROFILES:
       with self.subTest(profile=profile):
@@ -138,10 +171,10 @@ class TestEarlyDecel(OpenpilotTestCase):
     for before, after in zip(samples[:-1], samples[1:], strict=True):
       self.assertGreaterEqual(after - before, -EARLY_DECEL_TIGHTEN_RATE * DT_MDL - 1e-9)
 
-  def test_dropout_and_stale_radar_release_at_bound(self):
-    releases = ((radar(), True), (restrictive_radar(), False))
-    for radar_state, radar_fresh in releases:
-      with self.subTest(radar_fresh=radar_fresh):
+  def test_fresh_dropout_and_unhealthy_radar_release_at_bound(self):
+    releases = ((radar(), True, True), (restrictive_radar(), False, False))
+    for radar_state, radar_fresh, radar_healthy in releases:
+      with self.subTest(radar_fresh=radar_fresh, radar_healthy=radar_healthy):
         instance = controller()
         for _ in range(10):
           update(instance, restrictive_radar())
@@ -149,7 +182,7 @@ class TestEarlyDecel(OpenpilotTestCase):
         self.assertIsNotNone(previous)
 
         for _ in range(30):
-          decision = update(instance, radar_state, radar_fresh=radar_fresh)
+          decision = update(instance, radar_state, radar_fresh=radar_fresh, radar_healthy=radar_healthy)
           current = decision.early_decel or 0.0
           self.assertLessEqual(current, 0.0)
           self.assertGreaterEqual(current, previous - 1e-9)
@@ -159,6 +192,17 @@ class TestEarlyDecel(OpenpilotTestCase):
             break
         self.assertIsNone(instance.early_decel)
 
+  def test_healthy_duplicate_radar_holds_early_decel(self):
+    instance = controller()
+    for _ in range(10):
+      update(instance, restrictive_radar())
+    previous = instance.early_decel
+
+    decision = update(instance, restrictive_radar(), radar_fresh=False, radar_healthy=True)
+
+    self.assertEqual(decision.early_decel, previous)
+    self.assertEqual(instance.state, AccelControllerState.hold)
+
   def test_early_decel_is_bounded_by_profile_comfort(self):
     for profile in ACCEL_PROFILES:
       with self.subTest(profile=profile):

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py

@@ -58,7 +58,9 @@ def planner_for_hook(*, enabled=True, profile=AccelProfile.normal):
   dynamic_planner.dec = SimpleNamespace(active=lambda: False)
   dynamic_planner.output_v_target = 30.0
   dynamic_planner.previous_plan_accel = 0.0
+  dynamic_planner.a_cruise = 0.0
   dynamic_planner._radar_fresh_this_cycle = True
+  dynamic_planner._radar_healthy_this_cycle = True
   return planner
 
 
@@ -87,6 +89,30 @@ class TestPlannerHook(OpenpilotTestCase):
     candidates = ((-0.4, MpcSource.lead0, True), (0.2, MpcSource.e2e, False))
     self.assertIs(planner.update_accel_controller(PlannerSM(), candidates), candidates)
 
+  def test_inactive_acc_does_not_publish_a_stale_positive_cruise_candidate(self):
+    planner = planner_for_hook(profile=AccelProfile.eco)
+    sm = PlannerSM()
+    sm["controlsState"].longControlState = LongCtrlState.off
+    candidates = [(-0.4, MpcSource.lead0, True), (0.6, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(result, [candidates[0], (0.0, MpcSource.cruise, False)])
+    self.assertEqual(planner.a_cruise, 0.0)
+    self.assertEqual(planner.accel_controller.state, AccelControllerState.inactive)
+
+    stale_positive = [(1.3, MpcSource.lead0, False), (0.9, MpcSource.cruise, False)]
+    self.assertEqual(min(candidate[0] for candidate in planner.update_accel_controller(sm, stale_positive)), 0.0)
+
+    braking = [(-0.4, MpcSource.lead0, True), (-0.2, MpcSource.cruise, True)]
+    self.assertIs(planner.update_accel_controller(sm, braking), braking)
+
+    disabled = planner_for_hook(enabled=False)
+    self.assertIs(disabled.update_accel_controller(sm, candidates), candidates)
+
+    blended = planner_for_hook(profile=AccelProfile.eco)
+    self.assertIs(blended.update_accel_controller(PlannerSM(experimental=True), candidates), candidates)
+
   def test_profiles_scale_final_stock_turn_and_coast_candidates(self):
     cp = SimpleNamespace(steerRatio=15.0, wheelbase=2.7)
     scenarios = (
@@ -143,6 +169,7 @@ class TestPlannerHook(OpenpilotTestCase):
     sm.valid["radarState"] = False
     sm.logMonoTime["radarState"] = 102
     self.assertFalse(planner._update_radar_freshness(sm))
+    self.assertFalse(planner._radar_healthy_this_cycle)
 
 
 class TestParamsSchemaAndTelemetry(OpenpilotTestCase):

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

@@ -133,17 +133,21 @@ class ModeTransitionManager:
 
 
 class ModelAccelTransition:
-  """Smooths model acceleration while DEC enters blended mode."""
+  """Smooths acceleration while DEC changes modes."""
 
   def __init__(self, dt: float = DT_MDL):
     self._max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * dt
     self._accel = 0.0
     self._active = False
-    self._was_blended = False
+    self._blended = False
 
   def reset(self) -> None:
     self._active = False
-    self._was_blended = False
+    self._blended = False
+
+  @property
+  def active(self) -> bool:
+    return self._active
 
   def update(self, mpc_accel: float, model_accel: float, previous_accel: float, *, blended: bool,
              urgent: bool = False, reset: bool = False) -> float:
@@ -151,26 +155,24 @@ class ModelAccelTransition:
     if reset or not all(math.isfinite(accel) for accel in (mpc_accel, model_accel, previous_accel)):
       self.reset()
       return selected_accel
-    if not blended:
-      self.reset()
-      return selected_accel
 
-    if not self._was_blended:
+    if blended != self._blended:
       self._accel = previous_accel
       self._active = True
-    self._was_blended = True
+      self._blended = blended
 
-    if urgent:
+    if urgent and selected_accel <= self._accel:
       self._accel = selected_accel
       self._active = True
       return selected_accel
     if not self._active:
       return selected_accel
 
-    preview_accel = max(self._accel - self._max_step, min(self._accel + self._max_step, model_accel))
+    transition_target = model_accel if blended else mpc_accel
+    preview_accel = max(self._accel - self._max_step, min(self._accel + self._max_step, transition_target))
     output_accel = min(mpc_accel, preview_accel)
     self._accel = output_accel
-    if mpc_accel >= model_accel and math.isclose(preview_accel, model_accel, abs_tol=1e-9):
+    if mpc_accel >= transition_target and math.isclose(preview_accel, transition_target, abs_tol=1e-9):
       self._active = False
     return output_accel
 
@@ -242,9 +244,6 @@ class DynamicExperimentalController:
   def active(self) -> bool:
     return self._active
 
-  def accel_transition_urgent(self) -> bool:
-    return self._has_mpc_fcw or (self._has_slow_down and self._urgency > 0.7)
-
   def set_mpc_fcw_crash_cnt(self) -> None:
     """Set MPC FCW crash count"""
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt

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

@@ -33,10 +33,6 @@ class MockDec:
   def enabled(self) -> bool:
     return True
 
-  def accel_transition_urgent(self) -> bool:
-    return False
-
-
 class MockSubMaster(dict):
   def __init__(self, services: dict):
     super().__init__(services)

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

@@ -121,7 +121,7 @@ class TestModelAccelTransition(OpenpilotTestCase):
     self.assertAlmostEqual(transition.update(-2.0, -0.5, 0.30, blended=True), -2.0)
     self.assertAlmostEqual(transition.update(0.0, -0.5, -2.0, blended=True), -1.85)
 
-  def test_only_entry_is_shaped(self):
+  def test_entry_and_exit_are_rate_bounded(self):
     transition = ModelAccelTransition()
     output = 0.30
     for _ in range(20):
@@ -131,5 +131,16 @@ class TestModelAccelTransition(OpenpilotTestCase):
     else:
       self.fail("transition did not converge")
 
-    self.assertAlmostEqual(transition.update(0.0, -1.50, output, blended=True), -1.50)
-    self.assertAlmostEqual(transition.update(0.0, -0.20, output, blended=False), 0.0)
+    output = transition.update(0.0, -1.50, output, blended=True)
+    self.assertAlmostEqual(output, -1.50)
+    self.assertAlmostEqual(transition.update(0.0, -0.20, output, blended=False), -1.35)
+
+  def test_harder_mpc_braking_bypasses_exit_ramp(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(2.0, 0.76, 0.76, blended=True), 0.76)
+    self.assertAlmostEqual(transition.update(-2.0, 0.76, 0.76, blended=False), -2.0)
+
+  def test_urgent_release_remains_rate_bounded(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(-1.0, -1.0, -1.0, blended=True), -1.0)
+    self.assertAlmostEqual(transition.update(1.0, -1.0, -1.0, blended=False, urgent=True), -0.85)

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

@@ -38,6 +38,7 @@ class LongitudinalPlannerSP:
     self.e2e_alerts_helper = E2EAlertsHelper()
     self._radar_log_mono_time = None
     self._radar_fresh_this_cycle = True
+    self._radar_healthy_this_cycle = True
 
     self.output_v_target = 0.
     self.output_a_target = 0.
@@ -52,9 +53,8 @@ class LongitudinalPlannerSP:
 
   def select_model_accel(self, mpc_accel: float, model_accel: float, *, blended: bool,
                          should_stop: bool, fcw: bool, reset: bool) -> float:
-    urgent = should_stop or fcw or self.dec.accel_transition_urgent()
     return self.model_accel_transition.update(
-      mpc_accel, model_accel, self.previous_plan_accel, blended=blended, urgent=urgent, reset=reset or not self.dec.active(),
+      mpc_accel, model_accel, self.previous_plan_accel, blended=blended, urgent=should_stop or fcw, reset=reset or not self.dec.active(),
     )
 
   def update_accel_controller(self, sm: messaging.SubMaster, candidates):
@@ -67,13 +67,23 @@ class LongitudinalPlannerSP:
     reset_state = ((long_control_off if self.accel_controller.available else not sm['selfdriveState'].enabled)
                    or CS.vCruise == V_CRUISE_UNSET)
     cruise_accel = candidates[cruise_index][0]
+    acc_selected = not self.is_e2e(sm)
     decision = self.accel_controller.update(
       sm['radarState'], v_ego=CS.vEgo, a_ego=CS.aEgo, v_cruise=self.output_v_target,
       follow_personality=sm['selfdriveState'].personality, engaged=not reset_state,
-      cruise_initialized=CS.vCruise != V_CRUISE_UNSET, acc_selected=not self.is_e2e(sm),
+      cruise_initialized=CS.vCruise != V_CRUISE_UNSET, acc_selected=acc_selected,
       stock_accel_max=max(cruise_accel, 0.0), radar_fresh=self._radar_fresh_this_cycle,
+      radar_healthy=self._radar_healthy_this_cycle,
       force_decel=sm['controlsState'].forceDecel, previous_plan_accel=self.previous_plan_accel,
     )
+    if self.accel_controller.is_enabled and acc_selected and reset_state:
+      # Do not carry an unactuated cruise ramp into engagement.
+      self.a_cruise = 0.0
+      if cruise_accel > 0.0:
+        candidates = list(candidates)
+        _, source, stop = candidates[cruise_index]
+        candidates[cruise_index] = (0.0, source, stop)
+        return candidates
     if decision.cruise_accel_max is None and decision.early_decel is None:
       return candidates
 
@@ -88,6 +98,7 @@ class LongitudinalPlannerSP:
   def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
     radar_log_mono_time = sm.logMonoTime['radarState']
     radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
+    self._radar_healthy_this_cycle = radar_healthy
     radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
     if radar_advanced:
       self._radar_log_mono_time = radar_log_mono_time

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

@@ -26,7 +26,12 @@ from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_con
   _ENTERING_PRED_LAT_ACC_TH,
   _MIN_ACTIVATION_SPEED,
   _RELIEF_CONFIRMATION_FRAMES,
+  _TARGET_RELEASE_CONFIRMATION_FRAMES,
   _TARGET_RELEASE_RATE,
+  _TARGET_TIGHTEN_CONFIRMATION_FRAMES,
+  _TARGET_TIGHTEN_RATE,
+  _TURNING_LAT_ACC_TH,
+  _URGENT_PRED_LAT_ACC_TH,
   SmartCruiseControlVision,
 )
 from openpilot.common.test import OpenpilotTestCase
@@ -176,13 +181,14 @@ class TestSmartCruiseControlVision(OpenpilotTestCase):
       self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0)
     assert self.scc_v.state == VisionState.enabled
 
-  def test_unconfirmed_leaving_and_reentry_only_shape_speed(self):
+  def test_unconfirmed_release_holds_but_urgent_reentry_tightens(self):
     self.enter_curve()
     targets = [self.scc_v.output_v_target]
 
     self.update_lat_accels(2.0, 2.2, a_ego=-0.8)
     assert self.scc_v.state == VisionState.turning
     assert self.scc_v.output_a_target == -0.8
+    turning_demand = self.scc_v._v_demand()
     targets.append(self.scc_v.output_v_target)
 
     self.update_lat_accels(1.2, 1.2, a_ego=0.3)
@@ -193,12 +199,15 @@ class TestSmartCruiseControlVision(OpenpilotTestCase):
     self.update_lat_accels(1.0, 3.0, a_ego=-1.2)
     assert self.scc_v.state == VisionState.entering
     assert self.scc_v.output_a_target == -1.2
+    reentry_demand = self.scc_v._v_demand()
     targets.append(self.scc_v.output_v_target)
 
     entering, turning, leaving, reentering = targets
-    self.assert_approx(turning, entering)
-    assert 0.0 < leaving - turning <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+    assert turning < entering
+    self.assert_approx(turning, turning_demand)
+    self.assert_approx(leaving, turning)
     assert reentering < leaving
+    self.assert_approx(reentering, reentry_demand)
 
   def test_new_curve_interrupts_confirmed_release_immediately(self):
     self.enter_curve()
@@ -271,18 +280,18 @@ class TestSmartCruiseControlVision(OpenpilotTestCase):
     self.assert_approx(active_v_targets[-1], release_cruise)
     assert np.all((np.diff(active_v_targets) >= 0.0) & (np.diff(active_v_targets) <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9))
 
-  def test_target_release_slows_after_reaching_ego_speed(self):
+  def test_target_release_waits_for_relief_above_ego_speed(self):
     self.enter_curve()
+    held_v_target = self.scc_v.output_v_target
+    self.assert_approx(held_v_target, self.scc_v.v_ego)
 
-    for _ in range(100):
-      previous_v_target = self.scc_v.output_v_target
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES + _TARGET_RELEASE_CONFIRMATION_FRAMES - 2):
       self.update_lat_accels(0.8, 0.8)
-      if previous_v_target >= self.scc_v.v_ego:
-        rise = self.scc_v.output_v_target - previous_v_target
-        assert 0.0 < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9
-        break
-    else:
-      self.fail("curve target did not release to ego speed")
+      self.assert_approx(self.scc_v.output_v_target, held_v_target)
+
+    self.update_lat_accels(0.8, 0.8)
+    rise = self.scc_v.output_v_target - held_v_target
+    assert 0.0 < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9
 
   def test_curve_target_is_independent_of_ego_speed(self):
     model_speed = 24.0
@@ -354,20 +363,79 @@ class TestSmartCruiseControlVision(OpenpilotTestCase):
     assert self.scc_v.state == VisionState.entering
     assert self.scc_v.is_active
 
-  def test_sequential_curve_tightens_immediately_and_releases_bounded(self):
+  def test_nonurgent_activation_has_no_target_cliff(self):
+    v_ego = _MIN_ACTIVATION_SPEED + 0.01
+    model_speed = 8.0
+    self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed)
+    self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed)
+
+    self.assert_approx(self.scc_v.v_target, 8.0)
+    self.assert_approx(self.scc_v.output_v_target, v_ego)
+
+  def test_nonurgent_tightening_is_confirmed_and_rate_limited(self):
+    self.enter_curve()
+    initial_v_target = self.scc_v.output_v_target
+
+    for _ in range(_TARGET_TIGHTEN_CONFIRMATION_FRAMES - 1):
+      self.update_lat_accels(0.5, 2.8)
+      self.assert_approx(self.scc_v.output_v_target, initial_v_target)
+
+    self.update_lat_accels(0.5, 2.8)
+    drop = initial_v_target - self.scc_v.output_v_target
+    assert 0.0 < drop <= _TARGET_TIGHTEN_RATE * DT_MDL + 1e-9
+
+  def test_one_frame_curve_prediction_does_not_pulse_target(self):
+    self.enter_curve()
+    for _ in range(10):
+      self.update_lat_accels(0.5, 2.2)
+    stable_v_target = self.scc_v.output_v_target
+
+    self.update_lat_accels(0.5, 2.8)
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+    self.update_lat_accels(0.5, 2.2)
+
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+
+  def test_one_frame_release_does_not_reverse_target(self):
+    self.enter_curve(_URGENT_PRED_LAT_ACC_TH)
+    stable_v_target = self.scc_v.output_v_target
+
+    self.update_lat_accels(0.5, 2.2)
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+    self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH)
+
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+
+  def test_urgent_predicted_curve_is_not_delayed(self):
+    self.enter_curve()
+    self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH)
+
+    self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand())
+
+  def test_current_curve_is_not_delayed(self):
+    self.enter_curve()
+    self.update_lat_accels(_TURNING_LAT_ACC_TH, 2.8)
+
+    self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand())
+
+  def test_sequential_curve_confirms_release_and_tightens_urgently(self):
     self.enter_curve(3.0)
     for _ in range(20):
       self.update_lat_accels(0.5, 3.0)
     restrictive_v_target = self.scc_v.output_v_target
 
     self.update_lat_accels(0.5, 1.4, a_ego=0.4)
-    first_relief_v_target = self.scc_v.output_v_target
     assert self.scc_v.state == VisionState.entering
-    assert 0.0 < first_relief_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+    self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
     assert self.scc_v.output_a_target == 0.4
 
+    for _ in range(_TARGET_RELEASE_CONFIRMATION_FRAMES - 2):
+      self.update_lat_accels(0.5, 1.4)
+      self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
+
     self.update_lat_accels(0.5, 1.4)
-    assert 0.0 <= self.scc_v.output_v_target - first_relief_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+    released_v_target = self.scc_v.output_v_target
+    assert 0.0 < released_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
 
     self.update_lat_accels(0.5, 3.0, a_ego=-0.6)
     assert self.scc_v.state == VisionState.entering
@@ -376,7 +444,7 @@ class TestSmartCruiseControlVision(OpenpilotTestCase):
 
     for _ in range(4):
       self.update_lat_accels(0.5, 1.4)
-      assert 0.0 < self.scc_v.output_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
       self.update_lat_accels(0.5, 3.0)
       self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
 

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -23,6 +23,7 @@ _ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger enteri
 _ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
 
 _TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning state.
+_URGENT_PRED_LAT_ACC_TH = 3.  # Predicted Lat Acc threshold that requires an immediate speed reduction.
 
 _LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
 _FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cycle.
@@ -30,6 +31,9 @@ _FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cyc
 _A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
 
 _RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL)))
+_TARGET_TIGHTEN_CONFIRMATION_FRAMES = max(1, int(round(0.1 / DT_MDL)))
+_TARGET_RELEASE_CONFIRMATION_FRAMES = max(1, int(round(0.15 / DT_MDL)))
+_TARGET_TIGHTEN_RATE = 5.  # m/s^2
 _TARGET_RELEASE_RATE = 1.  # m/s^2
 _BELOW_EGO_TARGET_RELEASE_RATE = 3.  # m/s^2
 _MIN_PRED_SPEED = 1.  # m/s
@@ -58,15 +62,50 @@ class SmartCruiseControlVision:
     self.current_lat_acc = 0.
     self.max_pred_lat_acc = 0.
     self.relief_frames = 0
+    self.tighten_frames = 0
+    self.release_frames = 0
 
   def _v_demand(self) -> float:
     return max(MIN_V, min(self.v_target, self.v_cruise_setpoint))
 
-  def _released_v_target(self) -> float:
+  def _curve_is_urgent(self) -> bool:
+    return self.current_lat_acc >= _TURNING_LAT_ACC_TH or self.max_pred_lat_acc >= _URGENT_PRED_LAT_ACC_TH
+
+  def _filtered_v_target(self) -> float:
     demand = self._v_demand()
+
+    if self.output_v_target == V_CRUISE_UNSET:
+      self.tighten_frames = 0
+      self.release_frames = 0
+      if self._curve_is_urgent():
+        return demand
+      return max(demand, min(self.v_ego, self.v_cruise_setpoint))
+
     if demand < self.output_v_target:
-      return demand
-    release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if self.output_v_target < min(self.v_ego, demand) else _TARGET_RELEASE_RATE
+      self.release_frames = 0
+      if self._curve_is_urgent():
+        self.tighten_frames = 0
+        return demand
+
+      self.tighten_frames += 1
+      if self.tighten_frames < _TARGET_TIGHTEN_CONFIRMATION_FRAMES:
+        return self.output_v_target
+      return max(demand, self.output_v_target - _TARGET_TIGHTEN_RATE * DT_MDL)
+
+    self.tighten_frames = 0
+    releasing_brake = self.output_v_target < min(self.v_ego, demand)
+    if not releasing_brake and self.relief_frames < _RELIEF_CONFIRMATION_FRAMES:
+      self.release_frames = 0
+      return self.output_v_target
+
+    if demand > self.output_v_target:
+      self.release_frames += 1
+      if self.release_frames < _TARGET_RELEASE_CONFIRMATION_FRAMES:
+        return self.output_v_target
+    else:
+      self.release_frames = 0
+
+    release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if releasing_brake else _TARGET_RELEASE_RATE
     return min(demand, self.output_v_target + release_rate * DT_MDL)
 
   def get_a_target_from_control(self) -> float:
@@ -74,10 +113,10 @@ class SmartCruiseControlVision:
 
   def get_v_target_from_control(self) -> float:
     if self.is_active:
-      if self.output_v_target == V_CRUISE_UNSET:
-        return self._v_demand()
-      return self._released_v_target()
+      return self._filtered_v_target()
 
+    self.tighten_frames = 0
+    self.release_frames = 0
     return V_CRUISE_UNSET
 
   def _update_params(self) -> None:

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py

@@ -1,5 +1,6 @@
 import inspect
 from typing import Any
+from unittest import mock
 
 import numpy as np
 
@@ -106,9 +107,6 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
       def mode(self):
         return self.mode_name
 
-      def accel_transition_urgent(self):
-        return False
-
     plant = Plant(
       enabled=True, e2e=True, speed=22.0,
       model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False),
@@ -116,7 +114,8 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
     )
     configure(plant)
     dec = DecStub()
-    plant.planner.dec = dec
+    planner: Any = plant.planner
+    planner.dec = dec
 
     outputs = []
     for frame in range(20):
@@ -127,3 +126,91 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
 
     self.assertGreaterEqual(outputs[10] - outputs[9], -0.15 - 1e-9)
     self.assertFalse(result["fcw"])
+
+  def test_model_endpoint_urgency_does_not_bypass_entry_ramp(self):
+    plant = Plant(enabled=True, e2e=True, speed=21.0)
+    planner: Any = plant.planner
+    planner.dec._active = True
+    planner.dec._has_slow_down = True
+    planner.dec._urgency = 1.0
+    planner.previous_plan_accel = -0.136
+
+    first = planner.select_model_accel(0.0, -1.140, blended=True, should_stop=False, fcw=False, reset=False)
+    second = planner.select_model_accel(0.0, -1.140, blended=True, should_stop=False, fcw=False, reset=False)
+
+    self.assertAlmostEqual(first, -0.286)
+    self.assertAlmostEqual(second, -0.436)
+
+  def test_dec_exit_limits_positive_acceleration_step(self):
+    plant = Plant(enabled=True, e2e=True, speed=6.0)
+    planner: Any = plant.planner
+    planner.dec._active = True
+    planner.previous_plan_accel = 0.76
+
+    self.assertAlmostEqual(planner.select_model_accel(1.94, 0.76, blended=True, should_stop=False, fcw=False, reset=False), 0.76)
+    first = planner.select_model_accel(1.94, 0.76, blended=False, should_stop=False, fcw=False, reset=False)
+    second = planner.select_model_accel(1.94, 0.76, blended=False, should_stop=False, fcw=False, reset=False)
+
+    self.assertAlmostEqual(first, 0.91)
+    self.assertAlmostEqual(second, 1.06)
+
+  def test_dec_exit_transition_reaches_the_final_candidate_list(self):
+    class DecStub:
+      mode_name = "blended"
+
+      def update(self, _sm):
+        pass
+
+      def active(self):
+        return True
+
+      def mode(self):
+        return self.mode_name
+
+    plant = Plant(
+      enabled=True, e2e=True, speed=22.0,
+      model_action_fn=lambda _current_time, _v_ego, _a_ego: (0.20, False),
+    )
+    configure(plant, enabled=False)
+    dec = DecStub()
+    planner: Any = plant.planner
+    planner.dec = dec
+
+    def update_mpc(_radar_state, personality):
+      plant.planner.mpc.source = LongitudinalPlanSource.cruise
+      plant.planner.mpc.crash_cnt = 0
+
+    # Exercise final candidate arbitration without depending on the local ACADOS build.
+    with (
+      mock.patch.object(plant.planner.mpc, "update", side_effect=update_mpc),
+      mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan", return_value=1.94),
+      mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_cruise_accel", return_value=1.94),
+    ):
+      outputs = [plant.step(v_lead=0.0, v_cruise=30.0)["a_target"] for _ in range(4)]
+      self.assertAlmostEqual(outputs[-1], 0.20)
+
+      dec.mode_name = "acc"
+      first = plant.step(v_lead=0.0, v_cruise=30.0)["a_target"]
+      second = plant.step(v_lead=0.0, v_cruise=30.0)["a_target"]
+
+    self.assertLessEqual(first - outputs[-1], 0.15 + 1e-9)
+    self.assertLessEqual(second - first, 0.15 + 1e-9)
+    self.assertEqual(plant.planner.mpc.source, LongitudinalPlanSource.cruise)
+
+  def test_disengaged_cruise_state_cannot_leak_into_first_sport_accel(self):
+    plant = Plant(enabled=False, speed=10.0)
+    configure(plant, profile=AccelProfile.sport)
+
+    with (
+      mock.patch.object(plant.planner.mpc, "update", return_value=None),
+      mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan", return_value=1.30),
+    ):
+      for _ in range(12):
+        self.assertAlmostEqual(plant.step(v_lead=0.0, v_cruise=30.0)["a_target"], 0.0)
+        self.assertAlmostEqual(plant.planner.a_cruise, 0.0)
+
+      plant.enabled = True
+      first = plant.step(v_lead=0.0, v_cruise=30.0)["a_target"]
+
+    self.assertGreater(first, 0.0)
+    self.assertLessEqual(first, 0.10)