From a739d4d4379080fceffda8d7d080c0260519f144 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 26 Sep 2025 00:17:39 -0400 Subject: [PATCH] get buttonEvents at 100 hz and process for 20 hz consumption --- selfdrive/controls/plannerd.py | 6 ++ .../controls/lib/longitudinal_planner.py | 5 +- .../lib/speed_limit/speed_limit_assist.py | 65 ++++++++++++++----- 3 files changed, 57 insertions(+), 19 deletions(-) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 8994fb7c08..31c058ed95 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -26,9 +26,15 @@ 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 5a09fbbf14..2b40fd523b 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -46,8 +46,7 @@ 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]: - CS = sm['carState'] - v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) + v_cruise_cluster_kph = min(sm['carState'].vCruiseCluster, V_CRUISE_MAX) v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS long_enabled = sm['carControl'].enabled @@ -64,7 +63,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, CS) + self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp) 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 04abdc642b..99586a770d 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -4,6 +4,8 @@ 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 @@ -32,6 +34,7 @@ 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) @@ -72,6 +75,9 @@ 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 = { @@ -149,6 +155,37 @@ 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: @@ -158,20 +195,16 @@ class SpeedLimitAssist: else: self.state = SpeedLimitAssistState.pending - def _update_non_pcm_long_confirmed_state(self, CS) -> bool: + def _update_non_pcm_long_confirmed_state(self) -> bool: if self.target_set_speed_confirmed: return True - 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 () + if self.state != SpeedLimitAssistState.preActive: + return False - for b in CS.buttonEvents: - if b.type in expected and not b.pressed: - return True - - return False + 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) def update_state_machine_pcm_op_long(self): self.long_engaged_timer = max(0, self.long_engaged_timer - 1) @@ -240,7 +273,7 @@ class SpeedLimitAssist: return enabled, active - def update_state_machine_non_pcm_long(self, CS: car.CarState): + def update_state_machine_non_pcm_long(self): self.long_engaged_timer = max(0, self.long_engaged_timer - 1) self.pre_active_timer = max(0, self.pre_active_timer - 1) @@ -260,7 +293,7 @@ class SpeedLimitAssist: # PRE_ACTIVE elif self.state == SpeedLimitAssistState.preActive: - if self._update_non_pcm_long_confirmed_state(CS): + if self._update_non_pcm_long_confirmed_state(): self.state = SpeedLimitAssistState.active elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD: # Timeout - session ended @@ -271,7 +304,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(CS): + elif self._update_non_pcm_long_confirmed_state(): self.state = SpeedLimitAssistState.active # DISABLED @@ -282,7 +315,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(CS): + if self._update_non_pcm_long_confirmed_state(): self.state = SpeedLimitAssistState.active else: self.state = SpeedLimitAssistState.preActive @@ -312,7 +345,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, CS: car.CarState) -> None: + speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None: self.long_enabled = long_enabled self.v_ego = v_ego self.a_ego = a_ego @@ -329,7 +362,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(CS) + self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long() self.update_events(events_sp)