From ebdface431708afe4d0743a157d23eae6e26ddc7 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 22 Apr 2026 21:58:17 -0500 Subject: [PATCH] throttle hysteresis --- .../controls/lib/longitudinal_planner.py | 17 +++++-- .../tests/test_longitudinal_planner.py | 44 ++++++++++++++++--- 2 files changed, 53 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index fa34c28cb..68777dbfe 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -22,6 +22,9 @@ A_CRUISE_MAX_BP = [0.0, 5., 10., 15., 20., 25., 40.] A_CRUISE_MAX_VALS = [1.125, 1.125, 1.125, 1.125, 1.25, 1.25, 1.5] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.4 +ALLOW_THROTTLE_HYSTERESIS = 0.05 +ALLOW_THROTTLE_ENABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD + ALLOW_THROTTLE_HYSTERESIS +ALLOW_THROTTLE_DISABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD - ALLOW_THROTTLE_HYSTERESIS MIN_ALLOW_THROTTLE_SPEED = 2.5 RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5 RAW_LEAD_SAFETY_TTC = 7.0 @@ -144,6 +147,7 @@ class LongitudinalPlanner: self.mpc = LongitudinalMpc(dt=dt) self.fcw = False self.dt = dt + self.model_allow_throttle = True self.allow_throttle = True self.mode = 'acc' @@ -298,15 +302,22 @@ class LongitudinalPlanner: self.v_desired_filter.x = v_ego # Clip aEgo to cruise limits to prevent large accelerations when becoming active self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) + self.model_allow_throttle = True # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error self.v_model_error = self.get_model_speed_error(sm['modelV2'], v_ego) x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, starpilot_toggles) - # Don't clip at low speeds since throttle_prob doesn't account for creep - self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED - self.allow_throttle &= not sm['starpilotPlan'].disableThrottle + # Don't clip at low speeds since throttle_prob doesn't account for creep. Use + # hysteresis here because raw gasPressProb noise can chatter the throttle gate. + if v_ego <= MIN_ALLOW_THROTTLE_SPEED: + self.model_allow_throttle = True + elif self.model_allow_throttle: + self.model_allow_throttle = throttle_prob > ALLOW_THROTTLE_DISABLE_THRESHOLD + else: + self.model_allow_throttle = throttle_prob > ALLOW_THROTTLE_ENABLE_THRESHOLD + self.allow_throttle = self.model_allow_throttle and not sm['starpilotPlan'].disableThrottle if not self.allow_throttle: clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index b39cfb2ee..262a568ab 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -26,7 +26,7 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead return lead -def make_model(v_ego: float, desired_accel: float): +def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0): model = log.ModelDataV2.new_message() t_idxs = ModelConstants.T_IDXS @@ -45,14 +45,15 @@ def make_model(v_ego: float, desired_accel: float): model.acceleration.z = [0.0] * len(t_idxs) model.acceleration.t = [float(t) for t in t_idxs] - model.meta.disengagePredictions.gasPressProbs = [1.0] * 6 + model.meta.disengagePredictions.gasPressProbs = [float(gas_press_prob)] * 6 model.action.desiredAcceleration = desired_accel model.action.shouldStop = False return model def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True, - tracking_lead: bool = False, lead_one=None, lead_two=None): + tracking_lead: bool = False, lead_one=None, lead_two=None, + gas_press_prob: float = 1.0, disable_throttle: bool = False): return { "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]), "carState": SimpleNamespace( @@ -68,7 +69,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta forceDecel=False, ), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), - "modelV2": make_model(v_ego, desired_accel), + "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob), "radarState": SimpleNamespace( leadOne=lead_one if lead_one is not None else make_lead(status=False), leadTwo=lead_two if lead_two is not None else make_lead(status=False), @@ -78,7 +79,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta vCruise=v_ego + 5.0, minAcceleration=min_accel, maxAcceleration=2.0, - disableThrottle=False, + disableThrottle=disable_throttle, trackingLead=tracking_lead, accelerationJerk=5.0, dangerJerk=5.0, @@ -191,3 +192,36 @@ def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch): np.testing.assert_allclose(captured["speeds"], 3.0) np.testing.assert_allclose(captured["accels"], -0.1) assert action.shouldStop + + +def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): + v_ego = 10.0 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, gas_press_prob=0.5) + toggles = make_toggles() + + planner.update(sm, toggles) + assert planner.model_allow_throttle + assert planner.allow_throttle + + sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.37) + planner.update(sm, toggles) + assert planner.model_allow_throttle + assert planner.allow_throttle + + sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.34) + planner.update(sm, toggles) + assert not planner.model_allow_throttle + assert not planner.allow_throttle + + sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.43) + planner.update(sm, toggles) + assert not planner.model_allow_throttle + assert not planner.allow_throttle + + sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.46) + planner.update(sm, toggles) + assert planner.model_allow_throttle + assert planner.allow_throttle