diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index b06b257df3..daff74038e 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 = 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 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 2a84107a3d..f7b0a6854d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -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]) 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 deleted file mode 100644 index 915b53e8ee..0000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_allow_throttle.py +++ /dev/null @@ -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)) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index dbe15da30f..0e69ece9f6 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(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)