diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 21cfa804e5..dfe3557f59 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -188,6 +188,13 @@ class DynamicExperimentalController: def active(self) -> bool: return self._active + def has_radar_acc_lead(self) -> bool: + # Same criterion _desired_mode() uses to force 'acc' mode: a near or fast-closing radar lead is trusted + # enough that the e2e model's own opinion should never blend in over pure MPC. Computed every cycle + # regardless of the DEC param/active() state (see _update_calculations, called unconditionally from + # update()), so callers can use this as a lead-safety baseline independent of whether DEC itself is on. + return not self._CP.radarUnavailable and self._has_radar_acc_lead + def is_urgent(self) -> bool: # Same "immediate" criterion _desired_mode() uses to decide a mode switch can't wait: an FCW-flagged MPC, # or a model slow-down whose smoothed hysteresis has latched AND whose raw severity clears the urgent 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 b434b07629..66808fc926 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -311,3 +311,42 @@ def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, defau assert controller._has_lead_filtered assert controller.mode() == "acc" + + +def test_has_radar_acc_lead_true_for_near_lead(mock_cp, mock_mpc, default_sm): + # within RADAR_LEAD_ACC_MAX_DREL -- available regardless of whether DEC's own param/active() is on, since + # _update_calculations runs every cycle unconditionally. + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=40.0, vRel=0.0) + controller.update(default_sm) + assert controller.has_radar_acc_lead() + + +def test_has_radar_acc_lead_false_for_far_slow_lead(mock_cp, mock_mpc, default_sm): + # beyond MAX_DREL and not closing fast enough for the TTC gate -- correctly not trusted as an ACC-safe lead. + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0) + controller.update(default_sm) + assert not controller.has_radar_acc_lead() + + +def test_has_radar_acc_lead_false_when_radar_unavailable(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=40.0, vRel=0.0) + controller.update(default_sm) + assert not controller.has_radar_acc_lead() + + +def test_has_radar_acc_lead_independent_of_dec_param(mock_cp, mock_mpc, default_sm): + # DEC disabled (param False) must not affect this -- it's a lead-safety baseline other callers rely on + # regardless of whether DEC itself is on. + class MockParamsOff: + def get_bool(self, name): + return False + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParamsOff()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=40.0, vRel=0.0) + controller.update(default_sm) + assert not controller.enabled() + assert not controller.active() + assert controller.has_radar_acc_lead() diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 23159b5a94..86f709ecfa 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -69,10 +69,20 @@ class LongitudinalPlannerSP: def is_e2e(self, sm: messaging.SubMaster) -> bool: experimental_mode = sm['selfdriveState'].experimentalMode - if not self.dec.active(): - return experimental_mode + if not experimental_mode: + return False - return experimental_mode and self.dec.mode() == "blended" + # A near/fast-closing radar lead always routes to pure MPC, regardless of whether DEC itself is on -- + # this baseline is not something the DEC toggle should be able to bypass (dec.has_radar_acc_lead() is + # computed every cycle independent of dec.active()). DEC's own toggle only gates the OTHER blended + # triggers (standstill, slow-down, FCW) below. + if self.dec.has_radar_acc_lead(): + return False + + if not self.dec.active(): + return True + + return self.dec.mode() == "blended" def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: CS = sm['carState'] diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_is_e2e.py b/sunnypilot/selfdrive/controls/lib/tests/test_is_e2e.py new file mode 100644 index 0000000000..ddeaaa1610 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_is_e2e.py @@ -0,0 +1,62 @@ +""" +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. + +LongitudinalPlannerSP.is_e2e() decides whether the e2e model's raw action.desiredAcceleration blends into +the MPC's solution via min(). A near/fast-closing radar lead must always route to pure MPC regardless of +whether DEC itself is on -- that baseline previously lived entirely inside DEC's active()-gated branch, so +turning DEC off silently dropped it (identical lead input, different is_e2e() answer). These tests pin the +fix: the lead check now runs unconditionally, before DEC's own toggle is even consulted. +""" + +from types import SimpleNamespace + +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP + + +class FakeDec: + def __init__(self, active=False, mode="acc", has_radar_acc_lead=False): + self._active = active + self._mode = mode + self._has_radar_acc_lead = has_radar_acc_lead + + def active(self): + return self._active + + def mode(self): + return self._mode + + def has_radar_acc_lead(self): + return self._has_radar_acc_lead + + +def make_sm(experimental_mode=True): + return {'selfdriveState': SimpleNamespace(experimentalMode=experimental_mode)} + + +def is_e2e(dec): + # is_e2e only reads self.dec -- no need to construct the full LongitudinalPlannerSP. + return LongitudinalPlannerSP.is_e2e(SimpleNamespace(dec=dec), make_sm()) + + +def test_experimental_mode_off_never_e2e(): + sm_off = make_sm(experimental_mode=False) + assert not LongitudinalPlannerSP.is_e2e(SimpleNamespace(dec=FakeDec(active=True, mode="blended")), sm_off) + assert not LongitudinalPlannerSP.is_e2e(SimpleNamespace(dec=FakeDec(has_radar_acc_lead=True)), sm_off) + + +def test_lead_present_blocks_e2e_regardless_of_dec_active(): + # the bug this fixes: identical lead, DEC on vs off must agree. + assert not is_e2e(FakeDec(active=True, mode="acc", has_radar_acc_lead=True)) + assert not is_e2e(FakeDec(active=False, mode="acc", has_radar_acc_lead=True)) + + +def test_no_lead_dec_off_falls_back_to_experimental_mode(): + assert is_e2e(FakeDec(active=False, has_radar_acc_lead=False)) + + +def test_no_lead_dec_on_follows_dec_mode(): + assert is_e2e(FakeDec(active=True, mode="blended", has_radar_acc_lead=False)) + assert not is_e2e(FakeDec(active=True, mode="acc", has_radar_acc_lead=False))