Enhance the LFS sync script in release pipeline

This commit significantly extends the `sync-lfs.sh` script used in the release process. Rather than just switching LFS configurations and pulling/pushing, it now incorporates operations like creating a clone for branch, resetting changes, trapping exit signals and cleaning up.

This improvement enhances the reliability of synchronizing Large File Storage (LFS) objects between repositories by incorporating additional safety checks and informative echo statements. The shell script now also handles situations such as exiting in the midway more gracefully. This enhanced script is expected to provide a smoother and more reliable release process.

cereal/custom.capnp

@@ -203,24 +203,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

@@ -2388,7 +2388,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),

openpilot/sunnypilot

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

release/ci/sync-lfs.sh

@@ -1,7 +1,51 @@
 #!/usr/bin/env bash
-mv .lfsconfig .lfsconfig.bak
-mv .lfsconfig-comma .lfsconfig
-git lfs fetch --all; git lfs pull
-mv .lfsconfig .lfsconfig-comma
-mv .lfsconfig.bak .lfsconfig
-git lfs fetch --all; git lfs push --all origin
+# How to use: run from the root of the openpilot repository with the following command: ./release/ci/sync-lfs.sh
+
+# Set variables
+export their_repo="https://github.com/commaai/openpilot.git"
+export their_branch="master"
+export current_branch=$(git rev-parse --abbrev-ref HEAD)
+export our_lfs_url=${LFS_URL:-"https://gitlab.com/sunnypilot/public/sunnypilot-lfs.git/info/lfs"}
+export our_lfs_push_url=${LFS_PUSH_URL:-"ssh://git@gitlab.com/sunnypilot/public/sunnypilot-lfs.git"}
+
+echo "Syncing LFS objects from $their_repo:$their_branch to $our_lfs_url"
+echo "Pushing LFS objects to $our_lfs_push_url"
+
+# Function to reset to the current branch and perform a hard reset
+function reset_hard {
+    exit_status=$?
+    echo "Resetting to the current branch and performing a hard reset..."
+    git reset --hard
+    git switch $current_branch
+    exit $exit_status
+}
+
+# Set trap to ensure reset on exit
+trap reset_hard SIGINT SIGTERM EXIT
+
+# Add remote if it does not exist
+git remote add comma $their_repo 2>/dev/null || echo "Remote 'comma' already exists."
+
+# Fetch the specified branch from the remote
+git fetch comma $their_branch
+
+# Clean up old branch if it exists
+git branch -D ${their_branch}-upstream 2>/dev/null || echo "No old branch to clean up."
+
+# Checkout new branch based on the upstream branch
+git checkout -b ${their_branch}-upstream comma/$their_branch
+
+# Write the .lfsconfig directly
+cat <<EOL > .lfsconfig
+[lfs]
+    url = $our_lfs_url
+    pushurl = $our_lfs_push_url
+    locksverify = false
+EOL
+
+# Fetch all LFS objects and push them to the specified pushurl
+git lfs fetch --all
+git lfs push --all $our_lfs_push_url
+git restore .lfsconfig
+
+# Switch back to the current branch (handled by trap)

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)

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)