rename

This reverts commit 0570833ad3.

cereal/custom.capnp

@@ -32,6 +32,8 @@ enum ModelGeneration {
   three @3;
   four @4;
   five @5;
+  six @6;
+  seven @7;
 }
 
 struct ControlsStateSP @0x81c2f05a394cf4af {
@@ -203,24 +205,7 @@ struct ModelDataV2SP @0xf98d843bfd7004a3 {
   modelCapabilities @4 :UInt32;
 }
 
-struct CarControlSP @0xb86e6369214c01c8 {
-  customStockLongitudinalControl @0 :CustomStockLongitudinalControl;
-
-  struct CustomStockLongitudinalControl {
-    state @0 :ButtonControlState;
-    cruiseButton @1 :Int16;
-    finalSpeedKphDEPRECATED @2 :Float32;
-    vTarget @3 :Float32;
-    vCruiseCluster @4 :Float32;
-
-    enum ButtonControlState {
-      inactive @0;        # No button press or default state
-      loading @1;         # Loading state before transitioning to accelerating or decelerating
-      accelerating @2;    # Increasing speed
-      decelerating @3;    # Decreasing speed
-      holding @4;         # Holding steady speed
-    }
-  }
+struct CustomReserved7 @0xb86e6369214c01c8 {
 }
 
 struct CustomReserved8 @0xf416ec09499d9d19 {

cereal/log.capnp

@@ -871,6 +871,7 @@ struct DrivingModelData {
   frameId @0 :UInt32;
   frameIdExtra @1 :UInt32;
   frameDropPerc @6 :Float32;
+  modelExecutionTime @7 :Float32;
 
   action @2 :ModelDataV2.Action;
 
@@ -1010,6 +1011,8 @@ struct ModelDataV2 {
     brake3MetersPerSecondSquaredProbs @4 :List(Float32);
     brake4MetersPerSecondSquaredProbs @5 :List(Float32);
     brake5MetersPerSecondSquaredProbs @6 :List(Float32);
+    gasPressProbs @7 :List(Float32);
+    brakePressProbs @8 :List(Float32);
   }
 
   struct Pose {
@@ -2388,7 +2391,7 @@ struct Event {
     liveMapDataSP @111 :Custom.LiveMapDataSP;
     e2eLongStateSP @112 :Custom.E2eLongStateSP;
     modelV2SP @113 :Custom.ModelDataV2SP;
-    carControlSP @114 :Custom.CarControlSP;
+    customReserved7 @114 :Custom.CustomReserved7;
     customReserved8 @115 :Custom.CustomReserved8;
     customReserved9 @116 :Custom.CustomReserved9;
 

cereal/services.py

@@ -83,7 +83,6 @@ _services: dict[str, tuple] = {
   "liveMapDataSP": (True, 0.),
   "e2eLongStateSP": (True, 0.),
   "modelV2SP": (True, 20., 40),
-  "carControlSP": (True, 100., 10),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params.cc

@@ -239,6 +239,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DrivingModelGeneration", PERSISTENT},
     {"DrivingModelMetadataText", PERSISTENT},
     {"DrivingModelName", PERSISTENT},
+    {"DrivingModelSelectorVersion", CLEAR_ON_MANAGER_START},
     {"DrivingModelText", PERSISTENT},
     {"DrivingModelUrl", PERSISTENT},
     {"DynamicExperimentalControl", PERSISTENT | BACKUP},

openpilot/sunnypilot

@@ -1 +0,0 @@
-../sunnypilot/

selfdrive/car/card.py

@@ -31,7 +31,7 @@ class Car:
   def __init__(self, CI=None):
     self.can_sock = messaging.sub_sock('can', timeout=20)
     self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'])
-    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'carControlSP'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
 
     self.can_rcv_cum_timeout_counter = 0
 
@@ -70,12 +70,6 @@ class Car:
 
     if self.CP.customStockLongAvailable and self.CP.pcmCruise and self.params.get_bool("CustomStockLong"):
       self.CP.pcmCruiseSpeed = False
-      services = self.sm.data.keys() | {'longitudinalPlanSP'}
-      self.sm = messaging.SubMaster(list(services))
-
-      cslc_path = f'openpilot.sunnypilot.selfdrive.car.{self.CP.carName}.custom_stock_longitudinal_controller'
-      CustomStockLongitudinalController = __import__(cslc_path + '.controller', fromlist=['CustomStockLongitudinalController']).CustomStockLongitudinalController
-      self.custom_stock_longitudinal_controller = CustomStockLongitudinalController(self, self.CI.CC, self.CI.CS, self.CP)
 
     openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
 
@@ -181,16 +175,8 @@ class Car:
       # send car controls over can
       now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
       self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos)
-      if not self.CP.pcmCruiseSpeed:
-        can_sends.extend(self.custom_stock_longitudinal_controller.update(CS, CC))
       self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
 
-      cc_send_sp = messaging.new_message('carControlSP')
-      cc_send_sp.valid = self.sm.all_checks(['carControl'])
-      if not self.CP.pcmCruiseSpeed:
-        cc_send_sp.carControlSP.customStockLongitudinalControl = self.custom_stock_longitudinal_controller.state_publish()
-      self.pm.send('carControlSP', cc_send_sp)
-
       self.CC_prev = CC
 
   def step(self):

selfdrive/car/chrysler/carcontroller.py

@@ -1,10 +1,14 @@
 from cereal import car
 
+import cereal.messaging as messaging
+from common.conversions import Conversions as CV
 from opendbc.can.packer import CANPacker
-from openpilot.selfdrive.car import apply_meas_steer_torque_limits
+from openpilot.common.params import Params
+from openpilot.selfdrive.car import DT_CTRL, apply_meas_steer_torque_limits
 from openpilot.selfdrive.car.chrysler import chryslercan
 from openpilot.selfdrive.car.chrysler.values import RAM_CARS, RAM_DT, CarControllerParams, ChryslerFlags, ChryslerFlagsSP
 from openpilot.selfdrive.car.interfaces import CarControllerBase, FORWARD_GEARS
+from openpilot.selfdrive.controls.lib.drive_helpers import FCA_V_CRUISE_MIN
 
 ButtonType = car.CarState.ButtonEvent.Type
 
@@ -22,9 +26,64 @@ class CarController(CarControllerBase):
     self.packer = CANPacker(dbc_name)
     self.params = CarControllerParams(CP)
 
+    self.sm = messaging.SubMaster(['longitudinalPlanSP'])
+    self.param_s = Params()
+    self.last_speed_limit_sign_tap_prev = False
+    self.speed_limit = 0.
+    self.speed_limit_offset = 0
+    self.timer = 0
+    self.final_speed_kph = 0
+    self.init_speed = 0
+    self.current_speed = 0
+    self.v_set_dis = 0
+    self.v_cruise_min = 0
+    self.button_type = 0
+    self.button_select = 0
+    self.button_count = 0
+    self.target_speed = 0
+    self.t_interval = 7
+    self.slc_active_stock = False
+    self.sl_force_active_timer = 0
+    self.v_tsc_state = 0
+    self.slc_state = 0
+    self.m_tsc_state = 0
+    self.cruise_button = None
+    self.speed_diff = 0
+    self.v_tsc = 0
+    self.m_tsc = 0
+    self.steady_speed = 0
+    self.button_frame = 0
+
   def update(self, CC, CS, now_nanos):
+    if not self.CP.pcmCruiseSpeed:
+      self.sm.update(0)
+
+      if self.sm.updated['longitudinalPlanSP']:
+        self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
+        self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
+        self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState
+        self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit
+        self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset
+        self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
+        self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed
+
+      self.v_cruise_min = FCA_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
+
     can_sends = []
 
+    if not self.CP.pcmCruiseSpeed:
+      if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap:
+        self.sl_force_active_timer = self.frame
+        self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False)
+      self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap
+
+      sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
+      sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
+      sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False))
+      slc_active = not sl_inactive and not sl_temp_inactive
+
+      self.slc_active_stock = slc_active
+
     lkas_active = CC.latActive and CS.madsEnabled
 
     if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS:
@@ -55,6 +114,20 @@ class CarController(CarControllerBase):
         self.last_button_frame = self.frame
         can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, self.CP, resume=True))
 
+      if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and not self.CP.pcmCruiseSpeed and CS.out.cruiseState.enabled:
+        self.button_frame += 1
+        button_counter_offset = [1, 1, 0, None][self.button_frame % 4]
+        if ram_cars:
+          self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
+        elif button_counter_offset is not None:
+          self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
+
+        if self.cruise_button is not None:
+          if ram_cars:
+            can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter, das_bus, self.CP, buttons=self.cruise_button))
+          elif button_counter_offset is not None:
+            can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + button_counter_offset, das_bus, self.CP, buttons=self.cruise_button))
+
     # HUD alerts
     if self.frame % 25 == 0:
       if CS.lkas_car_model != -1:
@@ -106,3 +179,122 @@ class CarController(CarControllerBase):
     new_actuators.steerOutputCan = self.apply_steer_last
 
     return new_actuators, can_sends
+
+  # multikyd methods, sunnyhaibin logic
+  def get_cruise_buttons_status(self, CS):
+    if not CS.out.cruiseState.enabled:
+      for be in CS.out.buttonEvents:
+        if be.type in (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.resumeCruise) and be.pressed:
+          self.timer = 40
+    elif self.timer:
+      self.timer -= 1
+    else:
+      return 1
+    return 0
+
+  def get_target_speed(self, v_cruise_kph_prev):
+    v_cruise_kph = v_cruise_kph_prev
+    if self.slc_state > 1:
+      v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
+      if not self.slc_active_stock:
+        v_cruise_kph = v_cruise_kph_prev
+    return v_cruise_kph
+
+  def get_button_type(self, button_type):
+    self.type_status = "type_" + str(button_type)
+    self.button_picker = getattr(self, self.type_status, lambda: "default")
+    return self.button_picker()
+
+  def reset_button(self):
+    if self.button_type != 3:
+      self.button_type = 0
+
+  def type_default(self):
+    self.button_type = 0
+    return None
+
+  def type_0(self):
+    self.button_count = 0
+    self.target_speed = self.init_speed
+    self.speed_diff = self.target_speed - self.v_set_dis
+    if self.target_speed > self.v_set_dis:
+      self.button_type = 1
+    elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min:
+      self.button_type = 2
+    return None
+
+  def type_1(self):
+    cruise_button = 1
+    self.button_count += 1
+    if self.target_speed <= self.v_set_dis:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_2(self):
+    cruise_button = 2
+    self.button_count += 1
+    if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_3(self):
+    cruise_button = None
+    self.button_count += 1
+    if self.button_count > self.t_interval:
+      self.button_type = 0
+    return cruise_button
+
+  def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev):
+    if self.v_tsc_state != 0:
+      vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH
+      if int(vision_v_cruise_kph) == int(v_cruise_kph_prev):
+        vision_v_cruise_kph = 255
+    else:
+      vision_v_cruise_kph = 255
+    if self.m_tsc_state > 1:
+      map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH
+      if int(map_v_cruise_kph) == 0.0:
+        map_v_cruise_kph = 255
+    else:
+      map_v_cruise_kph = 255
+    curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH)
+    return min(target_speed_kph, curve_speed)
+
+  def get_button_control(self, CS, final_speed, v_cruise_kph_prev):
+    self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1))
+    self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH))
+    cruise_button = self.get_button_type(self.button_type)
+    return cruise_button
+
+  def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)):
+    if cur_speed > self.steady_speed:
+      self.steady_speed = cur_speed
+    elif cur_speed < self.steady_speed - hyst:
+      self.steady_speed = cur_speed
+    return self.steady_speed
+
+  def get_cruise_buttons(self, CS, v_cruise_kph_prev):
+    cruise_button = None
+    if not self.get_cruise_buttons_status(CS):
+      pass
+    elif CS.out.cruiseState.enabled:
+      set_speed_kph = self.get_target_speed(v_cruise_kph_prev)
+      if self.slc_state > 1:
+        target_speed_kph = set_speed_kph
+      else:
+        target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
+      if self.v_tsc_state != 0 or self.m_tsc_state > 1:
+        self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
+      else:
+        self.final_speed_kph = target_speed_kph
+
+      cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
+    return cruise_button

selfdrive/car/chrysler/chryslercan.py

@@ -65,9 +65,14 @@ def create_lkas_command(packer, CP, apply_steer, lkas_control_bit):
 
 def create_cruise_buttons(packer, frame, bus, CP, cruise_buttons_msg=None, buttons=0, cancel=False, resume=False):
 
+  acc_accel = 1 if buttons == 1 else 0
+  acc_decel = 1 if buttons == 2 else 0
+
   values = {
     "ACC_Cancel": cancel,
     "ACC_Resume": resume,
+    "ACC_Accel": acc_accel,
+    "ACC_Decel": acc_decel,
     "COUNTER": frame % 0x10,
   }
 

selfdrive/car/chrysler/interface.py

@@ -161,6 +161,8 @@ class CarInterface(CarInterfaceBase):
     if self.low_speed_alert:
       events.add(car.CarEvent.EventName.belowSteerSpeed)
 
+    ret.customStockLong = self.update_custom_stock_long()
+
     ret.events = events.to_msg()
 
     return ret

selfdrive/car/honda/carcontroller.py

@@ -1,12 +1,16 @@
 from collections import namedtuple
 
 from cereal import car
+import cereal.messaging as messaging
+from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.params import Params
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import DT_CTRL, rate_limit, make_tester_present_msg, create_gas_interceptor_command
 from openpilot.selfdrive.car.honda import hondacan
 from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
 from openpilot.selfdrive.car.interfaces import CarControllerBase
+from openpilot.selfdrive.controls.lib.drive_helpers import HONDA_V_CRUISE_MIN
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -124,7 +128,48 @@ class CarController(CarControllerBase):
     self.last_steer = 0.0
     self.last_button_frame = 0
 
+    self.sm = messaging.SubMaster(['longitudinalPlanSP'])
+    self.param_s = Params()
+    self.last_speed_limit_sign_tap_prev = False
+    self.speed_limit = 0.
+    self.speed_limit_offset = 0
+    self.timer = 0
+    self.final_speed_kph = 0
+    self.init_speed = 0
+    self.current_speed = 0
+    self.v_set_dis = 0
+    self.v_cruise_min = 0
+    self.button_type = 0
+    self.button_select = 0
+    self.button_count = 0
+    self.target_speed = 0
+    self.t_interval = 7
+    self.slc_active_stock = False
+    self.sl_force_active_timer = 0
+    self.v_tsc_state = 0
+    self.slc_state = 0
+    self.m_tsc_state = 0
+    self.cruise_button = None
+    self.speed_diff = 0
+    self.v_tsc = 0
+    self.m_tsc = 0
+    self.steady_speed = 0
+
   def update(self, CC, CS, now_nanos):
+    if not self.CP.pcmCruiseSpeed:
+      self.sm.update(0)
+
+      if self.sm.updated['longitudinalPlanSP']:
+        self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
+        self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
+        self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState
+        self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit
+        self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset
+        self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
+        self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed
+
+      self.v_cruise_min = HONDA_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
+
     actuators = CC.actuators
     hud_control = CC.hudControl
     conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric)
@@ -158,6 +203,19 @@ class CarController(CarControllerBase):
     apply_steer = int(interp(-limited_steer * self.params.STEER_MAX,
                              self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
 
+    if not self.CP.pcmCruiseSpeed:
+      if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap:
+        self.sl_force_active_timer = self.frame
+        self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False)
+      self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap
+
+      sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
+      sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
+      sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False))
+      slc_active = not sl_inactive and not sl_temp_inactive
+
+      self.slc_active_stock = slc_active
+
     # Send CAN commands
     can_sends = []
 
@@ -207,6 +265,15 @@ class CarController(CarControllerBase):
         can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.CANCEL, self.CP.carFingerprint))
       elif CC.cruiseControl.resume:
         can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, self.CP.carFingerprint))
+      elif CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
+        self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
+        if self.cruise_button is not None:
+          send_freq = 1
+          if not (self.v_tsc_state != 0 or self.m_tsc_state > 1) and abs(self.target_speed - self.v_set_dis) <= 2:
+            send_freq = 3
+          if (self.frame - self.last_button_frame) * DT_CTRL > 0.01 * send_freq:
+            can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, self.cruise_button, self.CP.carFingerprint))
+            self.last_button_frame = self.frame
 
     else:
       # Send gas and brake commands.
@@ -268,3 +335,120 @@ class CarController(CarControllerBase):
 
     self.frame += 1
     return new_actuators, can_sends
+
+  # multikyd methods, sunnyhaibin logic
+  def get_cruise_buttons_status(self, CS):
+    if not CS.out.cruiseState.enabled or CS.cruise_buttons != 0:
+      self.timer = 40
+    elif self.timer:
+      self.timer -= 1
+    else:
+      return 1
+    return 0
+
+  def get_target_speed(self, v_cruise_kph_prev):
+    v_cruise_kph = v_cruise_kph_prev
+    if self.slc_state > 1:
+      v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
+      if not self.slc_active_stock:
+        v_cruise_kph = v_cruise_kph_prev
+    return v_cruise_kph
+
+  def get_button_type(self, button_type):
+    self.type_status = "type_" + str(button_type)
+    self.button_picker = getattr(self, self.type_status, lambda: "default")
+    return self.button_picker()
+
+  def reset_button(self):
+    if self.button_type != 3:
+      self.button_type = 0
+
+  def type_default(self):
+    self.button_type = 0
+    return None
+
+  def type_0(self):
+    self.button_count = 0
+    self.target_speed = self.init_speed
+    self.speed_diff = self.target_speed - self.v_set_dis
+    if self.target_speed > self.v_set_dis:
+      self.button_type = 1
+    elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min:
+      self.button_type = 2
+    return None
+
+  def type_1(self):
+    cruise_button = CruiseButtons.RES_ACCEL
+    self.button_count += 1
+    if self.target_speed <= self.v_set_dis:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_2(self):
+    cruise_button = CruiseButtons.DECEL_SET
+    self.button_count += 1
+    if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_3(self):
+    cruise_button = None
+    self.button_count += 1
+    if self.button_count > self.t_interval:
+      self.button_type = 0
+    return cruise_button
+
+  def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev):
+    if self.v_tsc_state != 0:
+      vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH
+      if int(vision_v_cruise_kph) == int(v_cruise_kph_prev):
+        vision_v_cruise_kph = 255
+    else:
+      vision_v_cruise_kph = 255
+    if self.m_tsc_state > 1:
+      map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH
+      if int(map_v_cruise_kph) == 0.0:
+        map_v_cruise_kph = 255
+    else:
+      map_v_cruise_kph = 255
+    curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH)
+    return min(target_speed_kph, curve_speed)
+
+  def get_button_control(self, CS, final_speed, v_cruise_kph_prev):
+    self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1))
+    self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH))
+    cruise_button = self.get_button_type(self.button_type)
+    return cruise_button
+
+  def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)):
+    if cur_speed > self.steady_speed:
+      self.steady_speed = cur_speed
+    elif cur_speed < self.steady_speed - hyst:
+      self.steady_speed = cur_speed
+    return self.steady_speed
+
+  def get_cruise_buttons(self, CS, v_cruise_kph_prev):
+    cruise_button = None
+    if not self.get_cruise_buttons_status(CS):
+      pass
+    elif CS.out.cruiseState.enabled:
+      set_speed_kph = self.get_target_speed(v_cruise_kph_prev)
+      if self.slc_state > 1:
+        target_speed_kph = set_speed_kph
+      else:
+        target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
+      if self.v_tsc_state != 0 or self.m_tsc_state > 1:
+        self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
+      else:
+        self.final_speed_kph = target_speed_kph
+
+      cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
+    return cruise_button

selfdrive/car/honda/interface.py

@@ -329,6 +329,8 @@ class CarInterface(CarInterfaceBase):
     if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
       events.add(EventName.manualRestart)
 
+    ret.customStockLong = self.update_custom_stock_long()
+
     ret.events = events.to_msg()
 
     return ret

selfdrive/car/hyundai/carcontroller.py

@@ -7,8 +7,9 @@ from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg
 from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
 from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
-from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR
+from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR, LEGACY_SAFETY_MODE_CAR
 from openpilot.selfdrive.car.interfaces import CarControllerBase
+from openpilot.selfdrive.controls.lib.drive_helpers import HYUNDAI_V_CRUISE_MIN
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -61,10 +62,38 @@ class CarController(CarControllerBase):
     self.lat_disengage_init = False
     self.lat_active_last = False
 
+    sub_services = ['longitudinalPlanSP']
     if CP.openpilotLongitudinalControl:
-      self.sm = messaging.SubMaster(['radarState'])
+      sub_services.append('radarState')
+    # TODO: Always true, prep for future conditional refactoring
+    if sub_services:
+      self.sm = messaging.SubMaster(sub_services)
 
     self.param_s = Params()
+    self.last_speed_limit_sign_tap_prev = False
+    self.speed_limit = 0.
+    self.speed_limit_offset = 0
+    self.timer = 0
+    self.final_speed_kph = 0
+    self.init_speed = 0
+    self.current_speed = 0
+    self.v_set_dis = 0
+    self.v_cruise_min = 0
+    self.button_type = 0
+    self.button_select = 0
+    self.button_count = 0
+    self.target_speed = 0
+    self.t_interval = 7
+    self.slc_active_stock = False
+    self.sl_force_active_timer = 0
+    self.v_tsc_state = 0
+    self.slc_state = 0
+    self.m_tsc_state = 0
+    self.cruise_button = None
+    self.speed_diff = 0
+    self.v_tsc = 0
+    self.m_tsc = 0
+    self.steady_speed = 0
     self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
     self.lead_distance = 0
 
@@ -80,9 +109,21 @@ class CarController(CarControllerBase):
     return 19 if hud_control.leadVisible else 0
 
   def update(self, CC, CS, now_nanos):
-    if self.CP.openpilotLongitudinalControl and self.frame % 5 == 0:
+    if not self.CP.pcmCruiseSpeed or (self.CP.openpilotLongitudinalControl and self.frame % 5 == 0):
       self.sm.update(0)
 
+    if not self.CP.pcmCruiseSpeed:
+      if self.sm.updated['longitudinalPlanSP']:
+        self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
+        self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
+        self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState
+        self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit
+        self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset
+        self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
+        self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed
+
+      self.v_cruise_min = HYUNDAI_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
+
     actuators = CC.actuators
     hud_control = CC.hudControl
 
@@ -124,6 +165,19 @@ class CarController(CarControllerBase):
 
     blinking_icon = (self.frame - self.disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
 
+    if not self.CP.pcmCruiseSpeed:
+      if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap:
+        self.sl_force_active_timer = self.frame
+        self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False)
+      self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap
+
+      sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
+      sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
+      sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False))
+      slc_active = not sl_inactive and not sl_temp_inactive
+
+      self.slc_active_stock = slc_active
+
     self.lat_active_last = CC.latActive
 
     escc = self.CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC.value
@@ -178,6 +232,15 @@ class CarController(CarControllerBase):
       else:
         # button presses
         can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
+        if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
+          if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
+            # TODO: resume for alt button cars
+            pass
+          else:
+            self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
+            if self.cruise_button is not None:
+              if self.frame % 2 == 0:
+                can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
     else:
       can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req,
                                                 torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
@@ -187,6 +250,21 @@ class CarController(CarControllerBase):
 
       if not self.CP.openpilotLongitudinalControl:
         can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
+        if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
+          self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
+          if self.cruise_button is not None:
+            if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
+              send_freq = 1
+              if not (self.v_tsc_state != 0 or self.m_tsc_state > 1) and abs(self.target_speed - self.v_set_dis) <= 2:
+                send_freq = 5
+              # send resume at a max freq of 10Hz
+              if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
+                # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
+                can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP)] * 25)
+                if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
+                  self.last_button_frame = self.frame
+            elif self.frame % 2 == 0:
+              can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
@@ -258,3 +336,120 @@ class CarController(CarControllerBase):
             self.last_button_frame = self.frame
 
     return can_sends
+
+  # multikyd methods, sunnyhaibin logic
+  def get_cruise_buttons_status(self, CS):
+    if not CS.out.cruiseState.enabled or CS.cruise_buttons[-1] != Buttons.NONE:
+      self.timer = 40
+    elif self.timer:
+      self.timer -= 1
+    else:
+      return 1
+    return 0
+
+  def get_target_speed(self, v_cruise_kph_prev):
+    v_cruise_kph = v_cruise_kph_prev
+    if self.slc_state > 1:
+      v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
+      if not self.slc_active_stock:
+        v_cruise_kph = v_cruise_kph_prev
+    return v_cruise_kph
+
+  def get_button_type(self, button_type):
+    self.type_status = "type_" + str(button_type)
+    self.button_picker = getattr(self, self.type_status, lambda: "default")
+    return self.button_picker()
+
+  def reset_button(self):
+    if self.button_type != 3:
+      self.button_type = 0
+
+  def type_default(self):
+    self.button_type = 0
+    return None
+
+  def type_0(self):
+    self.button_count = 0
+    self.target_speed = self.init_speed
+    self.speed_diff = self.target_speed - self.v_set_dis
+    if self.target_speed > self.v_set_dis:
+      self.button_type = 1
+    elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min:
+      self.button_type = 2
+    return None
+
+  def type_1(self):
+    cruise_button = Buttons.RES_ACCEL
+    self.button_count += 1
+    if self.target_speed <= self.v_set_dis:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_2(self):
+    cruise_button = Buttons.SET_DECEL
+    self.button_count += 1
+    if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_3(self):
+    cruise_button = None
+    self.button_count += 1
+    if self.button_count > self.t_interval:
+      self.button_type = 0
+    return cruise_button
+
+  def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev):
+    if self.v_tsc_state != 0:
+      vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH
+      if int(vision_v_cruise_kph) == int(v_cruise_kph_prev):
+        vision_v_cruise_kph = 255
+    else:
+      vision_v_cruise_kph = 255
+    if self.m_tsc_state > 1:
+      map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH
+      if int(map_v_cruise_kph) == 0.0:
+        map_v_cruise_kph = 255
+    else:
+      map_v_cruise_kph = 255
+    curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH)
+    return min(target_speed_kph, curve_speed)
+
+  def get_button_control(self, CS, final_speed, v_cruise_kph_prev):
+    self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1))
+    self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH))
+    cruise_button = self.get_button_type(self.button_type)
+    return cruise_button
+
+  def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)):
+    if cur_speed > self.steady_speed:
+      self.steady_speed = cur_speed
+    elif cur_speed < self.steady_speed - hyst:
+      self.steady_speed = cur_speed
+    return self.steady_speed
+
+  def get_cruise_buttons(self, CS, v_cruise_kph_prev):
+    cruise_button = None
+    if not self.get_cruise_buttons_status(CS):
+      pass
+    elif CS.out.cruiseState.enabled:
+      set_speed_kph = self.get_target_speed(v_cruise_kph_prev)
+      if self.slc_state > 1:
+        target_speed_kph = set_speed_kph
+      else:
+        target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
+      if self.v_tsc_state != 0 or self.m_tsc_state > 1:
+        self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
+      else:
+        self.final_speed_kph = target_speed_kph
+
+      cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
+    return cruise_button

selfdrive/car/hyundai/interface.py

@@ -288,6 +288,8 @@ class CarInterface(CarInterfaceBase):
     if self.CS.params_list.hyundai_radar_tracks_available and not self.CS.params_list.hyundai_radar_tracks_available_cache:
       events.add(car.CarEvent.EventName.hyundaiRadarTracksAvailable)
 
+    ret.customStockLong = self.update_custom_stock_long()
+
     ret.events = events.to_msg()
 
     return ret

selfdrive/car/mazda/carcontroller.py

@@ -1,9 +1,14 @@
 from cereal import car
+import cereal.messaging as messaging
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_driver_steer_torque_limits
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.mazda import mazdacan
 from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
+from openpilot.selfdrive.controls.lib.drive_helpers import MAZDA_V_CRUISE_MIN
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 ButtonType = car.CarState.ButtonEvent.Type
@@ -16,9 +21,63 @@ class CarController(CarControllerBase):
     self.packer = CANPacker(dbc_name)
     self.brake_counter = 0
 
+    self.sm = messaging.SubMaster(['longitudinalPlanSP'])
+    self.param_s = Params()
+    self.last_speed_limit_sign_tap_prev = False
+    self.speed_limit = 0.
+    self.speed_limit_offset = 0
+    self.timer = 0
+    self.final_speed_kph = 0
+    self.init_speed = 0
+    self.current_speed = 0
+    self.v_set_dis = 0
+    self.v_cruise_min = 0
+    self.button_type = 0
+    self.button_select = 0
+    self.button_count = 0
+    self.target_speed = 0
+    self.t_interval = 7
+    self.slc_active_stock = False
+    self.sl_force_active_timer = 0
+    self.v_tsc_state = 0
+    self.slc_state = 0
+    self.m_tsc_state = 0
+    self.cruise_button = None
+    self.speed_diff = 0
+    self.v_tsc = 0
+    self.m_tsc = 0
+    self.steady_speed = 0
+
   def update(self, CC, CS, now_nanos):
+    if not self.CP.pcmCruiseSpeed:
+      self.sm.update(0)
+
+      if self.sm.updated['longitudinalPlanSP']:
+        self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
+        self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
+        self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState
+        self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit
+        self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset
+        self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
+        self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed
+
+      self.v_cruise_min = MAZDA_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
+
     can_sends = []
 
+    if not self.CP.pcmCruiseSpeed:
+      if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap:
+        self.sl_force_active_timer = self.frame
+        self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False)
+      self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap
+
+      sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
+      sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
+      sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False))
+      slc_active = not sl_inactive and not sl_temp_inactive
+
+      self.slc_active_stock = slc_active
+
     apply_steer = 0
 
     if CC.latActive:
@@ -43,6 +102,11 @@ class CarController(CarControllerBase):
         # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
         # Send Resume button when planner wants car to move
         can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
+      elif CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
+        self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
+        if self.cruise_button is not None:
+          if self.frame % 10 == 0:
+            can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, self.frame // 10, self.cruise_button))
 
     self.apply_steer_last = apply_steer
 
@@ -64,3 +128,125 @@ class CarController(CarControllerBase):
 
     self.frame += 1
     return new_actuators, can_sends
+
+  # multikyd methods, sunnyhaibin logic
+  def get_cruise_buttons_status(self, CS):
+    if not CS.out.cruiseState.enabled:
+      for be in CS.out.buttonEvents:
+        if be.type in (ButtonType.accelCruise, ButtonType.resumeCruise,
+                       ButtonType.decelCruise, ButtonType.setCruise) and be.pressed:
+          self.timer = 40
+    elif self.timer:
+      self.timer -= 1
+    else:
+      return 1
+    return 0
+
+  def get_target_speed(self, v_cruise_kph_prev):
+    v_cruise_kph = v_cruise_kph_prev
+    if self.slc_state > 1:
+      v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
+      if not self.slc_active_stock:
+        v_cruise_kph = v_cruise_kph_prev
+    return v_cruise_kph
+
+  def get_button_type(self, button_type):
+    self.type_status = "type_" + str(button_type)
+    self.button_picker = getattr(self, self.type_status, lambda: "default")
+    return self.button_picker()
+
+  def reset_button(self):
+    if self.button_type != 3:
+      self.button_type = 0
+
+  def type_default(self):
+    self.button_type = 0
+    return None
+
+  def type_0(self):
+    self.button_count = 0
+    self.target_speed = self.init_speed
+    self.speed_diff = self.target_speed - self.v_set_dis
+    if self.target_speed > self.v_set_dis:
+      return Buttons.SET_PLUS
+      self.button_type = 1
+    elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min:
+      return Buttons.SET_MINUS
+      self.button_type = 2
+    return None
+
+  def type_1(self):
+    cruise_button = Buttons.SET_PLUS
+    self.button_count += 1
+    if self.target_speed <= self.v_set_dis:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_2(self):
+    cruise_button = Buttons.SET_MINUS
+    self.button_count += 1
+    if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_3(self):
+    cruise_button = None
+    self.button_count += 1
+    if self.button_count > self.t_interval:
+      self.button_type = 0
+    return cruise_button
+
+  def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev):
+    if self.v_tsc_state != 0:
+      vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH
+      if int(vision_v_cruise_kph) == int(v_cruise_kph_prev):
+        vision_v_cruise_kph = 255
+    else:
+      vision_v_cruise_kph = 255
+    if self.m_tsc_state > 1:
+      map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH
+      if int(map_v_cruise_kph) == 0.0:
+        map_v_cruise_kph = 255
+    else:
+      map_v_cruise_kph = 255
+    curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH)
+    return min(target_speed_kph, curve_speed)
+
+  def get_button_control(self, CS, final_speed, v_cruise_kph_prev):
+    self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1))
+    self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH))
+    cruise_button = self.get_button_type(self.button_type)
+    return cruise_button
+
+  def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)):
+    if cur_speed > self.steady_speed:
+      self.steady_speed = cur_speed
+    elif cur_speed < self.steady_speed - hyst:
+      self.steady_speed = cur_speed
+    return self.steady_speed
+
+  def get_cruise_buttons(self, CS, v_cruise_kph_prev):
+    cruise_button = None
+    if not self.get_cruise_buttons_status(CS):
+      pass
+    elif CS.out.cruiseState.enabled:
+      set_speed_kph = self.get_target_speed(v_cruise_kph_prev)
+      if self.slc_state > 1:
+        target_speed_kph = set_speed_kph
+      else:
+        target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
+      if self.v_tsc_state != 0 or self.m_tsc_state > 1:
+        self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
+      else:
+        self.final_speed_kph = target_speed_kph
+
+      cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
+    return cruise_button

selfdrive/car/mazda/interface.py

@@ -89,6 +89,8 @@ class CarInterface(CarInterfaceBase):
     if self.CS.low_speed_alert:
       events.add(EventName.belowSteerSpeed)
 
+    ret.customStockLong = self.update_custom_stock_long()
+
     ret.events = events.to_msg()
 
     return ret

selfdrive/car/mazda/mazdacan.py

@@ -92,20 +92,22 @@ def create_button_cmd(packer, CP, counter, button):
 
   can = int(button == Buttons.CANCEL)
   res = int(button == Buttons.RESUME)
+  inc = int(button == Buttons.SET_PLUS)
+  dec = int(button == Buttons.SET_MINUS)
 
   if CP.flags & MazdaFlags.GEN1:
     values = {
       "CAN_OFF": can,
       "CAN_OFF_INV": (can + 1) % 2,
 
-      "SET_P": 0,
-      "SET_P_INV": 1,
+      "SET_P": inc,
+      "SET_P_INV": (inc + 1) % 2,
 
       "RES": res,
       "RES_INV": (res + 1) % 2,
 
-      "SET_M": 0,
-      "SET_M_INV": 1,
+      "SET_M": dec,
+      "SET_M_INV": (dec + 1) % 2,
 
       "DISTANCE_LESS": 0,
       "DISTANCE_LESS_INV": 1,

selfdrive/car/volkswagen/carcontroller.py

@@ -1,11 +1,14 @@
 from cereal import car
+import cereal.messaging as messaging
 from opendbc.can.packer import CANPacker
 from openpilot.common.numpy_fast import clip
 from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
 from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan
 from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags
+from openpilot.selfdrive.controls.lib.drive_helpers import VOLKSWAGEN_V_CRUISE_MIN
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -25,12 +28,70 @@ class CarController(CarControllerBase):
     self.eps_timer_soft_disable_alert = False
     self.hca_frame_timer_running = 0
     self.hca_frame_same_torque = 0
+    self.last_button_frame = 0
+
+    self.sm = messaging.SubMaster(['longitudinalPlanSP'])
+    self.param_s = Params()
+    self.last_speed_limit_sign_tap_prev = False
+    self.speed_limit = 0.
+    self.speed_limit_offset = 0
+    self.timer = 0
+    self.final_speed_kph = 0
+    self.init_speed = 0
+    self.current_speed = 0
+    self.v_set_dis = 0
+    self.v_set_dis_prev = 0
+    self.v_cruise_min = 0
+    self.button_type = 0
+    self.button_select = 0
+    self.button_count = 0
+    self.target_speed = 0
+    self.t_interval = 7
+    self.slc_active_stock = False
+    self.sl_force_active_timer = 0
+    self.v_tsc_state = 0
+    self.slc_state = 0
+    self.m_tsc_state = 0
+    self.cruise_button = None
+    self.last_cruise_button = None
+    self.speed_diff = 0
+    self.v_tsc = 0
+    self.m_tsc = 0
+    self.steady_speed = 0
+    self.acc_type = -1
+    self.send_count = 0
 
   def update(self, CC, CS, now_nanos):
+    if not self.CP.pcmCruiseSpeed:
+      self.sm.update(0)
+
+      if self.sm.updated['longitudinalPlanSP']:
+        self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
+        self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
+        self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState
+        self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit
+        self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset
+        self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
+        self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed
+
+      self.v_cruise_min = VOLKSWAGEN_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
     actuators = CC.actuators
     hud_control = CC.hudControl
     can_sends = []
 
+    if not self.CP.pcmCruiseSpeed:
+      if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap:
+        self.sl_force_active_timer = self.frame
+        self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False)
+      self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap
+
+      sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
+      sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
+      sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False))
+      slc_active = not sl_inactive and not sl_temp_inactive
+
+      self.slc_active_stock = slc_active
+
     # **** Steering Controls ************************************************ #
 
     if self.frame % self.CCP.STEER_STEP == 0:
@@ -110,11 +171,155 @@ class CarController(CarControllerBase):
     if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume):
       can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values,
                                                            cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
+    if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled:
+      if not self.CP.pcmCruiseSpeed:
+        self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
+        if self.cruise_button is not None:
+          if self.acc_type == -1:
+            if self.button_count >= 2 and self.v_set_dis_prev != self.v_set_dis:
+              self.acc_type = 1 if abs(self.v_set_dis - self.v_set_dis_prev) >= 10 and self.last_cruise_button in (1, 2) else \
+                              0 if abs(self.v_set_dis - self.v_set_dis_prev) < 10 and self.last_cruise_button not in (1, 2) else 1
+            if self.send_count >= 10 and self.v_set_dis_prev == self.v_set_dis:
+              self.cruise_button = 3 if self.cruise_button == 1 else 4
+          if self.acc_type == 0:
+            self.cruise_button = 1 if self.cruise_button == 1 else 2  # accel, decel
+          elif self.acc_type == 1:
+            self.cruise_button = 3 if self.cruise_button == 1 else 4  # resume, set
+          if self.frame % self.CCP.BTN_STEP == 0:
+            can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, CS.gra_stock_values, frame=(self.frame // self.CCP.BTN_STEP),
+                                                                 buttons=self.cruise_button, custom_stock_long=True))
+            self.send_count += 1
+        else:
+          self.send_count = 0
+        self.last_cruise_button = self.cruise_button
 
     new_actuators = actuators.as_builder()
     new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX
     new_actuators.steerOutputCan = self.apply_steer_last
 
     self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"]
+    self.v_set_dis_prev = self.v_set_dis
     self.frame += 1
     return new_actuators, can_sends
+
+  # multikyd methods, sunnyhaibin logic
+  def get_cruise_buttons_status(self, CS):
+    if not CS.out.cruiseState.enabled:
+      for be in CS.out.buttonEvents:
+        if be.type in (ButtonType.accelCruise, ButtonType.resumeCruise,
+                       ButtonType.decelCruise, ButtonType.setCruise) and be.pressed:
+          self.timer = 40
+        elif be.type == ButtonType.gapAdjustCruise and be.pressed:
+          self.timer = 300
+    elif self.timer:
+      self.timer -= 1
+    else:
+      return 1
+    return 0
+
+  def get_target_speed(self, v_cruise_kph_prev):
+    v_cruise_kph = v_cruise_kph_prev
+    if self.slc_state > 1:
+      v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
+      if not self.slc_active_stock:
+        v_cruise_kph = v_cruise_kph_prev
+    return v_cruise_kph
+
+  def get_button_type(self, button_type):
+    self.type_status = "type_" + str(button_type)
+    self.button_picker = getattr(self, self.type_status, lambda: "default")
+    return self.button_picker()
+
+  def reset_button(self):
+    if self.button_type != 3:
+      self.button_type = 0
+
+  def type_default(self):
+    self.button_type = 0
+    return None
+
+  def type_0(self):
+    self.button_count = 0
+    self.target_speed = self.init_speed
+    self.speed_diff = self.target_speed - self.v_set_dis
+    if self.target_speed > self.v_set_dis:
+      self.button_type = 1
+    elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min:
+      self.button_type = 2
+    return None
+
+  def type_1(self):
+    cruise_button = 1
+    self.button_count += 1
+    if self.target_speed <= self.v_set_dis:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_2(self):
+    cruise_button = 2
+    self.button_count += 1
+    if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min:
+      self.button_count = 0
+      self.button_type = 3
+    elif self.button_count > 5:
+      self.button_count = 0
+      self.button_type = 3
+    return cruise_button
+
+  def type_3(self):
+    cruise_button = None
+    self.button_count += 1
+    if self.button_count > self.t_interval:
+      self.button_type = 0
+    return cruise_button
+
+  def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev):
+    if self.v_tsc_state != 0:
+      vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH
+      if int(vision_v_cruise_kph) == int(v_cruise_kph_prev):
+        vision_v_cruise_kph = 255
+    else:
+      vision_v_cruise_kph = 255
+    if self.m_tsc_state > 1:
+      map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH
+      if int(map_v_cruise_kph) == 0.0:
+        map_v_cruise_kph = 255
+    else:
+      map_v_cruise_kph = 255
+    curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH)
+    return min(target_speed_kph, curve_speed)
+
+  def get_button_control(self, CS, final_speed, v_cruise_kph_prev):
+    self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1))
+    self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH))
+    cruise_button = self.get_button_type(self.button_type)
+    return cruise_button
+
+  def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)):
+    if cur_speed > self.steady_speed:
+      self.steady_speed = cur_speed
+    elif cur_speed < self.steady_speed - hyst:
+      self.steady_speed = cur_speed
+    return self.steady_speed
+
+  def get_cruise_buttons(self, CS, v_cruise_kph_prev):
+    cruise_button = None
+    if not self.get_cruise_buttons_status(CS):
+      pass
+    elif CS.out.cruiseState.enabled:
+      set_speed_kph = self.get_target_speed(v_cruise_kph_prev)
+      if self.slc_state > 1:
+        target_speed_kph = set_speed_kph
+      else:
+        target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
+      if self.v_tsc_state != 0 or self.m_tsc_state > 1:
+        self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
+      else:
+        self.final_speed_kph = target_speed_kph
+
+      cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
+    return cruise_button

selfdrive/car/volkswagen/interface.py

@@ -171,6 +171,8 @@ class CarInterface(CarInterfaceBase):
     if self.CC.eps_timer_soft_disable_alert:
       events.add(EventName.steerTimeLimit)
 
+    ret.customStockLong = self.update_custom_stock_long()
+
     ret.events = events.to_msg()
 
     return ret

selfdrive/car/volkswagen/mqbcan.py

@@ -49,7 +49,7 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_p
   return packer.make_can_msg("LDW_02", bus, values)
 
 
-def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False):
+def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False):
   values = {s: gra_stock_values[s] for s in [
     "GRA_Hauptschalter",           # ACC button, on/off
     "GRA_Typ_Hauptschalter",       # ACC main button type
@@ -58,10 +58,18 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resu
     "GRA_ButtonTypeInfo",          # unknown related to stalk type
   ]}
 
+  accel_cruise = 1 if buttons == 1 else 0
+  decel_cruise = 1 if buttons == 2 else 0
+  resume_cruise = 1 if buttons == 3 else 0
+  set_cruise = 1 if buttons == 4 else 0
+
   values.update({
-    "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
+    "COUNTER": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16,
     "GRA_Abbrechen": cancel,
-    "GRA_Tip_Wiederaufnahme": resume,
+    "GRA_Tip_Wiederaufnahme": resume or resume_cruise,
+    "GRA_Tip_Setzen": set_cruise,
+    "GRA_Tip_Runter": decel_cruise,
+    "GRA_Tip_Hoch": accel_cruise,
   })
 
   return packer.make_can_msg("GRA_ACC_01", bus, values)

selfdrive/car/volkswagen/pqcan.py

@@ -31,7 +31,7 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_p
   return packer.make_can_msg("LDW_Status", bus, values)
 
 
-def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False):
+def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False):
   values = {s: gra_stock_values[s] for s in [
     "GRA_Hauptschalt",      # ACC button, on/off
     "GRA_Typ_Hauptschalt",  # ACC button, momentary vs latching
@@ -39,10 +39,18 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resu
     "GRA_Sender",           # ACC button, CAN message originator
   ]}
 
+  accel_cruise = 1 if buttons == 1 else 0
+  decel_cruise = 1 if buttons == 2 else 0
+  resume_cruise = 1 if buttons == 3 else 0
+  set_cruise = 1 if buttons == 4 else 0
+
   values.update({
-    "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
+    "COUNTER": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16,
     "GRA_Abbrechen": cancel,
-    "GRA_Recall": resume,
+    "GRA_Recall": resume or resume_cruise,
+    "GRA_Neu_Setzen": set_cruise,
+    "GRA_Down_kurz": decel_cruise,
+    "GRA_Up_kurz": accel_cruise,
   })
 
   return packer.make_can_msg("GRA_Neu", bus, values)

selfdrive/controls/radard.py

@@ -255,8 +255,8 @@ class RadarD:
     self.radar_state.radarErrors = list(radar_errors)
     self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
 
-    if len(sm['modelV2'].temporalPose.trans):
-      model_v_ego = sm['modelV2'].temporalPose.trans[0]
+    if len(sm['modelV2'].velocity.x):
+      model_v_ego = sm['modelV2'].velocity.x[0]
     else:
       model_v_ego = self.v_ego
     leads_v3 = sm['modelV2'].leadsV3

selfdrive/modeld/constants.py

@@ -76,13 +76,14 @@ class Plan:
 class Meta:
   ENGAGED = slice(0, 1)
   # next 2, 4, 6, 8, 10 seconds
-  GAS_DISENGAGE = slice(1, 36, 7)
-  BRAKE_DISENGAGE = slice(2, 36, 7)
-  STEER_OVERRIDE = slice(3, 36, 7)
-  HARD_BRAKE_3 = slice(4, 36, 7)
-  HARD_BRAKE_4 = slice(5, 36, 7)
-  HARD_BRAKE_5 = slice(6, 36, 7)
-  GAS_PRESS = slice(7, 36, 7)
+  GAS_DISENGAGE = slice(1, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
   # next 0, 2, 4, 6, 8, 10 seconds
-  LEFT_BLINKER = slice(36, 48, 2)
-  RIGHT_BLINKER = slice(37, 48, 2)
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)

selfdrive/modeld/custom_model_metadata.py

@@ -1,9 +1,10 @@
 from enum import IntFlag
-import os
+import os, sys
 
 from cereal import custom
 from openpilot.common.params import Params
 
+DRIVING_MODEL_SELECTOR_VERSION = 6
 SIMULATION = "SIMULATION" in os.environ
 
 ModelGeneration = custom.ModelGeneration
@@ -33,6 +34,10 @@ class ModelCapabilities(IntFlag):
   In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output.
   """
 
+  PlanVelocity = 2 ** 5
+
+  ModelOutputSlicesV1 = 2 ** 6
+
 
 class CustomModelMetadata:
   def __init__(self, params=None, init_only=False) -> None:
@@ -45,13 +50,19 @@ class CustomModelMetadata:
     self.capabilities: ModelCapabilities = self.get_model_capabilities()
     self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \
                        self.capabilities != ModelCapabilities.Default
+    self.initialized = False
 
   def read_model_generation_param(self) -> ModelGeneration:
     return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default)
 
   def get_model_capabilities(self) -> ModelCapabilities:
     """Returns the model capabilities for a given generation."""
-    if self.generation == ModelGeneration.five:
+    if self.generation == ModelGeneration.seven:
+      return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanVelocity | \
+             ModelCapabilities.ModelOutputSlicesV1
+    elif self.generation == ModelGeneration.six:
+      return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanVelocity
+    elif self.generation == ModelGeneration.five:
       return ModelCapabilities.DesiredCurvatureV2
     elif self.generation == ModelGeneration.four:
       return ModelCapabilities.DesiredCurvatureV2
@@ -64,3 +75,29 @@ class CustomModelMetadata:
     else:
       # Default model is meant to represent the capabilities of the prebuilt model
       return ModelCapabilities.Default
+
+  def custom_meta(self):
+    if not self.initialized:
+      if self.capabilities & ModelCapabilities.ModelOutputSlicesV1:
+        class Meta:
+          ENGAGED = slice(0, 1)
+          # next 2, 4, 6, 8, 10 seconds
+          GAS_DISENGAGE = slice(1, 41, 8)
+          BRAKE_DISENGAGE = slice(2, 41, 8)
+          STEER_OVERRIDE = slice(3, 41, 8)
+          HARD_BRAKE_3 = slice(4, 41, 8)
+          HARD_BRAKE_4 = slice(5, 41, 8)
+          HARD_BRAKE_5 = slice(6, 41, 8)
+          GAS_PRESS = slice(7, 41, 8)
+          BRAKE_PRESS = slice(8, 41, 8)
+          # next 0, 2, 4, 6, 8, 10 seconds
+          LEFT_BLINKER = slice(41, 53, 2)
+          RIGHT_BLINKER = slice(42, 53, 2)
+
+        module = 'openpilot.selfdrive.modeld.constants'
+        if module in sys.modules:
+          sys.modules[module].Meta = Meta
+        else:
+          raise ImportError(f"The module '{module}' is not imported yet or does not exist.")
+
+      self.initialized = True

selfdrive/modeld/dmonitoringmodeld.py

@@ -14,7 +14,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.realtime import set_realtime_priority
 from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
-from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid
+from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
 
 CALIB_LEN = 3
 REG_SCALE = 0.25
@@ -88,15 +88,15 @@ def fill_driver_state(msg, ds_result: DriverStateResult):
   msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
   msg.facePosition = [x * REG_SCALE for x in ds_result.face_position[:2]]
   msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
-  msg.faceProb = sigmoid(ds_result.face_prob)
-  msg.leftEyeProb = sigmoid(ds_result.left_eye_prob)
-  msg.rightEyeProb = sigmoid(ds_result.right_eye_prob)
-  msg.leftBlinkProb = sigmoid(ds_result.left_blink_prob)
-  msg.rightBlinkProb = sigmoid(ds_result.right_blink_prob)
-  msg.sunglassesProb = sigmoid(ds_result.sunglasses_prob)
-  msg.occludedProb = sigmoid(ds_result.occluded_prob)
-  msg.readyProb = [sigmoid(x) for x in ds_result.ready_prob]
-  msg.notReadyProb = [sigmoid(x) for x in ds_result.not_ready_prob]
+  msg.faceProb = float(sigmoid(ds_result.face_prob))
+  msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob))
+  msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob))
+  msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob))
+  msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob))
+  msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob))
+  msg.occludedProb = float(sigmoid(ds_result.occluded_prob))
+  msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob]
+  msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob]
 
 def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
   model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
@@ -105,8 +105,8 @@ def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts:
   ds.frameId = frame_id
   ds.modelExecutionTime = execution_time
   ds.dspExecutionTime = dsp_execution_time
-  ds.poorVisionProb = sigmoid(model_result.poor_vision_prob)
-  ds.wheelOnRightProb = sigmoid(model_result.wheel_on_right_prob)
+  ds.poorVisionProb = float(sigmoid(model_result.poor_vision_prob))
+  ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob))
   ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
   fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
   fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)

selfdrive/modeld/fill_model_msg.py

@@ -3,7 +3,7 @@ import capnp
 import numpy as np
 from cereal import log
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
-from openpilot.selfdrive.modeld.custom_model_metadata import ModelCapabilities
+from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
@@ -55,7 +55,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
                    vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
                    timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
                    nav_enabled: bool, valid: bool,
-                   custom_model_valid: bool, custom_model_capabilities: ModelCapabilities) -> None:
+                   custom_model: CustomModelMetadata) -> None:
+  custom_model.custom_meta()
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
   frame_drop_perc = frame_drop * 100
   extended_msg.valid = valid
@@ -66,9 +67,10 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   driving_model_data.frameId = vipc_frame_id
   driving_model_data.frameIdExtra = vipc_frame_id_extra
   driving_model_data.frameDropPerc = frame_drop_perc
+  driving_model_data.modelExecutionTime = model_execution_time
 
   action = driving_model_data.action
-  model_use_lateral_planner = custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution
+  model_use_lateral_planner = custom_model.valid and custom_model.capabilities & ModelCapabilities.LateralPlannerSolution
   if not model_use_lateral_planner:
     action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
 
@@ -78,7 +80,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   modelV2.frameAge = frame_age
   modelV2.frameDropPerc = frame_drop_perc
   modelV2.timestampEof = timestamp_eof
-  model_use_nav = custom_model_valid and custom_model_capabilities & ModelCapabilities.NoO
+  model_use_nav = custom_model.valid and custom_model.capabilities & ModelCapabilities.NoO
   if model_use_nav:
     modelV2.locationMonoTimeDEPRECATED = timestamp_llk
   modelV2.modelExecutionTime = model_execution_time
@@ -172,6 +174,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
   disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
   disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
+  disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
+  disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist()
 
   publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
   publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
@@ -183,10 +187,22 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
 
   # temporal pose
   temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
-  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
-  temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
-  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
+  if custom_model.valid:
+    if custom_model.capabilities & ModelCapabilities.PlanVelocity:
+      temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
+      temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
+      temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
+      temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
+    else:
+      temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
+      temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
+      temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
+      temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
+  else:
+    temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
+    temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
+    temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
+    temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
 
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:

selfdrive/modeld/modeld.py

@@ -377,7 +377,7 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
       fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
                      frame_drop_ratio, meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen,
-                     custom_model_metadata.valid, custom_model_metadata.capabilities)
+                     custom_model_metadata)
 
       if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution):
         desire_state = modelv2_send.modelV2.meta.desireState

selfdrive/modeld/models/commonmodel.cc

@@ -47,8 +47,4 @@ ModelFrame::~ModelFrame() {
   CL_CHECK(clReleaseMemObject(u_cl));
   CL_CHECK(clReleaseMemObject(y_cl));
   CL_CHECK(clReleaseCommandQueue(q));
-}
-
-float sigmoid(float input) {
-  return 1 / (1 + expf(-input));
-}
+}

selfdrive/modeld/models/commonmodel.h

@@ -16,8 +16,6 @@
 #include "selfdrive/modeld/transforms/loadyuv.h"
 #include "selfdrive/modeld/transforms/transform.h"
 
-float sigmoid(float input);
-
 class ModelFrame {
 public:
   ModelFrame(cl_device_id device_id, cl_context context);

selfdrive/modeld/models/commonmodel.pxd

@@ -12,8 +12,6 @@ cdef extern from "common/clutil.h":
   cl_context cl_create_context(cl_device_id)
 
 cdef extern from "selfdrive/modeld/models/commonmodel.h":
-  float sigmoid(float)
-
   cppclass ModelFrame:
     int buf_size
     ModelFrame(cl_device_id, cl_context)

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -8,10 +8,8 @@ from libc.string cimport memcpy
 from msgq.visionipc.visionipc cimport cl_mem
 from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
 from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
-from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame
+from .commonmodel cimport mat3, ModelFrame as cppModelFrame
 
-def sigmoid(x):
-  return cppSigmoid(x)
 
 cdef class CLContext(BaseCLContext):
   def __cinit__(self):

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:39786068cae1ed8c0dc34ef80c281dfcc67ed18a50e06b90765c49bcfdbf7db4
-size 51453312
+oid sha256:0dab94dc94db5e372f56d26f9dda98bd31fd76e4c16027da611132263848d0c1
+size 62486347

selfdrive/modeld/parse_model_outputs.py

@@ -1,15 +1,19 @@
 import numpy as np
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
+def safe_exp(x, out=None):
+  # -11 is around 10**14, more causes float16 overflow
+  return np.exp(np.clip(x, -np.inf, 11), out=out)
+
 def sigmoid(x):
-  return 1. / (1. + np.exp(-x))
+  return 1. / (1. + safe_exp(-x))
 
 def softmax(x, axis=-1):
   x -= np.max(x, axis=axis, keepdims=True)
   if x.dtype == np.float32 or x.dtype == np.float64:
-    np.exp(x, out=x)
+    safe_exp(x, out=x)
   else:
-    x = np.exp(x)
+    x = safe_exp(x)
   x /= np.sum(x, axis=axis, keepdims=True)
   return x
 
@@ -44,7 +48,7 @@ class Parser:
 
     n_values = (raw.shape[2] - out_N)//2
     pred_mu = raw[:,:,:n_values]
-    pred_std = np.exp(raw[:,:,n_values: 2*n_values])
+    pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
 
     if in_N > 1:
       weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
@@ -87,7 +91,8 @@ class Parser:
     self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
     self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    if 'sim_pose' in outs:
+      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
                    out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))

selfdrive/modeld/runners/onnxmodel.py

@@ -14,8 +14,12 @@ def attributeproto_fp16_to_fp32(attr):
   attr.data_type = 1
   attr.raw_data = float32_list.astype(np.float32).tobytes()
 
-def convert_fp16_to_fp32(path):
-  model = onnx.load(path)
+def convert_fp16_to_fp32(onnx_path_or_bytes):
+  if isinstance(onnx_path_or_bytes, bytes):
+    model = onnx.load_from_string(onnx_path_or_bytes)
+  elif isinstance(onnx_path_or_bytes, str):
+    model = onnx.load(onnx_path_or_bytes)
+
   for i in model.graph.initializer:
     if i.data_type == 10:
       attributeproto_fp16_to_fp32(i)
@@ -23,6 +27,8 @@ def convert_fp16_to_fp32(path):
     if i.type.tensor_type.elem_type == 10:
       i.type.tensor_type.elem_type = 1
   for i in model.graph.node:
+    if i.op_type == 'Cast' and i.attribute[0].i == 10:
+      i.attribute[0].i = 1
     for a in i.attribute:
       if hasattr(a, 't'):
         if a.t.data_type == 10:

selfdrive/test/process_replay/model_replay.py

@@ -130,6 +130,7 @@ if __name__ == "__main__":
       ignore = [
         'logMonoTime',
         'drivingModelData.frameDropPerc',
+        'drivingModelData.modelExecutionTime',
         'modelV2.frameDropPerc',
         'modelV2.modelExecutionTime',
         'driverStateV2.modelExecutionTime',

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-01e8432830805165527d1dbadd5f009d19430b39
+9d4d653d8cc361fe2ba53f74fd8fcfe1f1d559ed

selfdrive/test/process_replay/process_replay.py

@@ -566,7 +566,7 @@ CONFIGS = [
     proc_name="modeld",
     pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"],
     subs=["modelV2", "drivingModelData", "cameraOdometry"],
-    ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"],
+    ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"],
     should_recv_callback=ModeldCameraSyncRcvCallback(),
     tolerance=NUMPY_TOLERANCE,
     processing_time=0.020,

selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.cc

@@ -177,5 +177,18 @@ std::vector<Model> ModelsFetcher::getModelsFromURL(const QString&url) {
 }
 
 std::vector<Model> ModelsFetcher::getModelsFromURL() {
-  return getModelsFromURL("https://docs.sunnypilot.ai/models_v5.json");
+  return getModelsFromURL("https://docs.sunnypilot.ai/driving_models.json");
+}
+
+std::vector<Model> ModelsFetcher::getCompatibleModels(const QString &selector_version) {
+  std::vector<Model> allModels = getModelsFromURL();
+  std::vector<Model> compatibleModels;
+
+  for (const auto &model : allModels) {
+    if (model.isCompatible(selector_version)) {
+      compatibleModels.push_back(model);
+    }
+  }
+
+  return compatibleModels;
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.h

@@ -34,6 +34,7 @@ Last updated: July 29, 2024
 #include <QDir>
 #include <QJsonObject>
 #include <QTimer>
+#include <QVersionNumber>
 
 #include "common/swaglog.h"
 #include "common/util.h"
@@ -86,6 +87,8 @@ public:
     index = json["index"].toString();
     environment = json["environment"].toString();
     generation = json["generation"].toString();
+
+    minDrivingModelSelectorVersion = json["minimum_selector_version"].toString();
   }
 
   // Method to convert model back to QJsonObject, if needed
@@ -117,6 +120,9 @@ public:
     json["index"] = index;
     json["environment"] = environment;
     json["generation"] = generation;
+
+    json["minimum_selector_version"] = minDrivingModelSelectorVersion;
+
     return json;
   }
 
@@ -134,6 +140,13 @@ public:
   QString index;
   QString environment;
   QString generation;
+
+  QString minDrivingModelSelectorVersion;
+
+  bool isCompatible(const QString &selector_version) const {
+    return QVersionNumber::fromString(selector_version) >=
+           QVersionNumber::fromString(minDrivingModelSelectorVersion);
+  }
 };
 
 class ModelsFetcher : public QObject {
@@ -145,6 +158,7 @@ public:
   static std::vector<Model> getModelsFromURL(const QUrl&url);
   static std::vector<Model> getModelsFromURL(const QString&url);
   static std::vector<Model> getModelsFromURL();
+  static std::vector<Model> getCompatibleModels(const QString &selector_version);
 
 signals:
   void downloadProgress(double percentage);

selfdrive/ui/sunnypilot/qt/offroad/settings/software_settings.cc

@@ -210,12 +210,13 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
   checkNetwork();
   const auto currentModelName = QString::fromStdString(params.get("DrivingModelName"));
   const bool is_release_sp = params.getBool("IsReleaseSPBranch");
-  const auto models = ModelsFetcher::getModelsFromURL();
+  const QString selector_version = QString::fromStdString(params.get("DrivingModelSelectorVersion"));
+  const auto compatible_models = ModelsFetcher::getCompatibleModels(selector_version);
 
   QMap<QString, QString> index_to_model;
 
   // Collecting indices with display names
-  for (const auto &model : models) {
+  for (const auto &model : compatible_models) {
     if ((is_release_sp && model.environment == "release") || !is_release_sp) {
       index_to_model.insert(model.index, model.displayName);
     }
@@ -243,7 +244,7 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
   }
 
   // Finding and setting the selected model
-  for (auto &model : models) {
+  for (auto &model : compatible_models) {
     if (model.displayName == selectedModelName) {
       selectedModelToDownload = model;
       selectedNavModelToDownload = model;

sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py

@@ -1,127 +0,0 @@
-from abc import abstractmethod, ABC
-
-from cereal import car, custom
-from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import ACTIVE_STATES
-from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.states import InactiveState, \
-  LoadingState, AcceleratingState, DeceleratingState, HoldingState
-from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.helpers import get_set_point, \
-  speed_hysteresis, update_manual_button_timers
-
-ButtonType = car.CarState.ButtonEvent.Type
-ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState
-SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
-TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
-VisionTurnControllerState = custom.LongitudinalPlanSP.VisionTurnControllerState
-
-SendCan = tuple[int, bytes, int]
-
-
-class CustomStockLongitudinalControllerBase(ABC):
-  def __init__(self, car, car_controller, car_state, CP):
-    self.car = car
-    self.car_controller = car_controller
-    self.car_state = car_state
-    self.CP = CP
-
-    self.v_target = 0
-    self.v_cruise_cluster = 0
-    self.v_cruise_min = 0
-    self.cruise_button = None
-    self.button_state = ButtonControlState.inactive
-
-    self.v_tsc_state = VisionTurnControllerState.disabled
-    self.slc_state = SpeedLimitControlState.inactive
-    self.m_tsc_state = TurnSpeedControlState.inactive
-    self.v_tsc = 0
-    self.speed_limit_offseted = 0
-    self.m_tsc = 0
-
-    self.is_ready = False
-    self.is_ready_prev = False
-    self.speed_steady = 0
-
-    self.accel_button = None
-    self.decel_button = None
-    self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
-
-    self.button_states = {
-      ButtonControlState.inactive: InactiveState(self),
-      ButtonControlState.loading: LoadingState(self),
-      ButtonControlState.accelerating: AcceleratingState(self),
-      ButtonControlState.decelerating: DeceleratingState(self),
-      ButtonControlState.holding: HoldingState(self),
-    }
-
-  @abstractmethod
-  def create_mock_button_messages(self) -> list[SendCan]:
-    pass
-
-  def state_publish(self) -> custom.CarControlSP.CustomStockLongitudinalControl:
-    customStockLongitudinalControl = custom.CarControlSP.CustomStockLongitudinalControl.new_message()
-    customStockLongitudinalControl.state = self.button_state
-    customStockLongitudinalControl.cruiseButton = 0 if self.cruise_button is None else int(self.cruise_button)
-    customStockLongitudinalControl.vTarget = float(self.v_target)
-    customStockLongitudinalControl.vCruiseCluster = float(self.v_cruise_cluster)
-    return customStockLongitudinalControl
-
-  def update_msgs(self) -> None:
-    if self.car.sm.updated['longitudinalPlanSP']:
-      self.v_tsc_state = self.car.sm['longitudinalPlanSP'].visionTurnControllerState
-      self.slc_state = self.car.sm['longitudinalPlanSP'].speedLimitControlState
-      self.m_tsc_state = self.car.sm['longitudinalPlanSP'].turnSpeedControlState
-      self.v_tsc = self.car.sm['longitudinalPlanSP'].visionTurnSpeed
-
-      speed_limit = self.car.sm['longitudinalPlanSP'].speedLimit
-      speed_limit_offset = self.car.sm['longitudinalPlanSP'].speedLimitOffset
-      self.speed_limit_offseted = speed_limit + speed_limit_offset
-
-      self.m_tsc = self.car.sm['longitudinalPlanSP'].turnSpeed
-
-  def update_calculations(self, CS: car.CarState, CC: car.CarControl) -> None:
-    is_metric = self.car_state.params_list.is_metric
-    v_cruise_kph = CC.vCruise
-    v_cruise = v_cruise_kph * CV.KPH_TO_MS
-    self.v_cruise_min = get_set_point(is_metric)
-    self.v_cruise_cluster = round(CS.cruiseState.speed * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH))
-
-    v_targets = {'cruise': CC.vCruise}
-
-    if self.v_tsc_state != VisionTurnControllerState.disabled:
-      v_targets['v_tsc'] = self.v_tsc
-
-    if self.slc_state in ACTIVE_STATES:
-      v_targets['slc'] = self.speed_limit_offseted
-
-    if self.m_tsc_state > TurnSpeedControlState.tempInactive:
-      v_targets['m_tsc'] = self.m_tsc
-
-    source = min(v_targets, key=v_targets.get)
-
-    v_target = speed_hysteresis(self, v_targets[source], self.speed_steady, 1.5 * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS))
-    v_target = min(v_target, v_cruise)
-    v_target = round(v_target * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH))
-
-    self.v_target = v_target
-
-  def update_state(self, CS: car.CarState, CC: car.CarControl) -> None:
-    update_manual_button_timers(CS, self.cruise_buttons)
-
-    ready = CS.cruiseState.enabled and not CC.cruiseControl.cancel and not CC.cruiseControl.resume
-    button_pressed = any(self.cruise_buttons[k] > 0 for k in self.cruise_buttons)
-    self.is_ready = ready and not button_pressed
-
-    self.cruise_button = self.button_states[self.button_state]()
-
-    self.is_ready_prev = self.is_ready
-
-  def update(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]:
-    self.update_msgs()
-
-    self.update_calculations(CS, CC)
-
-    self.update_state(CS, CC)
-
-    can_sends = self.create_mock_button_messages()
-
-    return can_sends

sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py

@@ -1,30 +0,0 @@
-from cereal import car
-
-ButtonType = car.CarState.ButtonEvent.Type
-
-
-def get_set_point(is_metric: bool) -> float:
-  return 30 if is_metric else 20
-
-
-def speed_hysteresis(controller, speed: float, speed_steady: float, hyst: float) -> float:
-  if speed > speed_steady + hyst:
-    speed_steady = speed - hyst
-  elif speed < speed_steady - hyst:
-    speed_steady = speed + hyst
-
-  controller.speed_steady = speed
-
-  return speed_steady
-
-
-def update_manual_button_timers(CS: car.CarState, button_timers: dict[ButtonType, int]) -> None:
-  # increment timer for buttons still pressed
-  for k in button_timers:
-    if button_timers[k] > 0:
-      button_timers[k] += 1
-
-  for b in CS.buttonEvents:
-    if b.type.raw in button_timers:
-      # Start/end timer and store current state on change of button pressed
-      button_timers[b.type.raw] = 1 if b.pressed else 0

sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py

@@ -1,85 +0,0 @@
-from abc import ABC, abstractmethod
-from random import randint
-
-from cereal import custom
-
-ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState
-
-INACTIVE_TIMER = 40
-RESET_COUNT = 5
-HOLD_TIME = 7
-
-
-class ButtonStateBase(ABC):
-  def __init__(self, controller):
-    self.controller = controller
-    self.button_count = 0
-    self.timer = INACTIVE_TIMER
-
-  def __call__(self) -> int | None:
-    if self.controller.is_ready:
-      # TODO-SP: Validate different hold time intervals for different platforms to prevent temporary cruise fault
-      pass
-    else:
-      if self.controller.is_ready_prev:
-        self.controller.button_state = ButtonControlState.inactive
-        return None
-
-    return self.handle()
-
-  @abstractmethod
-  def handle(self) -> int | None:
-    pass
-
-
-class InactiveState(ButtonStateBase):
-  def handle(self) -> None:
-    self.button_count = 0
-
-    if not self.controller.is_ready:
-      self.timer = INACTIVE_TIMER
-    elif self.timer > 0:
-      self.timer -= 1
-    else:
-      self.controller.button_state = ButtonControlState.loading
-    return None
-
-
-class LoadingState(ButtonStateBase):
-  def handle(self) -> None:
-    if self.controller.v_target > self.controller.v_cruise_cluster:
-      self.controller.button_state = ButtonControlState.accelerating
-    elif self.controller.v_target < self.controller.v_cruise_cluster and self.controller.v_cruise_cluster > self.controller.v_cruise_min:
-      self.controller.button_state = ButtonControlState.decelerating
-    else:
-      self.controller.button_state = ButtonControlState.holding
-    return None
-
-
-class AcceleratingState(ButtonStateBase):
-  def handle(self) -> int | None:
-    self.button_count += 1
-    if self.controller.v_target <= self.controller.v_cruise_cluster or self.button_count > RESET_COUNT:
-      self.button_count = 0
-      self.controller.button_state = ButtonControlState.holding
-      return None
-    return self.controller.accel_button
-
-
-class DeceleratingState(ButtonStateBase):
-  def handle(self) -> int | None:
-    self.button_count += 1
-    if self.controller.v_target >= self.controller.v_cruise_cluster or self.controller.v_cruise_cluster <= self.controller.v_cruise_min or self.button_count > RESET_COUNT:
-      self.button_count = 0
-      self.controller.button_state = ButtonControlState.holding
-      return None
-    return self.controller.decel_button
-
-
-class HoldingState(ButtonStateBase):
-  def handle(self) -> None:
-    self.button_count += 1
-    if self.button_count > HOLD_TIME:
-      self.button_count = 0
-      self.controller.button_state = ButtonControlState.loading
-    return None

sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/controller.py

@@ -1,32 +0,0 @@
-from cereal import car
-from openpilot.selfdrive.car.chrysler.values import RAM_CARS
-from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
-  SendCan
-from openpilot.sunnypilot.selfdrive.car.chrysler.custom_stock_longitudinal_controller import helpers
-
-ButtonType = car.CarState.ButtonEvent.Type
-# TODO-SP: Handle this better in the base class
-ACCEL_BUTTON = 1
-DECEL_BUTTON = 2
-
-
-class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
-  def __init__(self, car, car_controller, car_state, CP):
-    super().__init__(car, car_controller, car_state, CP)
-    self.accel_button = ACCEL_BUTTON
-    self.decel_button = DECEL_BUTTON
-    self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.resumeCruise: 0}
-
-  def create_mock_button_messages(self) -> list[SendCan]:
-    can_sends = []
-    ram_cars = self.CP.carFingerprint in RAM_CARS
-    das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
-    button_counter_offset = [1, 1, 0, None][self.car_controller.frame % 4]
-
-    if self.cruise_button is not None:
-      if ram_cars:
-        can_sends.append(helpers.create_cruise_buttons(self.car_controller.packer, self.car_state.button_counter, das_bus, self.cruise_button))
-      elif button_counter_offset is not None:
-        can_sends.append(helpers.create_cruise_buttons(self.car_controller.packer, self.car_state.button_counter + button_counter_offset, das_bus, self.cruise_button))
-
-    return can_sends

sunnypilot/selfdrive/car/chrysler/custom_stock_longitudinal_controller/helpers.py

@@ -1,9 +0,0 @@
-def create_cruise_buttons(packer, frame, bus, buttons=0):
-
-  values = {
-    "ACC_Accel": 1 if buttons == 1 else 0,
-    "ACC_Decel": 1 if buttons == 2 else 0,
-    "COUNTER": frame % 0x10,
-  }
-
-  return packer.make_can_msg("CRUISE_BUTTONS", bus, values)

sunnypilot/selfdrive/car/honda/custom_stock_longitudinal_controller/controller.py

@@ -1,25 +0,0 @@
-from cereal import car
-from openpilot.selfdrive.car.honda import hondacan
-from openpilot.selfdrive.car.honda.values import CruiseButtons
-from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
-  SendCan
-from openpilot.sunnypilot.selfdrive.car.honda.custom_stock_longitudinal_controller import helpers
-
-ButtonType = car.CarState.ButtonEvent.Type
-
-
-class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
-  def __init__(self, car, car_controller, car_state, CP):
-    super().__init__(car, car_controller, car_state, CP)
-    self.accel_button = CruiseButtons.RES_ACCEL
-    self.decel_button = CruiseButtons.DECEL_SET
-    self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
-                           ButtonType.altButton3: 0, ButtonType.cancel: 0}
-
-  def create_mock_button_messages(self) -> list[SendCan]:
-    can_sends = []
-
-    if self.cruise_button is not None:
-      can_sends.append(hondacan.spam_buttons_command(self.car_controller.packer, self.car_controller.CAN, self.cruise_button, self.CP.carFingerprint))
-
-    return can_sends

sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py

@@ -1,58 +0,0 @@
-from random import choices
-
-from cereal import car
-from openpilot.common.numpy_fast import interp
-from openpilot.selfdrive.car.hyundai import hyundaicanfd
-from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CANFD_CAR
-from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
-  SendCan
-from openpilot.sunnypilot.selfdrive.car.hyundai.custom_stock_longitudinal_controller import helpers
-
-ButtonType = car.CarState.ButtonEvent.Type
-
-BUTTON_COPIES = 2
-BUTTON_COPIES_TIME = 7
-BUTTON_COPIES_TIME_IMPERIAL = [BUTTON_COPIES_TIME + 3, 70]
-BUTTON_COPIES_TIME_METRIC = [BUTTON_COPIES_TIME, 40]
-POPULATION = [0, 1]
-WEIGHTS = [0.51, 0.49]
-
-
-class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
-  def __init__(self, car, car_controller, car_state, CP):
-    super().__init__(car, car_controller, car_state, CP)
-    self.accel_button = Buttons.RES_ACCEL
-    self.decel_button = Buttons.SET_DECEL
-    self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0}
-
-  def create_can_mock_button_messages(self) -> list[SendCan]:
-    can_sends = []
-    if self.cruise_button is not None:
-      copies_xp = BUTTON_COPIES_TIME_METRIC if self.car_state.params_list.is_metric else BUTTON_COPIES_TIME_IMPERIAL
-      copies = int(interp(BUTTON_COPIES_TIME, copies_xp, [1, BUTTON_COPIES]))
-      can_sends.extend([helpers.create_clu11(self.car_controller.packer, self.car_state.clu11, self.cruise_button, self.CP)] * copies)
-
-    return can_sends
-
-  def create_canfd_mock_button_messages(self) -> list[SendCan]:
-    can_sends = []
-    if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
-      # TODO: resume for alt button cars
-      pass
-    else:
-      if self.cruise_button is not None:
-        for _ in range(BUTTON_COPIES):
-          can_sends.append(hyundaicanfd.create_buttons(self.car_controller.packer, self.CP, self.car_controller.CAN,
-                                                       self.car_state.buttons_counter + choices(POPULATION, WEIGHTS)[0],
-                                                       self.cruise_button))
-
-    return can_sends
-
-  def create_mock_button_messages(self) -> list[SendCan]:
-    can_sends = []
-    if self.CP.carFingerprint in CANFD_CAR:
-      can_sends.extend(self.create_canfd_mock_button_messages())
-    else:
-      can_sends.extend(self.create_can_mock_button_messages())
-
-    return can_sends

sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/helpers.py

@@ -1,22 +0,0 @@
-from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
-
-def create_clu11(packer, clu11, button, CP):
-  values = {s: clu11[s] for s in [
-    "CF_Clu_CruiseSwState",
-    "CF_Clu_CruiseSwMain",
-    "CF_Clu_SldMainSW",
-    "CF_Clu_ParityBit1",
-    "CF_Clu_VanzDecimal",
-    "CF_Clu_Vanz",
-    "CF_Clu_SPEED_UNIT",
-    "CF_Clu_DetentOut",
-    "CF_Clu_RheostatLevel",
-    "CF_Clu_CluInfo",
-    "CF_Clu_AmpInfo",
-    "CF_Clu_AliveCnt1",
-  ]}
-  values["CF_Clu_CruiseSwState"] = button
-  values["CF_Clu_AliveCnt1"] = (values["CF_Clu_AliveCnt1"] + 1) % 0x10
-  # send buttons to camera on camera-scc based cars
-  bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0
-  return packer.make_can_msg("CLU11", bus, values)

sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/controller.py

@@ -1,25 +0,0 @@
-from cereal import car
-from openpilot.selfdrive.car.mazda.values import Buttons
-from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
-  SendCan
-from openpilot.sunnypilot.selfdrive.car.mazda.custom_stock_longitudinal_controller import helpers
-
-ButtonType = car.CarState.ButtonEvent.Type
-
-
-class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
-  def __init__(self, car, car_controller, car_state, CP):
-    super().__init__(car, car_controller, car_state, CP)
-    self.accel_button = Buttons.SET_PLUS
-    self.decel_button = Buttons.SET_MINUS
-    self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
-                           ButtonType.resumeCruise: 0, ButtonType.setCruise: 0}
-
-  def create_mock_button_messages(self) -> list[SendCan]:
-    can_sends = []
-
-    if self.cruise_button is not None:
-      if self.car_controller.frame % 10 == 0:
-        can_sends.append(helpers.create_button_cmd(self.car_controller.packer, self.CP, self.car_controller.frame // 10, self.cruise_button))
-
-    return can_sends

sunnypilot/selfdrive/car/mazda/custom_stock_longitudinal_controller/helpers.py

@@ -1,40 +0,0 @@
-def create_button_cmd(packer, CP, counter, button):
-
-  can = int(button == Buttons.CANCEL)
-  res = int(button == Buttons.RESUME)
-  inc = int(button == Buttons.SET_PLUS)
-  dec = int(button == Buttons.SET_MINUS)
-
-  if CP.flags & MazdaFlags.GEN1:
-    values = {
-      "CAN_OFF": can,
-      "CAN_OFF_INV": (can + 1) % 2,
-
-      "SET_P": inc,
-      "SET_P_INV": (inc + 1) % 2,
-
-      "RES": res,
-      "RES_INV": (res + 1) % 2,
-
-      "SET_M": dec,
-      "SET_M_INV": (dec + 1) % 2,
-
-      "DISTANCE_LESS": 0,
-      "DISTANCE_LESS_INV": 1,
-
-      "DISTANCE_MORE": 0,
-      "DISTANCE_MORE_INV": 1,
-
-      "MODE_X": 0,
-      "MODE_X_INV": 1,
-
-      "MODE_Y": 0,
-      "MODE_Y_INV": 1,
-
-      "BIT1": 1,
-      "BIT2": 1,
-      "BIT3": 1,
-      "CTR": (counter + 1) % 16,
-    }
-
-    return packer.make_can_msg("CRZ_BTNS", 0, values)

sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/controller.py

@@ -1,77 +0,0 @@
-from cereal import car, custom
-from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
-  SendCan
-
-ButtonType = car.CarState.ButtonEvent.Type
-ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState
-# TODO-SP: Handle this better in the base class
-ACCEL_BUTTON = 1
-DECEL_BUTTON = 2
-RESUME_CRUISE = 3
-SET_CRUISE = 4
-
-ACC_TYPE_BUTTONS = {
-  0: {1: ACCEL_BUTTON, 2: DECEL_BUTTON},  # accel, decel
-  1: {1: RESUME_CRUISE, 2: SET_CRUISE}    # resume, set
-}
-
-
-class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
-  def __init__(self, car, car_controller, car_state, CP):
-    super().__init__(car, car_controller, car_state, CP)
-    self.accel_button = ACCEL_BUTTON
-    self.decel_button = DECEL_BUTTON
-    self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
-                           ButtonType.resumeCruise: 0, ButtonType.setCruise: 0}
-
-    self.last_v_cruise_cluster = 0
-    self.last_cruise_button = False
-
-    self.acc_type = -1
-    self.button_frame = 0
-
-    self.initial_v_target = 0
-    self.initial_v_cruise_cluster = 0
-
-  def update_checks(self) -> None:
-    if self.button_state > ButtonControlState.inactive:
-      self.button_frame += 1
-      if self.button_frame == 1:
-        self.initial_v_target = self.v_target
-        self.initial_v_cruise_cluster = self.v_cruise_cluster
-
-      self.set_acc_type()
-    else:
-      self.button_frame = 0
-      self.initial_v_target = 0
-      self.initial_v_cruise_cluster = 0
-
-  def set_acc_type(self):
-    current_v_cruise_diff = abs(self.v_target - self.v_cruise_cluster)
-    initial_v_cruise_diff = abs(self.initial_v_target - self.initial_v_cruise_cluster)
-
-    if self.last_v_cruise_cluster != self.v_cruise_cluster and current_v_cruise_diff != initial_v_cruise_diff:
-      if self.acc_type == -1:
-        v_cruise_diff = abs(self.v_cruise_cluster - self.last_v_cruise_cluster)
-
-        if v_cruise_diff >= 10 and self.last_cruise_button in (ACCEL_BUTTON, DECEL_BUTTON):
-          self.acc_type = 1
-        else:
-          self.acc_type = 0
-
-      if self.cruise_button is not None:
-        self.cruise_button = ACC_TYPE_BUTTONS.get(self.acc_type, {}).get(self.cruise_button, self.cruise_button)
-
-  def create_mock_button_messages(self) -> list[SendCan]:
-    can_sends = []
-
-    self.update_checks()
-
-    if self.cruise_button is not None:
-      if self.car_state.gra_stock_values["COUNTER"] != self.car_controller.gra_acc_counter_last:
-        can_sends.append(self.car_controller.CCS.create_acc_buttons_control(self.car_controller.packer_pt, self.car_controller.ext_bus, self.car_state.gra_stock_values, buttons=self.cruise_button))
-
-    self.last_cruise_button = self.cruise_button
-    self.last_v_cruise_cluster = self.v_cruise_cluster
-
-    return can_sends

sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/mqbcan.py

@@ -1,18 +0,0 @@
-def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0):
-  values = {s: gra_stock_values[s] for s in [
-    "GRA_Hauptschalter",           # ACC button, on/off
-    "GRA_Typ_Hauptschalter",       # ACC main button type
-    "GRA_Codierung",               # ACC button configuration/coding
-    "GRA_Tip_Stufe_2",             # unknown related to stalk type
-    "GRA_ButtonTypeInfo",          # unknown related to stalk type
-  ]}
-
-  values.update({
-    "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
-    "GRA_Tip_Wiederaufnahme": 1 if buttons == 3 else 0,
-    "GRA_Tip_Setzen": 1 if buttons == 4 else 0,
-    "GRA_Tip_Runter": 1 if buttons == 2 else 0,
-    "GRA_Tip_Hoch": 1 if buttons == 1 else 0,
-  })
-
-  return packer.make_can_msg("GRA_ACC_01", bus, values)

sunnypilot/selfdrive/car/volkswagen/custom_stock_longitudinal_controller/pqcan.py

@@ -1,17 +0,0 @@
-def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0):
-  values = {s: gra_stock_values[s] for s in [
-    "GRA_Hauptschalt",      # ACC button, on/off
-    "GRA_Typ_Hauptschalt",  # ACC button, momentary vs latching
-    "GRA_Kodierinfo",       # ACC button, configuration
-    "GRA_Sender",           # ACC button, CAN message originator
-  ]}
-
-  values.update({
-    "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
-    "GRA_Recall": 1 if buttons == 3 else 0,
-    "GRA_Neu_Setzen": 1 if buttons == 4 else 0,
-    "GRA_Down_kurz": 1 if buttons == 2 else 0,
-    "GRA_Up_kurz": 1 if buttons == 1 else 0,
-  })
-
-  return packer.make_can_msg("GRA_Neu", bus, values)

system/manager/manager.py

@@ -11,6 +11,7 @@ import openpilot.system.sentry as sentry
 from openpilot.common.api.sunnylink import UNREGISTERED_SUNNYLINK_DONGLE_ID
 from openpilot.common.params import Params, ParamKeyType
 from openpilot.common.text_window import TextWindow
+from openpilot.selfdrive.modeld.custom_model_metadata import DRIVING_MODEL_SELECTOR_VERSION
 from openpilot.system.hardware import HARDWARE, PC
 from openpilot.system.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog
 from openpilot.system.manager.mapd_installer import VERSION
@@ -64,6 +65,7 @@ def manager_init() -> None:
     ("DisableOnroadUploads", "0"),
     ("DisengageLateralOnBrake", "0"),
     ("DrivingModelGeneration", "0"),
+    ("DrivingModelSelectorVersion", str(DRIVING_MODEL_SELECTOR_VERSION)),
     ("DynamicLaneProfile", "1"),
     ("DynamicPersonality", "0"),
     ("EnableMads", "1"),