From 60017b61e2cca183772bf73f8f8694c039728d60 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 26 Sep 2025 01:09:42 -0400 Subject: [PATCH] Revert "get v cruise cluster from outer loop" This reverts commit be8068e8ab9b52c7e8e1b3f29e89f994b3e911df. --- selfdrive/controls/plannerd.py | 2 +- .../controls/lib/longitudinal_planner.py | 5 ++- .../lib/speed_limit/speed_limit_assist.py | 43 ++++++++----------- 3 files changed, 23 insertions(+), 27 deletions(-) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 04d0f70773..31c058ed95 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -34,7 +34,7 @@ def main(): sm.update() sm_ext.update() if sm.updated['modelV2']: - speed_limit_assist.update_car_state(sm_ext['carState']) + 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 16b6dc9f55..2b40fd523b 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -46,6 +46,9 @@ 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) + v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS + long_enabled = sm['carControl'].enabled long_override = sm['carControl'].cruiseControl.override @@ -59,7 +62,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, self.resolver.speed_limit, + 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) targets = { 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 1acfaa0582..99586a770d 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -32,8 +32,6 @@ 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_MAX = 145 V_CRUISE_UNSET = 255 CONFIRM_BUTTON_TTL = 0.5 # s @@ -119,28 +117,9 @@ class SpeedLimitAssist: self.is_metric = self.params.get_bool("IsMetric") self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist - def update_car_state(self, sm_ext): - """ - This is invoked directly in plannerd, running at 100 Hz. - """ - - CS = sm_ext['carState'] - v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) - self.v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS - - self.update_buttons(CS) - - def update_buttons(self, CS: car.CarState) -> None: - 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 update_calculations(self) -> None: + def update_calculations(self, v_cruise_cluster: float) -> None: speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + self.v_cruise_cluster = v_cruise_cluster # Update current velocity offset (error) self.v_offset = self._speed_limit_final_last - self.v_ego @@ -176,6 +155,20 @@ 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: @@ -351,7 +344,7 @@ class SpeedLimitAssist: elif self.speed_limit_prev > 0: events_sp.add(EventNameSP.speedLimitChanged) - def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, speed_limit: float, + 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: self.long_enabled = long_enabled self.v_ego = v_ego @@ -363,7 +356,7 @@ class SpeedLimitAssist: self._distance = distance self.update_params() - self.update_calculations() + self.update_calculations(v_cruise_cluster) self._state_prev = self.state if self.pcm_op_long: