From 65812911fbb35e719bae274ed7deb98df1dd6330 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 8 Jul 2026 12:36:57 -0700 Subject: [PATCH] fix(long): revert leadunstale --- .../lib/accel_personality/accel_controller.py | 19 +++--------- .../tests/test_accel_controller.py | 29 ------------------- .../controls/lib/longitudinal_planner.py | 4 +-- 3 files changed, 5 insertions(+), 47 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 8d88740493..128e37fdee 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -11,11 +11,7 @@ Acceleration Personality (ECO / NORMAL / SPORT). Tunes only MPC INPUTS, never th any fresh accel<->decel direction change, when the tracked lead is itself braking hard, or when the gap is closing fast for any other reason (cut-in, ego overtaking a slower lead) -- the last one is the only proactive trigger keyed on an MPC INPUT (vRel) rather than a_ego's own realized sign flip, so it can - soften the very first brake jab instead of only the recovery after it. Both lead-keyed factors read the - RAW radarState lead (RadarDistanceController's cleanup only reaches the MPC, not here by construction -- - see get_jerk_scale) and are gated off (forced to 1.0, i.e. no relax) during a lead RadarDistance flagged - unstable last cycle, so a fusion/churn glitch on aLeadK/vRel can't fire a spurious relax the MPC's own - (cleaned-up) view never corroborates; + soften the very first brake jab instead of only the recovery after it; * add-only, speed-dependent follow-gap widen on the MPC t_follow -> earlier/gentler braking, roomier gap; * sticky should_stop hysteresis -> no stop-and-go gas-brake-gas-brake. Add-only gap => desired distance >= stock => braking >= stock. Disabled => stock everywhere (byte-stock). @@ -77,7 +73,6 @@ class AccelController: self._onset_factor = 1.0 self._lead_brake_factor = 1.0 self._closing_factor = 1.0 - self._lead_unstable = False self._read_params() def _read_params(self) -> None: @@ -87,16 +82,11 @@ class AccelController: return self._personality = get_sanitize_int_param("AccelPersonality", PERSONALITY_MIN, PERSONALITY_MAX, self._params) - def update(self, sm: messaging.SubMaster, lead_unstable: bool = False) -> None: - # lead_unstable: RadarDistanceController.lead_unstable() from the END of the PREVIOUS cycle (this - # cycle's radar_distance stability update hasn't run yet -- it happens inside smooth_radarstate(), - # called later, only on the MPC's path). One cycle (~20ms) stale is fine: instability episodes run - # ~0.4s+ (telemetry-verified), so a 1-cycle-old flag still covers the same episode almost always. + def update(self, sm: messaging.SubMaster) -> None: if self._frame % int(1. / DT_MDL) == 0: self._read_params() self._v_ego = float(sm['carState'].vEgo) self._a_ego = float(sm['carState'].aEgo) - self._lead_unstable = lead_unstable if self._enabled: lead = sm['radarState'].leadOne @@ -112,12 +102,12 @@ class AccelController: self._frame += 1 def _get_lead_brake_factor(self, lead) -> float: - if not lead.status or self._lead_unstable: + if not lead.status: return 1.0 return float(np.interp(lead.aLeadK, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V[self._personality])) def _get_closing_factor(self, lead) -> float: - if not lead.status or self._lead_unstable: + if not lead.status: return 1.0 return float(np.interp(lead.vRel, CLOSING_VREL_BP, CLOSING_FACTOR_V[self._personality])) @@ -128,7 +118,6 @@ class AccelController: self._onset_factor = 1.0 self._lead_brake_factor = 1.0 self._closing_factor = 1.0 - self._lead_unstable = False def get_max_accel(self, v_ego: float) -> float: # Disabled -> stock ceiling (off == stock, independent of the NORMAL profile so NORMAL is free to differ). 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 f33c5335d0..1bd8fe1daf 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 @@ -337,35 +337,6 @@ def test_closing_fires_before_a_ego_moves(): assert ctrl.get_jerk_scale(20.0) == pytest.approx(CLOSING_FACTOR_V[NORMAL][0]) -# --- lead_unstable gate: suppress both lead-keyed relax factors during a radar glitch ---------------------- - -def test_lead_unstable_suppresses_closing_factor(): - ctrl = make_controller(personality=SPORT) - ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=-6.0)), lead_unstable=True) - assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) - - -def test_lead_unstable_suppresses_lead_brake_factor(): - ctrl = make_controller(personality=SPORT) - ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, aLeadK=-3.0)), lead_unstable=True) - assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) - - -def test_lead_unstable_defaults_to_not_gating(): - # update() without the kwarg (existing callers, e.g. tests) must behave exactly as before this fix. - ctrl = make_controller(personality=SPORT) - ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=-6.0))) - assert ctrl.get_jerk_scale(20.0) == pytest.approx(CLOSING_FACTOR_V[SPORT][0]) - - -def test_lead_unstable_clears_once_stable_again(): - ctrl = make_controller(personality=SPORT) - ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=-6.0)), lead_unstable=True) - assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) - ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=-6.0)), lead_unstable=False) - assert ctrl.get_jerk_scale(20.0) == pytest.approx(CLOSING_FACTOR_V[SPORT][0]) - - # --- combined: get_jerk_scale takes the most-relaxed of all three factors ---------------------------------- def test_combined_takes_most_relaxed_factor(): diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 11bc175c81..23159b5a94 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -107,9 +107,7 @@ class LongitudinalPlannerSP: def update(self, sm: messaging.SubMaster) -> None: self.events_sp.clear() self.dec.update(sm) - # lead_unstable() reflects the previous cycle's radar_distance stability check (this cycle's hasn't run - # yet -- see AccelController.update's docstring) -- gates the accel-side lead-keyed relax off a glitch. - self.accel.update(sm, lead_unstable=self.radar_distance.lead_unstable()) + self.accel.update(sm) self.radar_distance.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp)