Smooth cruise response to noisy throttle

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
 

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.20
+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,14 @@ 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

@@ -0,0 +1,86 @@
+"""
+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

@@ -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)