From 736cb76f4e00d573ac8041687fbdaf5cf5cc7cac Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Sep 2025 00:04:37 -0400 Subject: [PATCH] use button events instead of raw vals --- sunnypilot/selfdrive/car/cruise_ext.py | 12 +++++- .../controls/lib/longitudinal_planner.py | 5 ++- .../lib/speed_limit/speed_limit_assist.py | 37 +++++++++-------- .../tests/test_speed_limit_assist.py | 41 +++++++++++-------- 4 files changed, 57 insertions(+), 38 deletions(-) diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py index 671c97c36a..62766a4e65 100644 --- a/sunnypilot/selfdrive/car/cruise_ext.py +++ b/sunnypilot/selfdrive/car/cruise_ext.py @@ -11,6 +11,7 @@ from opendbc.car import structs from openpilot.common.constants import CV from openpilot.common.params import Params from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES ButtonType = car.CarState.ButtonEvent.Type SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState @@ -55,6 +56,7 @@ class VCruiseHelperSP: self.sla_state = SpeedLimitAssistState.disabled self.prev_sla_state = SpeedLimitAssistState.disabled self.speed_limit_final_last_kph = 0. + self.prev_speed_limit_final_last_kph = 0. def read_custom_set_speed_params(self) -> None: self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") @@ -103,12 +105,18 @@ class VCruiseHelperSP: self.speed_limit_final_last_kph = LP_SP.speedLimit.resolver.speedLimitFinalLast * CV.MS_TO_KPH self.sla_state = LP_SP.speedLimit.assist.state + @property + def update_speed_limit_final_last_changed(self) -> bool: + return bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph) + @property def update_speed_limit_assist_pre_active_confirmed(self) -> bool: - return self.sla_state == SpeedLimitAssistState.active and self.prev_sla_state == SpeedLimitAssistState.preActive + return self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive def update_speed_limit_assist_v_cruise_non_pcm(self) -> None: - if self.update_speed_limit_assist_pre_active_confirmed: + if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or + self.update_speed_limit_final_last_changed): self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX) self.prev_sla_state = self.sla_state + self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 2b40fd523b..5a09fbbf14 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -46,7 +46,8 @@ class LongitudinalPlannerSP: return self.dec.mode() def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: - v_cruise_cluster_kph = min(sm['carState'].vCruiseCluster, V_CRUISE_MAX) + CS = sm['carState'] + v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS long_enabled = sm['carControl'].enabled @@ -63,7 +64,7 @@ class LongitudinalPlannerSP: # Speed Limit Assist has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit, - self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp) + self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp, CS) targets = { LongitudinalPlanSource.cruise: (v_cruise, a_ego), diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index 7fa3d587e8..20d2775890 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -4,11 +4,10 @@ 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. """ -from cereal import custom +from cereal import custom, car from openpilot.common.params import Params from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP @@ -16,6 +15,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode from openpilot.selfdrive.modeld.constants import ModelConstants +ButtonType = car.CarState.ButtonEvent.Type EventNameSP = custom.OnroadEventSP.EventName SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source @@ -30,6 +30,10 @@ LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active. LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers. LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. +V_CRUISE_UNSET = 255 + +CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise) +CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise) class SpeedLimitAssist: @@ -140,19 +144,18 @@ class SpeedLimitAssist: else: self.state = SpeedLimitAssistState.pending - def _update_non_pcm_long_confirmed_state(self): - target_delta = self.target_set_speed_conv - self.prev_v_cruise_cluster_conv - v_cruise_cluster_delta = self.v_cruise_cluster_conv - self.prev_v_cruise_cluster_conv - - if target_delta == 0: + def _update_non_pcm_long_confirmed_state(self, CS) -> bool: + if self.target_set_speed_confirmed: return True - if v_cruise_cluster_delta == 0: + + if self.state != SpeedLimitAssistState.preActive: return False - if self.state == SpeedLimitAssistState.preActive: - return (target_delta > 0) == (v_cruise_cluster_delta > 0) + req_plus = self.target_set_speed_conv > self.v_cruise_cluster_conv + req_minus = self.target_set_speed_conv < self.v_cruise_cluster_conv + expected = CRUISE_BUTTONS_PLUS if req_plus else CRUISE_BUTTONS_MINUS if req_minus else () - return False + return any(b.type in expected and not b.pressed for b in CS.buttonEvents) def update_state_machine_pcm_op_long(self): self.long_engaged_timer = max(0, self.long_engaged_timer - 1) @@ -217,7 +220,7 @@ class SpeedLimitAssist: return enabled, active - def update_state_machine_non_pcm_long(self): + def update_state_machine_non_pcm_long(self, CS: car.CarState): self.long_engaged_timer = max(0, self.long_engaged_timer - 1) self.pre_active_timer = max(0, self.pre_active_timer - 1) @@ -234,7 +237,7 @@ class SpeedLimitAssist: # PRE_ACTIVE elif self.state == SpeedLimitAssistState.preActive: - if self._update_non_pcm_long_confirmed_state(): + if self._update_non_pcm_long_confirmed_state(CS): self.state = SpeedLimitAssistState.active elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD: # Timeout - session ended @@ -245,7 +248,7 @@ class SpeedLimitAssist: if self.speed_limit_changed: self.state = SpeedLimitAssistState.preActive self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) - elif self._update_non_pcm_long_confirmed_state(): + elif self._update_non_pcm_long_confirmed_state(CS): self.state = SpeedLimitAssistState.active # DISABLED @@ -256,7 +259,7 @@ class SpeedLimitAssist: self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) elif self.long_engaged_timer <= 0: - if self._update_non_pcm_long_confirmed_state(): + if self._update_non_pcm_long_confirmed_state(CS): self.state = SpeedLimitAssistState.active else: self.state = SpeedLimitAssistState.preActive @@ -286,7 +289,7 @@ class SpeedLimitAssist: events_sp.add(EventNameSP.speedLimitChanged) def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float, - speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None: + speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP, CS: car.CarState) -> None: self.long_enabled = long_enabled self.v_ego = v_ego self.a_ego = a_ego @@ -303,7 +306,7 @@ class SpeedLimitAssist: if self.pcm_op_long: self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long() else: - self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long() + self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long(CS) self.update_events(events_sp) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index 67811360ea..e9d008e1d8 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -4,6 +4,7 @@ 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 cereal.messaging as messaging from cereal import custom from opendbc.car.car_helpers import interfaces @@ -31,6 +32,11 @@ SPEED_LIMITS = { } +def generate_carState(): + car_state = messaging.new_message('carState') + return car_state + + class TestSpeedLimitAssist: def setup_method(self): @@ -39,6 +45,7 @@ class TestSpeedLimitAssist: self.events_sp = EventsSP() CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2) self.sla = SpeedLimitAssist(CI.CP) + self.sm = {'carState': generate_carState()} self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) def teardown_method(self, method): @@ -84,58 +91,58 @@ class TestSpeedLimitAssist: def test_disabled(self): self.params.put("SpeedLimitMode", int(Mode.off)) for _ in range(int(10. / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.disabled def test_transition_disabled_to_preactive(self): for _ in range(int(3. / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.preActive assert self.sla.is_enabled and not self.sla.is_active def test_preactive_to_active_with_max_speed_confirmation(self): self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.active assert self.sla.is_enabled and self.sla.is_active, f"enabled: {self.sla.is_enabled}, active: {self.sla.is_active}" assert self.sla.output_v_target == SPEED_LIMITS['city'] def test_preactive_timeout_to_inactive(self): self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)): - self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.inactive def test_preactive_to_pending_no_speed_limit(self): self.sla.state = SpeedLimitAssistState.preActive - self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, 0, False, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, 0, False, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.pending assert self.sla.is_enabled and not self.sla.is_active def test_pending_to_active_when_speed_limit_available(self): self.sla.state = SpeedLimitAssistState.pending - self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.active def test_pending_to_adapting_when_below_speed_limit(self): self.sla.state = SpeedLimitAssistState.pending - self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.adapting assert self.sla.is_enabled and self.sla.is_active def test_active_to_adapting_transition(self): self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) - self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.adapting def test_adapting_to_active_transition(self): self.sla.state = SpeedLimitAssistState.adapting self.sla.v_cruise_cluster_prev = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric] - self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.active def test_manual_cruise_change_detection(self): @@ -144,7 +151,7 @@ class TestSpeedLimitAssist: self.sla.v_cruise_cluster_prev = expected_cruise different_cruise = SPEED_LIMITS['highway'] + 5 - self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.inactive def test_rapid_speed_limit_changes(self): @@ -152,7 +159,7 @@ class TestSpeedLimitAssist: speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']] for _, speed_limit in enumerate(speed_limits): - self.sla.update(True, False, speed_limit, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], speed_limit, speed_limit, True, 0, self.events_sp) + self.sla.update(True, False, speed_limit, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], speed_limit, speed_limit, True, 0, self.events_sp, self.sm['carState']) assert self.sla.state in ACTIVE_STATES def test_invalid_speed_limits_handling(self): @@ -161,7 +168,7 @@ class TestSpeedLimitAssist: invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] for invalid_limit in invalid_limits: - self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert isinstance(self.sla.output_v_target, (int, float)) assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0 @@ -169,7 +176,7 @@ class TestSpeedLimitAssist: self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) old_speed_limit = SPEED_LIMITS['city'] - self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, old_speed_limit, True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, old_speed_limit, True, 0, self.events_sp, self.sm['carState']) assert self.sla.state in ACTIVE_STATES assert self.sla.output_v_target == old_speed_limit @@ -181,7 +188,7 @@ class TestSpeedLimitAssist: current_speed = SPEED_LIMITS['highway'] target_speed = SPEED_LIMITS['city'] - self.sla.update(True, False, current_speed, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], target_speed, target_speed, True, distance, self.events_sp) + self.sla.update(True, False, current_speed, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], target_speed, target_speed, True, distance, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.adapting assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions @@ -189,7 +196,7 @@ class TestSpeedLimitAssist: self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]) self.sla.update(False, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], - SPEED_LIMITS['city'], True, 0, self.events_sp) + SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state == SpeedLimitAssistState.disabled assert self.sla.output_v_target == V_CRUISE_UNSET @@ -208,7 +215,7 @@ class TestSpeedLimitAssist: initial_state = state - self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp, self.sm['carState']) assert self.sla.state in ALL_STATES # Sanity check