CCmain for SLC

CC main button now accepts the current best speed limit source, even if new speed limit source is not flashing.
It snaps cruise speed to match (+ your offset), in either direction.
It works even before engaging longitudinal, so you can pre-set speed limit.
Won't interfere with cars using CC main for Always-On Lateral.

common/params_keys.h

@@ -482,6 +482,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"SNGHack", {PERSISTENT, BOOL, "1", "0", 2}},
     {"SoundPack", {PERSISTENT, STRING, "frog", "stock", 0}},
     {"SoundToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
+    {"SLCAdoptSpeedLimit", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},
+    {"SLCForceCruiseSpeed", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}},
     {"SpeedLimitAccepted", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},
     {"SpeedLimitChangedAlert", {PERSISTENT, BOOL, "0", "0", 0}},
     {"SpeedLimitController", {PERSISTENT, BOOL, "0", "0", 0}},

selfdrive/car/card.py

@@ -19,7 +19,8 @@ from opendbc.car.car_helpers import get_car, interfaces
 from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
 from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
-from openpilot.selfdrive.car.cruise import VCruiseHelper
+from openpilot.common.constants import CV
+from openpilot.selfdrive.car.cruise import VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN
 from openpilot.selfdrive.car.car_specific import MockCarState
 
 from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles
@@ -83,6 +84,7 @@ class Car:
     self.last_actuators_output = structs.CarControl.Actuators()
 
     self.params = Params()
+    self.params_memory = Params(memory=True)
 
     self.can_callbacks = can_comm_callbacks(self.can_sock, self.pm.sock['sendcan'])
 
@@ -225,6 +227,16 @@ class Car:
       self.sm['starpilotPlan'].speedLimitChanged,
       self.starpilot_toggles,
     )
+    slc_force_speed = self.params_memory.get_float("SLCForceCruiseSpeed")
+    if slc_force_speed > 0:
+      if self.is_metric:
+        new_cruise_kph = round(slc_force_speed * CV.MS_TO_KPH)
+      else:
+        new_cruise_kph = round(slc_force_speed * CV.MS_TO_MPH) * IMPERIAL_INCREMENT
+      self.v_cruise_helper.v_cruise_kph = max(min(new_cruise_kph, V_CRUISE_MAX), V_CRUISE_MIN)
+      self.v_cruise_helper.v_cruise_cluster_kph = self.v_cruise_helper.v_cruise_kph
+      self.params_memory.remove("SLCForceCruiseSpeed")
+
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on
       desired_speed_limit = self.sm['starpilotPlan'].slcSpeedLimit + self.sm['starpilotPlan'].slcSpeedLimitOffset

starpilot/controls/lib/speed_limit_controller.py

@@ -57,6 +57,8 @@ class SpeedLimitController:
     self.previous_source = "None"
     self.source = "None"
 
+    self._slc_adopt_counter = 0
+
     mapbox_requests_raw = self.starpilot_planner.params.get("MapBoxRequests", encoding="utf-8")
     try:
       self.mapbox_requests = json.loads(mapbox_requests_raw or "{}")
@@ -382,6 +384,21 @@ class SpeedLimitController:
       self.speed_limit_changed_timer = 0
       self.unconfirmed_speed_limit = 0
 
+    self._slc_adopt_counter += 1
+    if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"):
+      self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")
+      if desired_target > 0:
+        self.overridden_speed = 0
+        self.denied_target = 0
+        self.source = desired_source
+        self.target = desired_target
+        self.previous_source = desired_source
+        self.previous_target = desired_target
+        self.speed_limit_changed_timer = 0
+        self.unconfirmed_speed_limit = 0
+        self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target)
+        self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", self.target + self.offset)
+
   def update_map_speed_limit(self, v_ego, sm):
     next_speed_limit_distance = sm["mapdOut"].nextSpeedLimitDistance
 

starpilot/controls/starpilot_card.py

@@ -84,8 +84,11 @@ class StarPilotCard:
       for be in carState.buttonEvents:
         if be.type == ButtonType.lkas and be.pressed and starpilot_toggles.always_on_lateral_lkas:
           self.always_on_lateral_allowed = not self.always_on_lateral_allowed
-        elif be.type == ButtonType.mainCruise and be.pressed and starpilot_toggles.always_on_lateral_main:
-          self.always_on_lateral_allowed = not self.always_on_lateral_allowed
+        elif be.type == ButtonType.mainCruise and be.pressed:
+          if starpilot_toggles.always_on_lateral_main:
+            self.always_on_lateral_allowed = not self.always_on_lateral_allowed
+          elif starpilot_toggles.speed_limit_controller:
+            self.params_memory.put_bool("SLCAdoptSpeedLimit", True)
     elif starpilot_toggles.always_on_lateral_main:
       self.always_on_lateral_allowed = carState.cruiseState.available