diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index f3da82fe7e..d39acf53a0 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -166,8 +166,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) self.output_should_stop = any(should_stop for _, _, should_stop in candidates) self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX) - self.accel_controller_active = bool(self.accel_controller.is_enabled() and not force_decel and - self.mpc.source == LongitudinalPlanSource.cruise) + self.accel_controller_active = self.is_accel_controller_active(force_decel) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0 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 f0328d40c5..2f591177c5 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -9,7 +9,6 @@ import numpy as np from openpilot.cereal import custom from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot import get_sanitize_int_param AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile @@ -35,15 +34,11 @@ CRUISE_DECEL_ACCEL = { # m/s^2; comfort-first cruise deceleration target class AccelController: def __init__(self): self.params = Params() - self.frame = 0 - self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) - self._enabled = self.params.get_bool("AccelPersonalityEnabled") + self.update() def update(self) -> None: - self.frame += 1 - if self.frame % int(1.0 / DT_MDL) == 0: - self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) - self._enabled = self.params.get_bool("AccelPersonalityEnabled") + self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) + self._enabled = self.params.get_bool("AccelPersonalityEnabled") @property def profile(self) -> int: @@ -61,5 +56,6 @@ class AccelController: response_time = CRUISE_DECEL_RESPONSE_TIME[self._profile] target_delta = v_target - v_ego - target_delta = max(target_delta / response_time, CRUISE_DECEL_ACCEL[self._profile] * response_time) - return float(v_ego + target_delta) + if target_delta < CRUISE_DECEL_ACCEL[self._profile] * response_time * 2.0: + return v_target + return float(v_ego + target_delta / response_time) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index dcaa52dfff..26fabe5f85 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -134,28 +134,21 @@ class TestAccelController(OpenpilotTestCase): def test_profile_change_refreshes_ceiling(self): controller = self.set_profile(AccelProfile.normal) self.params.put("AccelPersonality", AccelProfile.sport, block=True) - controller.frame = int(1.0 / DT_MDL) - 1 controller.update() index = MAX_ACCEL_BREAKPOINTS.index(10.0) assert controller.get_max_accel(10.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][index] - def test_params_refresh_once_per_second(self): + def test_params_refresh_every_update(self): controller = self.set_profile(AccelProfile.normal) self.params.put("AccelPersonality", AccelProfile.sport, block=True) controller.update() - assert controller.profile == AccelProfile.normal - controller.frame = int(1.0 / DT_MDL) - 1 - controller.update() assert controller.profile == AccelProfile.sport - assert controller.frame == 0 def test_enabled_param_refresh(self): controller = self.set_profile(AccelProfile.normal) self.params.put_bool("AccelPersonalityEnabled", False, block=True) - controller.frame = int(1.0 / DT_MDL) - 1 controller.update() assert not controller.is_enabled() - assert controller.frame == 0 class TestPlannerIntegration(OpenpilotTestCase): @@ -241,7 +234,6 @@ class TestPlannerIntegration(OpenpilotTestCase): planner.a_cruise = planner.accel_controller.get_max_accel(v_ego) self.params.put("AccelPersonality", AccelProfile.eco, block=True) - planner.accel_controller.frame = int(1.0 / DT_MDL) - 1 planner.accel_controller.update() ceiling = planner.get_max_accel_override(v_ego) previous = planner.a_cruise diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 5b920ac4bb..34196bf92b 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -70,6 +70,10 @@ class LongitudinalPlannerSP: return self.accel_controller.get_cruise_target(v_ego, v_target) + def is_accel_controller_active(self, force_decel: bool) -> bool: + return bool(self.accel_controller.is_enabled() and not force_decel and + self.mpc.source == MpcPlanSource.cruise) + def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool: radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False) return radar_valid and ((source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or