From 883d88f2d32fd2c189b80a75707ff97a15a3d002 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 19 Jul 2026 13:01:51 -0700 Subject: [PATCH] Prevent longitudinal surging without softening launch --- .../lib/accel_personality/constants.py | 6 +- .../tests/test_accel_controller.py | 17 +- .../selfdrive/controls/lib/dec/constants.py | 7 +- sunnypilot/selfdrive/controls/lib/dec/dec.py | 64 +++++-- .../lib/dec/tests/test_dynamic_controller.py | 158 ++++++++++++++++- .../controls/lib/longitudinal_planner.py | 2 +- .../tests/test_vision_controller.py | 162 +++++++++++++++++- .../smart_cruise_control/vision_controller.py | 45 ++++- .../test_accel_controller_closed_loop.py | 36 ++++ 9 files changed, 455 insertions(+), 42 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 76c855fd5a..508dcfb981 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -23,9 +23,9 @@ PROFILE_CONFIGS = { ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0] ACCEL_PROFILE_MAX_V = { - AccelProfile.eco: [1.55, 1.25, 0.85, 0.40, 0.20], - AccelProfile.normal: [1.70, 1.40, 1.05, 0.55, 0.35], - AccelProfile.sport: [2.00, 1.90, 1.70, 0.90, 0.60], + AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16], + AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30], + AccelProfile.sport: [2.00, 1.90, 1.55, 0.80, 0.50], } CAP_FILTER_FRAMES = 5 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index bccb989ed2..ee99a211cd 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -68,9 +68,9 @@ class TestProfiles: def test_lookup_table_is_explicit_and_tunable(self): assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0] assert ACCEL_PROFILE_MAX_V == { - AccelProfile.eco: [1.55, 1.25, 0.85, 0.40, 0.20], - AccelProfile.normal: [1.70, 1.40, 1.05, 0.55, 0.35], - AccelProfile.sport: [2.00, 1.90, 1.70, 0.90, 0.60], + AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16], + AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30], + AccelProfile.sport: [2.00, 1.90, 1.55, 0.80, 0.50], } @pytest.mark.parametrize("profile", list(AccelProfile)) @@ -79,6 +79,8 @@ class TestProfiles: assert AccelController.get_profile_accel_max(profile, speed) == expected limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(-1.0, 50.0, 201)] assert all(0.0 <= limit <= ACCEL_MAX for limit in limits) + post_launch_limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(3.0, 40.0, 149)] + assert np.all(np.diff(post_launch_limits) <= 0.0) @pytest.mark.parametrize("speed", [0.0, 3.0, 10.0, 25.0, 40.0]) def test_profile_order_is_distinct(self, speed): @@ -98,6 +100,15 @@ class TestProfiles: else: np.testing.assert_array_equal(result.mpc_accel_max, min(expected + POSITIVE_MPC_HEADROOM, ACCEL_MAX)) + @pytest.mark.parametrize(("profile", "expected"), [ + (AccelProfile.eco, 1.25), (AccelProfile.normal, 1.40), (AccelProfile.sport, 1.90), + ]) + def test_launch_strength_is_preserved_through_three_meters_per_second(self, profile, expected): + result = update(make_controller(), v_ego=3.0, profile=profile) + assert result.profile_accel_max == expected + assert result.positive_accel_max == expected + assert result.effective_accel_max == expected + def test_turn_or_throttle_limit_intersects_profile(self): result = update(make_controller(), profile=AccelProfile.sport, stock_accel_max=0.0) assert result.positive_accel_max == 0.0 diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py index be3aab5ebc..ab173cf8ad 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -15,10 +15,9 @@ class WMACConstants: LEAD_EXIT_PROB = 0.25 LEAD_RISE_RATE = 1.0 LEAD_FALL_RATE = 0.35 - RADAR_LEAD_ACC_PROB = 0.5 - RADAR_LEAD_ACC_EXIT_PROB = 0.4 - RADAR_LEAD_ACC_RISE_RATE = 1.0 - RADAR_LEAD_ACC_FALL_RATE = 0.25 + RADAR_LEAD_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL))) + RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL))) + RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL))) SLOW_DOWN_PROB = 0.5 SLOW_DOWN_EXIT_PROB = 0.4 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 0f881c21dd..cb06593e04 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -135,12 +135,6 @@ class DynamicExperimentalController: rise_rate=WMACConstants.LEAD_RISE_RATE, fall_rate=WMACConstants.LEAD_FALL_RATE, ) - self._radar_acc_lead_tracker = HysteresisSignal( - enter_threshold=WMACConstants.RADAR_LEAD_ACC_PROB, - exit_threshold=WMACConstants.RADAR_LEAD_ACC_EXIT_PROB, - rise_rate=WMACConstants.RADAR_LEAD_ACC_RISE_RATE, - fall_rate=WMACConstants.RADAR_LEAD_ACC_FALL_RATE, - ) self._slow_down_tracker = HysteresisSignal( enter_threshold=WMACConstants.SLOW_DOWN_PROB, exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB, @@ -155,7 +149,12 @@ class DynamicExperimentalController: ) self._has_lead_filtered = False + self._has_any_lead = False + self._has_current_radar_acc_lead = False self._has_radar_acc_lead = False + self._radar_acc_lead_frames = 0 + self._radar_fresh = True + self._radar_stale_frames = 0 self._has_slow_down = False self._has_slowness = False self._has_mpc_fcw = False @@ -186,7 +185,7 @@ class DynamicExperimentalController: def set_mpc_fcw_crash_cnt(self) -> None: self._mpc_fcw_crash_cnt = self._mpc.crash_cnt - def _update_calculations(self, sm: messaging.SubMaster) -> None: + def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None: car_state = sm['carState'] radar_state = sm['radarState'] lead_one = radar_state.leadOne @@ -202,9 +201,24 @@ class DynamicExperimentalController: else: self._standstill_count = max(0, self._standstill_count - 1) - self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status)) - radar_acc_lead_score = max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two)) - self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(radar_acc_lead_score) + self._radar_fresh = bool(radar_fresh) + if self._radar_fresh: + self._radar_stale_frames = 0 + self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status)) + self._has_any_lead = bool(lead_one.status or lead_two.status) + self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two))) + self._update_radar_acc_lead() + else: + self._radar_stale_frames += 1 + self._has_current_radar_acc_lead = False + if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES: + self._update_radar_acc_lead() + else: + self._lead_tracker.reset() + self._has_lead_filtered = False + self._has_any_lead = False + self._has_radar_acc_lead = False + self._radar_acc_lead_frames = 0 self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0 self._calculate_slow_down(md) @@ -237,6 +251,18 @@ class DynamicExperimentalController: 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 _update_radar_acc_lead(self) -> None: + if self._has_current_radar_acc_lead: + self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + self._has_radar_acc_lead = True + return + + if not self._has_any_lead: + self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES) + + self._has_radar_acc_lead = self._radar_acc_lead_frames > 0 + self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1) + def _model_action_urgency(self, md) -> float: action = getattr(md, 'action', None) if action is None: @@ -265,15 +291,23 @@ class DynamicExperimentalController: return urgency def _desired_mode(self) -> tuple[ModeType, bool]: + standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES + urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB + + if not self._CP.radarUnavailable and self._has_current_radar_acc_lead: + return 'acc', True + + if (not self._radar_fresh or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down): + self._radar_acc_lead_frames = 0 + self._has_radar_acc_lead = False + return 'blended', True + if not self._CP.radarUnavailable and self._has_radar_acc_lead: return 'acc', True if self._has_mpc_fcw: return 'blended', True - standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES - urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB - if self._CP.radarUnavailable: if standstill or self._has_slow_down: return 'blended', urgent_slow_down @@ -284,10 +318,10 @@ class DynamicExperimentalController: return 'acc', False - def update(self, sm: messaging.SubMaster) -> None: + def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True) -> None: self._read_params() self.set_mpc_fcw_crash_cnt() - self._update_calculations(sm) + self._update_calculations(sm, radar_fresh) mode, immediate = self._desired_mode() self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES, diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index 00d803c7f3..a307960317 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -1,5 +1,6 @@ import pytest +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal @@ -286,28 +287,171 @@ 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, radar=True, radarTrackId=7) - controller.update(default_sm) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0) + for _ in range(2): + controller.update(default_sm) + assert controller._has_slow_down default_sm['radarState'] = MockRadarState(status=0.0) - default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) controller.update(default_sm) assert controller._has_lead_filtered assert controller.mode() == "acc" -def test_radar_lead_dropout_guard_expires(mock_cp, mock_mpc, default_sm): +def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) - controller.update(default_sm) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0) + for _ in range(2): + controller.update(default_sm) + assert controller._has_slow_down - default_sm['radarState'] = MockRadarState(status=0.0) - default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) - for _ in range(3): + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES): controller.update(default_sm) assert controller._has_radar_acc_lead assert controller.mode() == "acc" controller.update(default_sm) assert not controller._has_radar_acc_lead + assert controller.mode() == "acc" + + for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1): + controller.update(default_sm) assert controller.mode() == "blended" + + +def test_radar_lead_short_dropout_guard_expires_without_any_lead(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=0.0) + for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + + controller.update(default_sm) + assert not controller._has_radar_acc_lead + + +def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + + controller.update(default_sm, radar_fresh=False) + + assert not controller._has_current_radar_acc_lead + assert controller._has_radar_acc_lead + assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1 + assert controller._radar_stale_frames == 1 + assert controller.mode() == "acc" + + +def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + + for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1): + controller.update(default_sm, radar_fresh=False) + assert controller._has_radar_acc_lead + + controller.update(default_sm, radar_fresh=False) + + assert not controller._has_current_radar_acc_lead + assert not controller._has_radar_acc_lead + assert not controller._has_any_lead + assert not controller._has_lead_filtered + + +def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + for _ in range(WMACConstants.RADAR_STALE_FRAMES): + controller.update(default_sm, radar_fresh=False) + + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm, radar_fresh=False) + assert controller.mode() == "blended" + + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + controller.update(default_sm, radar_fresh=True) + + assert controller._radar_stale_frames == 0 + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + +@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"]) +def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=0.0) + if urgent_source == "fcw": + mock_mpc.crash_cnt = 1 + else: + default_sm['modelV2'] = MockModelData(valid=False, should_stop=True) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + mock_mpc.crash_cnt = 0 + default_sm['modelV2'] = MockModelData(valid=True) + controller.update(default_sm) + assert controller.mode() == "blended" + + +def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + + for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2): + if frame % 2 == 0: + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0)) + else: + default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8)) + controller.update(default_sm) + + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + +def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1): + controller.update(default_sm) + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two) + controller.update(default_sm) + + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index dcc9bddab1..03e2437c4a 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -204,7 +204,7 @@ class LongitudinalPlannerSP: def update(self, sm: messaging.SubMaster) -> None: self._read_accel_controller_params() self.events_sp.clear() - self.dec.update(sm) + self.dec.update(sm, radar_fresh=self._radar_fresh(sm)) self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py index 2a2c6a6be1..ef27022238 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py @@ -4,6 +4,8 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +from types import SimpleNamespace + import numpy as np import pytest @@ -13,8 +15,11 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import ( + _ACCEL_RELEASE_RATE, _ENTERING_PRED_LAT_ACC_TH, _RELIEF_CONFIRMATION_FRAMES, _TARGET_RELEASE_RATE, SmartCruiseControlVision, +) VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState @@ -118,6 +123,21 @@ class TestSmartCruiseControlVision: def reset_params(self): self.params.put_bool("SmartCruiseControlVision", True, block=True) + def set_lat_accels(self, current: float, predicted: float) -> None: + v_ego = 20. + self.sm['controlsState'].curvature = current / v_ego**2 + self.sm['modelV2'].velocity.x = [1.] * len(ModelConstants.T_IDXS) + self.sm['modelV2'].orientationRate.z = [predicted] * len(ModelConstants.T_IDXS) + + def update_lat_accels(self, current: float, predicted: float, cruise: float = 30., a_ego: float = 0.) -> None: + self.set_lat_accels(current, predicted) + self.scc_v.update(self.sm, True, False, 20., a_ego, cruise) + + def enter_curve(self, predicted: float = 2.2) -> None: + self.update_lat_accels(0.5, predicted) + self.update_lat_accels(0.5, predicted) + assert self.scc_v.state == VisionState.entering + def test_initial_state(self): assert self.scc_v.state == VisionState.disabled assert not self.scc_v.is_active @@ -143,6 +163,146 @@ class TestSmartCruiseControlVision: self.scc_v.update(self.sm, True, False, 0., 0., 0.) assert self.scc_v.state == VisionState.enabled + def test_unconfirmed_leaving_and_reentry_never_request_propulsion(self): + self.enter_curve() + targets = [(self.scc_v.output_v_target, self.scc_v.output_a_target)] + assert targets[-1][1] < 0. + + self.update_lat_accels(2., 2.2) + assert self.scc_v.state == VisionState.turning + targets.append((self.scc_v.output_v_target, self.scc_v.output_a_target)) + + self.update_lat_accels(1.2, 1.2) + assert self.scc_v.state == VisionState.leaving + targets.append((self.scc_v.output_v_target, self.scc_v.output_a_target)) + + self.update_lat_accels(1., 3.) + assert self.scc_v.state == VisionState.entering + targets.append((self.scc_v.output_v_target, self.scc_v.output_a_target)) + + v_targets, a_targets = np.array(targets).T + assert np.all(np.diff(v_targets[:-1]) <= 0.) + assert v_targets[-1] < v_targets[-2] + assert np.all(a_targets < 0.) + assert np.all(np.diff(a_targets[:-1]) <= 0.) + assert a_targets[-1] < a_targets[-2] + + def test_new_curve_interrupts_confirmed_release_immediately(self): + self.enter_curve() + for _ in range(_RELIEF_CONFIRMATION_FRAMES + 1): + self.update_lat_accels(0.8, 0.8) + releasing_v_target = self.scc_v.output_v_target + releasing_a_target = self.scc_v.output_a_target + assert self.scc_v.state == VisionState.leaving + + self.update_lat_accels(0.8, 3.) + assert self.scc_v.state == VisionState.entering + assert self.scc_v.output_v_target < releasing_v_target + assert self.scc_v.output_a_target < releasing_a_target + + def test_jitter_requires_confirmed_relief_then_releases_smoothly(self): + self.enter_curve() + held_v_target = self.scc_v.output_v_target + held_a_target = self.scc_v.output_a_target + + for frame in range(_RELIEF_CONFIRMATION_FRAMES * 2): + self.update_lat_accels(1., 1.05 if frame % 2 == 0 else 1.15) + assert self.scc_v.state == VisionState.entering + assert self.scc_v.output_v_target == held_v_target + assert self.scc_v.output_a_target == held_a_target + + for _ in range(_RELIEF_CONFIRMATION_FRAMES): + self.update_lat_accels(1.15, 0.8) + assert self.scc_v.state == VisionState.entering + assert self.scc_v.output_v_target == held_v_target + assert self.scc_v.output_a_target == held_a_target + + release_cruise = held_v_target + 2.5 * _TARGET_RELEASE_RATE * DT_MDL + for _ in range(_RELIEF_CONFIRMATION_FRAMES - 1): + self.update_lat_accels(0.8, 0.8, release_cruise) + assert self.scc_v.state == VisionState.entering + assert self.scc_v.output_v_target == held_v_target + assert self.scc_v.output_a_target == held_a_target + + active_v_targets = [held_v_target] + active_a_targets = [held_a_target] + for _ in range(_RELIEF_CONFIRMATION_FRAMES + 10): + self.update_lat_accels(0.8, 0.8, release_cruise) + if not self.scc_v.is_active: + break + assert self.scc_v.state == VisionState.leaving + assert self.scc_v.output_v_target != V_CRUISE_UNSET + active_v_targets.append(self.scc_v.output_v_target) + active_a_targets.append(self.scc_v.output_a_target) + + assert self.scc_v.state == VisionState.enabled + assert self.scc_v.output_v_target == V_CRUISE_UNSET + assert active_v_targets[-1] == pytest.approx(release_cruise) + assert np.all((np.diff(active_v_targets) >= 0.) & (np.diff(active_v_targets) <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9)) + assert np.all((np.diff(active_a_targets) >= 0.) & (np.diff(active_a_targets) <= _ACCEL_RELEASE_RATE * DT_MDL + 1e-9)) + emitted_a_targets = [*active_a_targets, self.scc_v.output_a_target] + assert np.max(np.abs(np.diff(emitted_a_targets)) / DT_MDL) <= _ACCEL_RELEASE_RATE + 1e-9 + assert np.max(np.abs(np.diff(emitted_a_targets)) / DT_MDL) < 3. + + def test_negative_accel_handoff_is_continuous_through_planner_arbitration(self): + car_control = messaging.new_message('carControl') + car_control.carControl.enabled = True + car_control.carControl.cruiseControl.override = False + self.sm['carControl'] = car_control.carControl + self.sm['carState'].vCruiseCluster = 108. + + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.scc = SimpleNamespace( + vision=self.scc_v, + map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=0.), + update=lambda sm, enabled, override, v_ego, a_ego, v_cruise: self.scc_v.update( + sm, enabled, override, v_ego, a_ego, v_cruise), + ) + planner.resolver = SimpleNamespace( + speed_limit_valid=False, speed_limit_last_valid=False, speed_limit=0., speed_limit_final_last=0., distance=0., + update=lambda _v_ego, _sm: None, + ) + planner.sla = SimpleNamespace( + output_v_target=V_CRUISE_UNSET, output_a_target=0., update=lambda *_args: None, + ) + planner.events_sp = SimpleNamespace() + + self.set_lat_accels(0.5, 2.2) + planner.update_targets(self.sm, 20., 0., 30.) + planner.update_targets(self.sm, 20., 0., 30.) + assert planner.source == LongitudinalPlanSource.sccVision + held_v_target = self.scc_v.output_v_target + release_cruise = held_v_target + 0.5 * _TARGET_RELEASE_RATE * DT_MDL + + self.set_lat_accels(0.8, 0.8) + for _ in range(_RELIEF_CONFIRMATION_FRAMES + 30): + planner.update_targets(self.sm, 20., 0., release_cruise) + if self.scc_v.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and abs(self.scc_v.output_a_target) <= _ACCEL_RELEASE_RATE * DT_MDL: + break + + assert self.scc_v.state == VisionState.leaving + assert planner.source == LongitudinalPlanSource.sccVision + assert self.scc_v.output_v_target < release_cruise + + prior_accel = planner.output_a_target + assert prior_accel > -1. + planner.update_targets(self.sm, 20., -1., release_cruise) + assert self.scc_v.state == VisionState.leaving + assert planner.source == LongitudinalPlanSource.sccVision + assert planner.output_a_target == -1. + assert planner.output_a_target < prior_accel + assert self.scc_v.output_v_target < release_cruise + + active_accel = planner.output_a_target + planner.update_targets(self.sm, 20., -1., release_cruise) + assert self.scc_v.state == VisionState.leaving + assert planner.source == LongitudinalPlanSource.cruise + assert abs(planner.output_a_target - active_accel) / DT_MDL < 3. + + planner.update_targets(self.sm, 20., -1., release_cruise) + assert self.scc_v.state == VisionState.enabled + assert planner.source == LongitudinalPlanSource.cruise + @pytest.mark.parametrize( "case, should_enter", [ diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py index a9d2a66227..40d4b97e06 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py @@ -31,6 +31,10 @@ _A_LAT_REG_MAX = 2. # Maximum lateral acceleration _NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting. +_RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL))) +_TARGET_RELEASE_RATE = 1. # m/s^2 +_ACCEL_RELEASE_RATE = 1. # m/s^3 + # Lookup table for the minimum smooth deceleration during the ENTERING state # depending on the actual maximum absolute lateral acceleration predicted on the turn ahead. _ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state @@ -65,13 +69,29 @@ class SmartCruiseControlVision: self.state = VisionState.disabled self.current_lat_acc = 0. self.max_pred_lat_acc = 0. + self.relief_frames = 0 def get_a_target_from_control(self) -> float: + if self.is_active: + if self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES: + return min(self.a_ego, self.output_a_target + _ACCEL_RELEASE_RATE * DT_MDL) + return min(self.a_target, self.output_a_target, self.a_ego) return self.a_target + def _accel_release_ready(self) -> bool: + return abs(self.output_a_target - self.a_ego) <= _ACCEL_RELEASE_RATE * DT_MDL + def get_v_target_from_control(self) -> float: if self.is_active: - return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON + v_target = max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON + if self.output_v_target == V_CRUISE_UNSET: + return v_target + elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES: + if self.v_cruise_setpoint < self.output_v_target: + return self.v_cruise_setpoint + released_v_target = min(self.v_cruise_setpoint, self.output_v_target + _TARGET_RELEASE_RATE * DT_MDL) + return self.output_v_target if released_v_target >= self.v_cruise_setpoint and not self._accel_release_ready() else released_v_target + return min(v_target, self.output_v_target) return V_CRUISE_UNSET @@ -101,6 +121,9 @@ class SmartCruiseControlVision: def _update_state_machine(self) -> tuple[bool, bool]: # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING + relief = self.current_lat_acc < _FINISH_LAT_ACC_TH and self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH + self.relief_frames = self.relief_frames + 1 if self.state in ACTIVE_STATES and relief else 0 + if self.state != VisionState.disabled: # longitudinal and feature disable always have priority in a non-disabled state if not self.long_enabled or not self.enabled: @@ -128,23 +151,27 @@ class SmartCruiseControlVision: # Transition to Turning if current lateral acceleration is over the threshold. if self.current_lat_acc >= _TURNING_LAT_ACC_TH: self.state = VisionState.turning - # Abort if the predicted lateral acceleration drops - elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH: - self.state = VisionState.enabled + # Begin releasing only after both current and predicted lateral acceleration stay clear. + elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES: + self.state = VisionState.leaving # TURNING elif self.state == VisionState.turning: - # Transition to Leaving if current lateral acceleration drops below a threshold. + # Transition out of Turning if current lateral acceleration drops below a threshold. if self.current_lat_acc <= _LEAVING_LAT_ACC_TH: - self.state = VisionState.leaving + self.state = VisionState.entering if self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH else VisionState.leaving # LEAVING elif self.state == VisionState.leaving: # Transition back to Turning if current lateral acceleration goes back over the threshold. if self.current_lat_acc >= _TURNING_LAT_ACC_TH: self.state = VisionState.turning - # Finish if current lateral acceleration goes below a threshold. - elif self.current_lat_acc < _FINISH_LAT_ACC_TH: + # Start a new turn cycle immediately if another curve is predicted. + elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: + self.state = VisionState.entering + # Finish after confirmed relief and a gradual release to the cruise setpoint. + elif (self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and self.output_v_target >= self.v_cruise_setpoint + and self._accel_release_ready()): self.state = VisionState.enabled # DISABLED @@ -157,6 +184,8 @@ class SmartCruiseControlVision: enabled = self.state in ENABLED_STATES active = self.state in ACTIVE_STATES + if not active: + self.relief_frames = 0 return enabled, active diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index b5bb25262b..b74d87e981 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -282,6 +282,42 @@ def test_e2e_to_radar_acc_handoff_keeps_braking_continuous(): assert active[transition] +def test_dec_retains_acc_through_route_like_radar_marker_dropout(): + dropout_start = 1.0 + reacquisition_time = 1.8 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation: + frame = round(current_time / DT_MDL) + if current_time < dropout_start: + marked_slot = "leadOne" if frame % 2 == 0 else "leadTwo" + return truth | {"radar": lead_name == marked_slot, "radarTrackId": 985 + frame if lead_name == marked_slot else -1} + if current_time < reacquisition_time: + return truth | {"radar": False, "radarTrackId": -1} + return truth | {"radar": lead_name == "leadOne", "radarTrackId": 1263 if lead_name == "leadOne" else -1} + + plant = Plant( + e2e=True, lead_relevancy=True, speed=20.0, distance_lead=35.0, lead_observation_fn=observe, + model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False), actuator_delay=0.15, actuator_lag=0.20, + ) + _configure_plant(plant, enabled=True, dec_enabled=True) + rows = [] + while plant.current_time < 2.5: + result = plant.step(v_lead=18.0, v_cruise=30.0) + rows.append((plant.current_time, result["a_target"], result["dec_mode"], str(result["mpc_source"]), result["fcw"])) + + time_values = np.asarray([row[0] for row in rows]) + acceleration = np.asarray([row[1] for row in rows]) + dropout = (time_values >= dropout_start) & (time_values < reacquisition_time) + response = (time_values >= dropout_start - DT_MDL) & (time_values <= reacquisition_time + 0.5) + assert all(row[2] == "acc" for row in rows) + assert all(row[3] != "e2e" for row in rows) + assert not any(row[4] for row in rows) + assert dropout.any() + assert not _has_propulsion_brake_cycle(acceleration[response]) + assert np.max(np.abs(np.diff(acceleration[response]) / DT_MDL)) < 3.0 + assert not plant.planner.accel_controller_fault_latched + + def test_active_controller_is_pre_mpc_and_preserves_stock_lead_authority(): plant = Plant(lead_relevancy=False, speed=0.0, actuator_delay=0.15, actuator_lag=0.20) _configure_plant(plant, enabled=True, profile=0)