diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index daff74038e..b06b257df3 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..2a84107a3d 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.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]) 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..915b53e8ee --- /dev/null +++ b/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)) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 0e69ece9f6..dbe15da30f 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)