From 1fadaa5d123b357c8e5d67ff0fc2cfc515d3be40 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 20 Aug 2026 14:30:03 -0700 Subject: [PATCH] Reduce cruise oscillation from noisy throttle intent --- .../controls/lib/longitudinal_planner.py | 2 +- .../lib/accel_controller/accel_controller.py | 27 +++++- .../tests/test_allow_throttle.py | 94 +++++++++++++++++++ .../controls/lib/longitudinal_planner.py | 5 +- 4 files changed, 125 insertions(+), 3 deletions(-) create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_allow_throttle.py diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 003937f189..8da6b21144 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -111,7 +111,7 @@ 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 + self.allow_throttle = self.update_allow_throttle(throttle_prob, low_speed_override=v_ego <= MIN_ALLOW_THROTTLE_SPEED, threshold=ALLOW_THROTTLE_THRESHOLD) steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg 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 f7b0a6854d..667b2d2e20 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -5,9 +5,12 @@ 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. """ +import math + import numpy as np from openpilot.cereal import custom +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot import get_sanitize_int_param @@ -30,9 +33,11 @@ MIN_ACCEL_BREAKPOINTS = [3., 4.5, 7., 9., 25.] ACCEL_SMOOTH_ALPHA = 0.90 DECEL_SMOOTH_ALPHA = 0.40 +ALLOW_THROTTLE_FILTER_RC = 0.10 +ALLOW_THROTTLE_HYSTERESIS = 0.05 class AccelController: - def __init__(self): + def __init__(self, dt: float = DT_MDL): self.params = Params() self.frame = 0 self.last_max_accel = 2.0 @@ -41,6 +46,8 @@ class AccelController: self.min_accel_first_run = True self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) self._enabled = self.params.get_bool("AccelPersonalityEnabled") + self._allow_throttle = True + self._throttle_prob_filter = FirstOrderFilter(0.0, ALLOW_THROTTLE_FILTER_RC, dt, initialized=False) def update(self, sm=None) -> None: self.frame += 1 @@ -55,6 +62,24 @@ class AccelController: def is_enabled(self) -> bool: return self._enabled + def update_allow_throttle(self, throttle_prob: float, low_speed_override: bool, threshold: float) -> bool: + if low_speed_override: + self._allow_throttle = True + self._throttle_prob_filter.x = 0.0 + self._throttle_prob_filter.initialized = False + return True + + if not math.isfinite(throttle_prob): + self._allow_throttle = False + self._throttle_prob_filter.x = 0.0 + self._throttle_prob_filter.initialized = True + return False + + filtered_throttle_prob = self._throttle_prob_filter.update(throttle_prob) + allow_threshold = threshold if self._allow_throttle else threshold + ALLOW_THROTTLE_HYSTERESIS + self._allow_throttle = bool(filtered_throttle_prob > allow_threshold) + return self._allow_throttle + def get_max_accel(self, v_ego: float) -> float: v_ego = max(0.0, v_ego) target_max = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_allow_throttle.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_allow_throttle.py new file mode 100644 index 0000000000..44f757ebcc --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_allow_throttle.py @@ -0,0 +1,94 @@ +""" +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. +""" + +import math + +from openpilot.common.params import Params +from openpilot.common.test import OpenpilotTestCase +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController + + +class TestAllowThrottle(OpenpilotTestCase): + def setUp(self): + self.controller = AccelController() + + def update(self, throttle_prob: float, low_speed_override: bool = False) -> bool: + return self.controller.update_allow_throttle(throttle_prob, low_speed_override=low_speed_override, threshold=0.4) + + def test_short_probability_dip(self): + self.assertTrue(self.update(1.0)) + self.assertTrue(self.update(0.0)) + self.assertTrue(self.update(0.0)) + self.assertFalse(self.update(0.0)) + + def test_probability_chatter(self): + self.assertTrue(self.update(1.0)) + for _ in range(100): + self.assertTrue(self.update(0.39)) + self.assertTrue(self.update(0.46)) + + self.controller = AccelController() + self.assertFalse(self.update(0.0)) + for _ in range(100): + self.assertFalse(self.update(0.39)) + self.assertFalse(self.update(0.46)) + + def test_sustained_probability_changes(self): + self.assertTrue(self.update(1.0)) + self.assertEqual([self.update(0.0) for _ in range(4)], [True, True, False, False]) + + for _ in range(20): + self.assertFalse(self.update(0.0)) + self.assertEqual([self.update(1.0) for _ in range(2)], [False, True]) + + def test_threshold_boundaries(self): + self.assertFalse(self.update(0.4)) + + self.controller._throttle_prob_filter.initialized = False + self.assertFalse(self.update(0.45)) + + self.controller._throttle_prob_filter.initialized = False + self.assertTrue(self.update(math.nextafter(0.45, math.inf))) + + def test_low_speed_override(self): + for _ in range(20): + self.assertTrue(self.update(0.0, True)) + + self.assertTrue(self.update(1.0)) + self.assertTrue(self.update(math.nan, True)) + self.assertTrue(self.update(1.0)) + self.assertTrue(self.update(0.0, True)) + self.assertFalse(self.update(0.0)) + + def test_nonfinite_probability(self): + self.assertTrue(self.update(1.0)) + for value in (math.inf, -math.inf, math.nan): + self.assertFalse(self.update(value)) + self.assertTrue(math.isfinite(self.controller._throttle_prob_filter.x)) + + self.assertFalse(self.update(1.0)) + self.assertTrue(self.update(1.0)) + + def test_route_probability_trace(self): + probabilities = (0.941, 0.093, 0.070, 0.429, 0.430, 0.083, 0.509, 0.068) + states = [self.update(probability) for probability in probabilities] + self.assertEqual(states, [True, True, True, True, True, False, False, False]) + + def test_filter_updates_once(self): + self.assertTrue(self.update(1.0)) + self.assertTrue(self.update(0.0)) + self.assertAlmostEqual(self.controller._throttle_prob_filter.x, 2.0 / 3.0) + + def test_profiles_disabled(self): + Params().put_bool("AccelPersonalityEnabled", False, block=True) + self.controller = AccelController() + self.assertFalse(self.controller.is_enabled()) + + self.assertTrue(self.update(1.0)) + self.assertTrue(self.update(0.0)) + self.assertTrue(self.update(0.0)) + self.assertFalse(self.update(0.0)) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f254b17f2d..c43a29015b 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -25,7 +25,7 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc): - self.accel_controller = AccelController() + self.accel_controller = AccelController(mpc.dt) self.lead_departure_controller = LeadDepartureController(CP.openpilotLongitudinalControl and CP.autoResumeSng and not CP.notCar) self.events_sp = EventsSP() self.dec = DynamicExperimentalController(CP, mpc) @@ -56,6 +56,9 @@ class LongitudinalPlannerSP: return None return self.accel_controller.get_min_accel(v_ego) + def update_allow_throttle(self, throttle_prob: float, low_speed_override: bool, threshold: float) -> bool: + return self.accel_controller.update_allow_throttle(throttle_prob, low_speed_override=low_speed_override, threshold=threshold) + def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> bool: radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False) return self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid)