let the non PCM party begins

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

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
 

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

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)