diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2e52c00827..f3ad96cef9 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -71,7 +71,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP']) self.can_rcv_cum_timeout_counter = 0 @@ -216,6 +216,7 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + self.v_cruise_helper.update_speed_limit_assist(self.sm['longitudinalPlanSP']) self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric) if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index c82287d2b1..35577ca0e3 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -53,6 +53,7 @@ class VCruiseHelper(VCruiseHelperSP): if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled): # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, _enabled, is_metric) + self.update_speed_limit_assist_v_cruise_non_pcm() self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -74,6 +75,12 @@ class VCruiseHelper(VCruiseHelperSP): if not enabled: return + # Speed Limit Assist for Non PCM long cars. + # True: Disallow set speed changes when user confirmed the target set speed during preActive state + # False: Allow set speed changes as SLA is not requesting user confirmation + if self.update_speed_limit_assist_pre_active_confirmed: + return + long_press = False button_type = None diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py index 716e3e1c93..64b7b81355 100644 --- a/sunnypilot/selfdrive/car/cruise_ext.py +++ b/sunnypilot/selfdrive/car/cruise_ext.py @@ -6,18 +6,22 @@ See the LICENSE.md file in the root directory for more details. """ import numpy as np -from cereal import car +from cereal import car, custom 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 ButtonType = car.CarState.ButtonEvent.Type +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.setCruise: 0, ButtonType.resumeCruise: 0, ButtonType.cancel: 0, ButtonType.mainCruise: 0} V_CRUISE_MIN = 8 +V_CRUISE_MAX = 145 +V_CRUISE_UNSET = 255 def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None: @@ -32,11 +36,11 @@ def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarSta button_timers[b.type.raw] = 1 if b.pressed else 0 - class VCruiseHelperSP: def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None: self.CP = CP self.CP_SP = CP_SP + self.v_cruise_kph = V_CRUISE_UNSET self.params = Params() self.v_cruise_min = 0 self.enabled_prev = False @@ -47,6 +51,11 @@ class VCruiseHelperSP: self.enable_button_timers = CRUISE_BUTTON_TIMER + # Speed Limit Assist + self.sla_state = SpeedLimitAssistState.disabled + self.prev_sla_state = SpeedLimitAssistState.disabled + self.speed_limit_final_kph = 0. + def read_custom_set_speed_params(self) -> None: self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True) @@ -89,3 +98,19 @@ class VCruiseHelperSP: return enabled and self.enabled_prev return enabled + + def update_speed_limit_assist(self, LP_SP: custom.LongitudinalPlanSP) -> None: + speed_limit_kph = LP_SP.speedLimit.resolver.speedLimit + speed_limit_offset_kph = LP_SP.speedLimit.resolver.speedLimitOffset + self.speed_limit_final_kph = (speed_limit_kph + speed_limit_offset_kph) * CV.MS_TO_KPH + self.sla_state = LP_SP.speedLimit.assist.state + + @property + def update_speed_limit_assist_pre_active_confirmed(self) -> bool: + return self.sla_state == SpeedLimitAssistState.active and 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: + self.v_cruise_kph = np.clip(round(self.speed_limit_final_kph, 1), self.v_cruise_min, V_CRUISE_MAX) + + self.prev_sla_state = self.sla_state 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 a488fddc76..6221e6e3fe 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -341,7 +341,10 @@ class SpeedLimitAssist: self.update_params() self.update_calculations(v_cruise_cluster) - self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long() + 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.update_events(events_sp)