From 1d8e7c7a0465120f8a1269658f0542c42a620faf Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 26 Sep 2025 01:09:43 -0400 Subject: [PATCH] Revert "get buttonEvents at 100 hz and process for 20 hz consumption" This reverts commit a739d4d4379080fceffda8d7d080c0260519f144. --- selfdrive/controls/plannerd.py | 6 -- .../controls/lib/longitudinal_planner.py | 5 +- .../lib/speed_limit/speed_limit_assist.py | 65 +++++-------------- 3 files changed, 19 insertions(+), 57 deletions(-) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 31c058ed95..8994fb7c08 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -26,15 +26,9 @@ def main(): 'liveMapDataSP', 'carStateSP', gps_location_service], poll='modelV2') - # Speed Limit Assist - sm_ext = messaging.SubMaster(['carState']) - speed_limit_assist = longitudinal_planner.sla - while True: sm.update() - sm_ext.update() if sm.updated['modelV2']: - speed_limit_assist.update_buttons(sm_ext['carState']) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) 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 99586a770d..04abdc642b 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -4,8 +4,6 @@ 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 time - from cereal import custom, car from openpilot.common.params import Params from openpilot.common.constants import CV @@ -34,7 +32,6 @@ LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on lim LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. V_CRUISE_UNSET = 255 -CONFIRM_BUTTON_TTL = 0.5 # s CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise) CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise) @@ -75,9 +72,6 @@ class SpeedLimitAssist: self._state_prev = SpeedLimitAssistState.disabled self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise - self._plus_hold = 0.0 - self._minus_hold = 0.0 - # TODO-SP: SLA's own output_a_target for planner # Solution functions mapped to respective states self.acceleration_solutions = { @@ -155,37 +149,6 @@ class SpeedLimitAssist: def get_active_state_target_acceleration(self) -> float: return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) - def update_buttons(self, CS: car.CarState) -> None: - """ - This is invoked directly in plannerd, running at 100 Hz. - :param CS: - :return: - """ - now = time.monotonic() - for b in CS.buttonEvents: - if not b.pressed: - if b.type in CRUISE_BUTTONS_PLUS: - self._plus_hold = max(self._plus_hold, now + CONFIRM_BUTTON_TTL) - elif b.type in CRUISE_BUTTONS_MINUS: - self._minus_hold = max(self._minus_hold, now + CONFIRM_BUTTON_TTL) - - def _get_button(self, req_plus: bool, req_minus: bool) -> bool: - now = time.monotonic() - if req_plus and now <= self._plus_hold: - self._plus_hold = 0.0 - return True - if req_minus and now <= self._minus_hold: - self._minus_hold = 0.0 - return True - - # expired - if now > self._plus_hold: - self._plus_hold = 0.0 - if now > self._minus_hold: - self._minus_hold = 0.0 - - return False - def _update_confirmed_state(self): if self._has_speed_limit: if self.v_offset < LIMIT_SPEED_OFFSET_TH: @@ -195,16 +158,20 @@ class SpeedLimitAssist: else: self.state = SpeedLimitAssistState.pending - def _update_non_pcm_long_confirmed_state(self) -> bool: + def _update_non_pcm_long_confirmed_state(self, CS) -> bool: if self.target_set_speed_confirmed: return True - if self.state != SpeedLimitAssistState.preActive: - return False + if self.state == SpeedLimitAssistState.preActive: + 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 () - req_plus = self.target_set_speed_conv > self.v_cruise_cluster_conv - req_minus = self.target_set_speed_conv < self.v_cruise_cluster_conv - return self._get_button(req_plus, req_minus) + for b in CS.buttonEvents: + if b.type in expected and not b.pressed: + return True + + return False def update_state_machine_pcm_op_long(self): self.long_engaged_timer = max(0, self.long_engaged_timer - 1) @@ -273,7 +240,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) @@ -293,7 +260,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 @@ -304,7 +271,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 @@ -315,7 +282,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 @@ -345,7 +312,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 @@ -362,7 +329,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)