mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-21 09:13:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Smooth cruise response to noisy throttle"

Browse Source 
This reverts commit ef9883660cb6bfe036221267c55fe745dd08ace5.
This commit is contained in:
rav4kumar
2026-08-20 00:44:06 -07:00
parent 0fe69a3b96
commit 1470f1cb9e
4 changed files with 3 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/longitudinal_planner.py
+1 -1
View File
@@ -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 = self.update_allow_throttle(throttle_prob, low_speed_override=v_ego <= MIN_ALLOW_THROTTLE_SPEED, threshold=ALLOW_THROTTLE_THRESHOLD)
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py
+1 -16
View File
@@ -5,12 +5,9 @@ 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
@@ -33,11 +30,9 @@ MIN_ACCEL_BREAKPOINTS = [3., 4.5, 7., 9., 25.]
ACCEL_SMOOTH_ALPHA = 0.90
DECEL_SMOOTH_ALPHA = 0.40
ALLOW_THROTTLE_FILTER_RC = 0.20
ALLOW_THROTTLE_HYSTERESIS = 0.05
class AccelController:
def __init__(self, dt: float = DT_MDL):
def __init__(self):
self.params = Params()
self.frame = 0
self.last_max_accel = 2.0
@@ -46,8 +41,6 @@ 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
@@ -62,14 +55,6 @@ 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 not math.isfinite(throttle_prob):
throttle_prob = 0.0
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 = filtered_throttle_prob > allow_threshold or low_speed_override
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])
openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_allow_throttle.py
-86
View File
@@ -1,86 +0,0 @@
"""
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_dips_do_not_toggle(self):
self.assertTrue(self.update(1.0))
for _ in range(4):
self.assertTrue(self.update(0.0))
def test_probability_chatter_preserves_schmitt_state(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_low_probability_disables(self):
self.assertTrue(self.update(1.0))
states = [self.update(0.0) for _ in range(6)]
self.assertEqual(states, [True, True, True, True, False, False])
def test_sustained_high_probability_reenables(self):
self.assertFalse(self.update(0.0))
states = [self.update(1.0) for _ in range(3)]
self.assertEqual(states, [False, 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_is_immediate(self):
self.assertTrue(self.update(0.0, True))
self.assertFalse(self.update(0.0))
def test_nonfinite_probability_fails_safe_without_poisoning_filter(self):
self.assertTrue(self.update(1.0))
self.assertTrue(self.update(math.nan))
self.assertTrue(math.isfinite(self.controller._throttle_prob_filter.x))
for value in (math.inf, -math.inf, math.nan):
self.update(value)
self.assertTrue(math.isfinite(self.controller._throttle_prob_filter.x))
self.assertFalse(self.update(math.nan))
self.assertTrue(self.update(1.0))
def test_filter_updates_once_per_call(self):
self.assertTrue(self.update(1.0))
self.assertTrue(self.update(0.0))
self.assertAlmostEqual(self.controller._throttle_prob_filter.x, 0.8)
def test_filter_remains_active_when_accel_profiles_are_disabled(self):
Params().put_bool("AccelPersonalityEnabled", False, block=True)
self.controller = AccelController()
self.assertFalse(self.controller.is_enabled())
self.assertTrue(self.update(1.0))
for _ in range(4):
self.assertTrue(self.update(0.0))
self.assertFalse(self.update(0.0))
openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+1 -4
View File
@@ -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(mpc.dt)
self.accel_controller = AccelController()
self.lead_departure_controller = LeadDepartureController(CP.openpilotLongitudinalControl and CP.autoResumeSng and not CP.notCar)
self.events_sp = EventsSP()
self.dec = DynamicExperimentalController(CP, mpc)
@@ -56,9 +56,6 @@ 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)