From da907078b066d7560a6b141cc876ccf41832c12f Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 16 Aug 2026 02:22:05 -0700 Subject: [PATCH] oscillation --- .../controls/lib/longitudinal_planner.py | 11 +- .../lib/accel_controller/accel_controller.py | 46 +++++--- .../lib/accel_controller/constants.py | 5 +- .../tests/test_accel_controller.py | 56 ++++++++-- .../tests/test_accel_controller_interfaces.py | 27 +++++ .../selfdrive/controls/lib/dec/dec.py | 27 +++-- .../lib/dec/tests/test_dec_planner_gate.py | 4 - .../lib/dec/tests/test_dynamic_controller.py | 17 ++- .../controls/lib/longitudinal_planner.py | 17 ++- .../tests/test_vision_controller.py | 102 +++++++++++++++--- .../smart_cruise_control/vision_controller.py | 51 +++++++-- .../test_accel_controller_closed_loop.py | 95 +++++++++++++++- 12 files changed, 381 insertions(+), 77 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 172bd52ce6..c508dd6b8f 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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]) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index 5f7d4b0cb7..4e81f201d8 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py index 58712019db..c86f0ae35f 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py +++ b/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: diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 270adc3a25..8a71e06f7c 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/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): diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py index b9a6457ee8..fd6fffe3fc 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py +++ b/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): diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py index dc3551768e..60840453d2 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py index 86746b4ce9..dc9b4ae380 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py +++ b/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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index 1f73ec1e1d..8b86880690 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 62ca1e625c..e55e4899ce 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py index 4ca4cb1cf5..21bf7d44a3 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py +++ b/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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py index 99b26ddf74..7916d6b3d3 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ b/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: diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index d25de9d161..f64c11516b 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/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)