Update interface.py

# Conflicts:
#	panda
#	selfdrive/car/hyundai/carcontroller.py

cereal/custom.capnp

@@ -35,6 +35,11 @@ enum ModelGeneration {
   five @5;
 }
 
+enum MpcSource {
+  acc @0;
+  blended @1;
+}
+
 struct ControlsStateSP @0x81c2f05a394cf4af {
   lateralState @0 :Text;
   personality @8 :LongitudinalPersonalitySP;
@@ -92,8 +97,10 @@ struct LongitudinalPlanSP @0xaedffd8f31e7b55d {
   desiredTF @13 :Float32;
   notSpeedLimit @14 :Int16;
   e2eX @15 :List(Float32);
-  e2eBlended @18 :Text;
+  e2eBlendedDEPRECATED @18 :Text;
   e2eStatus @22 :Bool;
+  mpcSource @23 :MpcSource;
+  dynamicExperimentalControl @24 :Bool;
 
   distToTurn @7 :Float32;
   turnSpeed @8 :Float32;
@@ -205,7 +212,24 @@ struct ModelDataV2SP @0xf98d843bfd7004a3 {
   modelCapabilities @4 :UInt32;
 }
 
-struct CustomReserved7 @0xb86e6369214c01c8 {
+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 CustomReserved8 @0xf416ec09499d9d19 {

cereal/log.capnp

@@ -2388,7 +2388,7 @@ struct Event {
     liveMapDataSP @111 :Custom.LiveMapDataSP;
     e2eLongStateSP @112 :Custom.E2eLongStateSP;
     modelV2SP @113 :Custom.ModelDataV2SP;
-    customReserved7 @114 :Custom.CustomReserved7;
+    carControlSP @114 :Custom.CarControlSP;
     customReserved8 @115 :Custom.CustomReserved8;
     customReserved9 @116 :Custom.CustomReserved9;
 

cereal/services.py

@@ -83,6 +83,7 @@ _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

@@ -0,0 +1 @@
+../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'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'carControlSP'])
 
     self.can_rcv_cum_timeout_counter = 0
 
@@ -70,6 +70,12 @@ 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")
 
@@ -175,8 +181,16 @@ 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,14 +1,10 @@
 from cereal import car
 
-import cereal.messaging as messaging
-from common.conversions import Conversions as CV
 from opendbc.can.packer import CANPacker
-from openpilot.common.params import Params
-from openpilot.selfdrive.car import DT_CTRL, apply_meas_steer_torque_limits
+from openpilot.selfdrive.car import 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
 
@@ -26,64 +22,9 @@ 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:
@@ -114,20 +55,6 @@ 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:
@@ -179,122 +106,3 @@ 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,14 +65,9 @@ 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,8 +161,6 @@ 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/ford/interface.py

@@ -24,6 +24,7 @@ class CarInterface(CarInterfaceBase):
     ret.steerControlType = car.CarParams.SteerControlType.angle
     ret.steerActuatorDelay = 0.2
     ret.steerLimitTimer = 1.0
+    ret.customStockLongAvailable = True
 
     if Params().get("DongleId", encoding='utf8') in ("4fde83db16dc0802", "112e4d6e0cad05e1", "e36b272d5679115f", "24574459dd7fb3e0", "83a4e056c7072678"):
       ret.spFlags |= FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value

selfdrive/car/honda/carcontroller.py

@@ -1,16 +1,12 @@
 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
@@ -128,48 +124,7 @@ 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)
@@ -203,19 +158,6 @@ 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 = []
 
@@ -265,15 +207,6 @@ 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.
@@ -335,120 +268,3 @@ 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,8 +329,6 @@ 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/interface.py

@@ -296,8 +296,6 @@ 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,14 +1,9 @@
 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
@@ -21,63 +16,9 @@ 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:
@@ -102,11 +43,6 @@ 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
 
@@ -128,125 +64,3 @@ 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,8 +89,6 @@ 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,22 +92,20 @@ 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,
+      "SET_P": 0,
+      "SET_P_INV": 1,
 
       "RES": res,
       "RES_INV": (res + 1) % 2,
 
-      "SET_M": dec,
-      "SET_M_INV": (dec + 1) % 2,
+      "SET_M": 0,
+      "SET_M_INV": 1,
 
       "DISTANCE_LESS": 0,
       "DISTANCE_LESS_INV": 1,

selfdrive/car/toyota/interface.py

@@ -160,7 +160,7 @@ class CarInterface(CarInterfaceBase):
 
     sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
 
-    # hand tuned (August 12, 2024)
+    # hand tuned (August 26, 2024)
     def custom_tss2_longitudinal_tuning():
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.25
@@ -178,13 +178,8 @@ class CarInterface(CarInterfaceBase):
     tune = ret.longitudinalTuning
     if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED:
       if sp_tss2_long_tune:
-        #tune.kiBP = [0., 0.1,  3.,  12.,   20.,  26.,  36.,  50]
-        #tune.kiV = [0.34, 0.35, 0.2896, 0.2237, 0.174, 0.10, 0.08, 0.06]
-        #smooth
-        tune.kiBP = [0.,   8.,   12.,  20.,  27.,  36.,  50]
-        tune.kiV = [0.326, 0.206, 0.20, 0.17, 0.12, 0.08, 0.06]
-        #tune.kiBP = [0.,  2.,   5.,  12.,  16,   20.,  27.,  36.,  50]
-        #tune.kiV = [0.27, 0.24, 0.2205, 0.20, 0.18, 0.17, 0.12, 0.08, 0.06]
+        tune.kiBP = [0.,   3.,    8.,    12.,  20.,  27.,  36.,  50]
+        tune.kiV = [0.322, 0.244, 0.224, 0.202, 0.17, 0.12, 0.08, 0.06]
         custom_tss2_longitudinal_tuning()
       else:
         tune.kpV = [0.0]

selfdrive/car/volkswagen/carcontroller.py

@@ -1,14 +1,11 @@
 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
@@ -28,70 +25,12 @@ 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:
@@ -171,155 +110,11 @@ 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,8 +171,6 @@ 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, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False):
+def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False):
   values = {s: gra_stock_values[s] for s in [
     "GRA_Hauptschalter",           # ACC button, on/off
     "GRA_Typ_Hauptschalter",       # ACC main button type
@@ -58,18 +58,10 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0
     "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": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16,
+    "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
     "GRA_Abbrechen": cancel,
-    "GRA_Tip_Wiederaufnahme": resume or resume_cruise,
-    "GRA_Tip_Setzen": set_cruise,
-    "GRA_Tip_Runter": decel_cruise,
-    "GRA_Tip_Hoch": accel_cruise,
+    "GRA_Tip_Wiederaufnahme": resume,
   })
 
   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, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False):
+def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=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,18 +39,10 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0
     "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": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16,
+    "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
     "GRA_Abbrechen": cancel,
-    "GRA_Recall": resume or resume_cruise,
-    "GRA_Neu_Setzen": set_cruise,
-    "GRA_Down_kurz": decel_cruise,
-    "GRA_Up_kurz": accel_cruise,
+    "GRA_Recall": resume,
   })
 
   return packer.make_can_msg("GRA_Neu", bus, values)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -92,14 +92,14 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
     x_vel =  [0,    5.,   5.01,  20.,   27.7]
     y_dist = [1.0,  1.0,  1.75,  1.75,  1.83]
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    x_vel =  [0,    5.,   5.01,  20.,   27.7]
-    y_dist = [1.45, 1.45, 1.75,  1.75,  1.70]
+    x_vel =  [0,    20.,   27.7]
+    y_dist = [1.75, 1.75,  1.70]
   elif personality==custom.LongitudinalPersonalitySP.moderate:
     x_vel =  [0,    27.69, 27.7]
     y_dist = [1.45, 1.45,  1.40]
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
     x_vel =  [0,    20.0, 20.01, 27.69, 27.7]
-    y_dist = [1.0,  1.0,  1.10,  1.10,  1.20]
+    y_dist = [1.07, 1.07, 1.12,  1.12,  1.20]
   else:
     raise NotImplementedError("Dynamic personality not supported")
   return np.interp(v_ego, x_vel, y_dist)

selfdrive/controls/lib/longitudinal_planner.py

@@ -36,6 +36,7 @@ _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
 
+MpcSource = custom.MpcSource
 EventName = car.CarEvent.EventName
 
 
@@ -131,10 +132,13 @@ class LongitudinalPlanner:
       self.read_param()
     self.param_read_counter += 1
     if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
-      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
+      self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
+      self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
+      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode()
     else:
       self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
+
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
@@ -267,7 +271,8 @@ class LongitudinalPlanner:
     longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state
     longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.v_target)
 
-    longitudinalPlanSP.e2eBlended = self.mpc.mode
+    longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc
+    longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled()
 
     pm.send('longitudinalPlanSP', plan_sp_send)
 

selfdrive/controls/lib/sunnypilot/accel_controller.py

@@ -29,13 +29,13 @@ from openpilot.common.numpy_fast import interp
 AccelPersonality = custom.AccelerationPersonality
 
 # accel personality by @arne182 modified by cgw and kumar
-_DP_CRUISE_MIN_V =       [-0.031, -0.031, -0.101, -0.101, -0.39, -0.39, -0.59, -0.59, -0.79, -0.79, -1.0,  -1.0]
-_DP_CRUISE_MIN_V_ECO =   [-0.030, -0.030, -0.100, -0.100, -0.38, -0.38, -0.58, -0.58, -0.78, -0.78, -1.0,  -1.0]
-_DP_CRUISE_MIN_V_SPORT = [-0.102, -0.102, -0.102, -0.102, -0.40, -0.40, -0.60, -0.60, -0.80, -0.80, -1.0,  -1.0]
-_DP_CRUISE_MIN_BP =      [0.,     5.0,    5.01,   10.,    10.01, 14.,   14.01, 18.,   18.01,  22.,  22.01, 30.]
+_DP_CRUISE_MIN_V =       [-0.031, -0.031, -0.080, -0.080, -0.19, -0.19, -0.59, -0.59, -0.79, -0.79, -1.0,  -1.0]
+_DP_CRUISE_MIN_V_ECO =   [-0.030, -0.030, -0.075, -0.075, -0.18, -0.18, -0.58, -0.58, -0.78, -0.78, -1.0,  -1.0]
+_DP_CRUISE_MIN_V_SPORT = [-0.102, -0.102, -0.085, -0.085, -0.20, -0.20, -0.60, -0.60, -0.80, -0.80, -1.0,  -1.0]
+_DP_CRUISE_MIN_BP =      [0.,     3.0,    3.01,   10.,    10.01, 14.,   14.01, 18.,   18.01,  22.,  22.01, 30.]
 
-_DP_CRUISE_MAX_V =       [2.0, 2.0, 1.9, 1.60, 1.11, .73,  .55,  .38,  .17]
-_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 1.8, 1.35, 0.86, .53,  .43,  .32,  .09]
+_DP_CRUISE_MAX_V =       [2.0, 2.0, 2.0, 1.70, 1.11, .70,  .54,  .38,  .17]
+_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 1.8, 1.40, 0.90, .53,  .43,  .32,  .09]
 _DP_CRUISE_MAX_V_SPORT = [2.0, 2.0, 2.0, 2.00, 1.40, .90,  .70,  .50,  .30]
 _DP_CRUISE_MAX_BP =      [0.,  4.,  6.,  8.,   11.,  20.,  25.,  30.,  40.]
 

selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py

@@ -21,33 +21,39 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 #
-# Version = 2024-1-29
+# Version = 2024-8-26
 from common.numpy_fast import interp
-from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
+import numpy as np
 
-LEAD_WINDOW_SIZE = 5
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+
+LEAD_WINDOW_SIZE = 4
 LEAD_PROB = 0.6
 
 SLOW_DOWN_WINDOW_SIZE = 5
 SLOW_DOWN_PROB = 0.6
+
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
 SLOW_DOWN_DIST = [20, 30., 50., 70., 80., 90., 105., 120.]
 
-SLOWNESS_WINDOW_SIZE = 20
-SLOWNESS_PROB = 0.6
+SLOWNESS_WINDOW_SIZE = 12
+SLOWNESS_PROB = 0.5
 SLOWNESS_CRUISE_OFFSET = 1.05
 
-DANGEROUS_TTC_WINDOW_SIZE = 5
-DANGEROUS_TTC = 2.0
+DANGEROUS_TTC_WINDOW_SIZE = 3
+DANGEROUS_TTC = 2.3
 
-HIGHWAY_CRUISE_KPH = 75
+HIGHWAY_CRUISE_KPH = 70
 
 STOP_AND_GO_FRAME = 60
 
 SET_MODE_TIMEOUT = 10
 
-MPC_FCW_WINDOW_SIZE = 5
-MPC_FCW_PROB = 0.6
+MPC_FCW_WINDOW_SIZE = 10
+MPC_FCW_PROB = 0.5
+
+V_ACC_MIN = 9.72
 
 
 class SNG_State:
@@ -77,29 +83,50 @@ class GenericMovingAverageCalculator:
     self.data = []
     self.total = 0
 
+class WeightedMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.weights = np.linspace(1, 2, window_size)  # Linear weights, adjust as needed
+
+  def add_data(self, value):
+    if len(self.data) == self.window_size:
+      self.data.pop(0)
+    self.data.append(value)
+
+  def get_weighted_average(self):
+    if len(self.data) == 0:
+      return None
+    weighted_sum = np.dot(self.data, self.weights[-len(self.data):])
+    weight_total = np.sum(self.weights[-len(self.data):])
+    return weighted_sum / weight_total
+
+  def reset_data(self):
+    self.data = []
 
 class DynamicExperimentalController:
   def __init__(self):
     self._is_enabled = False
     self._mode = 'acc'
     self._mode_prev = 'acc'
+    self._mode_changed = False
     self._frame = 0
 
-    self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
+    self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
     self._has_lead_filtered = False
     self._has_lead_filtered_prev = False
 
-    self._slow_down_gmac = GenericMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
+    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
     self._has_slow_down = False
 
     self._has_blinkers = False
 
-    self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
     self._has_slowness = False
 
     self._has_nav_instruction = False
 
-    self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
     self._has_dangerous_ttc = False
 
     self._v_ego_kph = 0.
@@ -113,13 +140,46 @@ class DynamicExperimentalController:
     self._sng_transit_frame = 0
     self._sng_state = SNG_State.off
 
-    self._mpc_fcw_gmac = GenericMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
+    self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
     self._has_mpc_fcw = False
     self._mpc_fcw_crash_cnt = 0
 
     self._set_mode_timeout = 0
     pass
 
+
+  def _adaptive_slowdown_threshold(self):
+    """
+    Adapts the slow down threshold based on vehicle speed and recent behavior.
+    """
+    return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data)))
+
+  def _anomaly_detection(self, recent_data, threshold=2.0):
+    """
+    Basic anomaly detection using standard deviation.
+    """
+    if len(recent_data) < 3:
+      return False
+    mean = np.mean(recent_data)
+    std_dev = np.std(recent_data)
+    anomaly = recent_data[-1] > mean + threshold * std_dev
+    return anomaly
+
+  def _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2):
+    """
+    Smoothing the lead detection to avoid erratic behavior.
+    """
+    self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
+    return self._has_lead_filtered > LEAD_PROB
+
+  def _adaptive_lead_prob_threshold(self):
+    """
+    Adapts lead probability threshold based on driving conditions.
+    """
+    if self._v_ego_kph > HIGHWAY_CRUISE_KPH:
+      return LEAD_PROB + 0.1  # Increase the threshold on highways
+    return LEAD_PROB
+
   def _update(self, car_state, lead_one, md, controls_state, maneuver_distance):
     self._v_ego_kph = car_state.vEgo * 3.6
     self._v_cruise_kph = controls_state.vCruise
@@ -128,18 +188,27 @@ class DynamicExperimentalController:
 
     # fcw detection
     self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
-    self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() >= MPC_FCW_PROB
+    self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB
 
     # nav enable detection
     self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
 
-    # lead detection
+    # lead detection with smoothing
     self._lead_gmac.add_data(lead_one.status)
-    self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_PROB
+    self._has_lead_filtered = self._lead_gmac.get_weighted_average() > LEAD_PROB
+    #lead_prob = self._lead_gmac.get_weighted_average() or 0
+    #self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
 
-    # slow down detection
-    self._slow_down_gmac.add_data(len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST))
-    self._has_slow_down = self._slow_down_gmac.get_moving_average() >= SLOW_DOWN_PROB
+    # adaptive slow down detection
+    adaptive_threshold = self._adaptive_slowdown_threshold()
+    slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
+    self._slow_down_gmac.add_data(slow_down_trigger)
+    self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB
+
+    # anomaly detection for slow down events
+    if self._anomaly_detection(self._slow_down_gmac.data):
+      # Handle anomaly: potentially log it, adjust behavior, or issue a warning
+      self._has_slow_down = False  # Reset slow down if anomaly detected
 
     # blinker detection
     self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
@@ -161,7 +230,7 @@ class DynamicExperimentalController:
     # slowness detection
     if not self._has_standstill:
       self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
-      self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB
+      self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB
 
     # dangerous TTC detection
     if not self._has_lead_filtered and self._has_lead_filtered_prev:
@@ -171,14 +240,14 @@ class DynamicExperimentalController:
     if self._has_lead and car_state.vEgo >= 0.01:
       self._dangerous_ttc_gmac.add_data(lead_one.dRel/car_state.vEgo)
 
-    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_moving_average() is not None and self._dangerous_ttc_gmac.get_moving_average() <= DANGEROUS_TTC
+    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC
 
     # keep prev values
     self._has_standstill_prev = self._has_standstill
     self._has_lead_filtered_prev = self._has_lead_filtered
     self._frame += 1
 
-  def _blended_priority_mode(self):
+  def _radarless_mode(self):
     # when mpc fcw crash prob is high
     # use blended to slow down quickly
     if self._has_mpc_fcw:
@@ -190,21 +259,21 @@ class DynamicExperimentalController:
       self._set_mode('blended')
       return
 
-    # when blinker is on and speed is driving below highway cruise speed: blended
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
     # we dont want it to switch mode at higher speed, blended may trigger hard brake
-    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
-      self._set_mode('blended')
-      return
+    #if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
 
     # when at highway cruise and SNG: blended
     # ensuring blended mode is used because acc is bad at catching SNG lead car
     # especially those who accel very fast and then brake very hard.
-    if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
-      self._set_mode('blended')
-      return
+    #if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
 
     # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
     if self._has_standstill:
       self._set_mode('blended')
       return
@@ -226,9 +295,9 @@ class DynamicExperimentalController:
       self._set_mode('acc')
       return
 
-    self._set_mode('blended')
+    self._set_mode('acc')
 
-  def _acc_priority_mode(self):
+  def _radar_mode(self):
     # when mpc fcw crash prob is high
     # use blended to slow down quickly
     if self._has_mpc_fcw:
@@ -236,18 +305,18 @@ class DynamicExperimentalController:
       return
 
     # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
-    #if self._has_lead_filtered and not self._has_standstill:
-    #  self._set_mode('acc')
-    #  return
-
-    # when blinker is on and speed is driving below highway cruise speed: blended
-    # we dont want it to switch mode at higher speed, blended may trigger hard brake
-    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
-      self._set_mode('blended')
+    if self._has_lead_filtered and not self._has_standstill:
+      self._set_mode('acc')
       return
 
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we dont want it to switch mode at higher speed, blended may trigger hard brake
+    #if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
     # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
     if self._has_standstill:
       self._set_mode('blended')
       return
@@ -270,17 +339,22 @@ class DynamicExperimentalController:
 
     self._set_mode('acc')
 
-  def get_mpc_mode(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
+  def update(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
     if self._is_enabled:
       self._update(car_state, lead_one, md, controls_state, maneuver_distance)
       if radar_unavailable:
-        self._blended_priority_mode()
+        self._radarless_mode()
       else:
-        self._acc_priority_mode()
-
+        self._radar_mode()
+    self._mode_changed = self._mode != self._mode_prev
     self._mode_prev = self._mode
+
+  def get_mpc_mode(self):
     return self._mode
 
+  def has_changed(self):
+    return self._mode_changed
+
   def set_enabled(self, enabled):
     self._is_enabled = enabled
 
@@ -297,4 +371,4 @@ class DynamicExperimentalController:
         self._set_mode_timeout = SET_MODE_TIMEOUT
 
     if self._set_mode_timeout > 0:
-      self._set_mode_timeout -= 1
+      self._set_mode_timeout -= 1

selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc

@@ -320,8 +320,8 @@ void AnnotatedCameraWidgetSP::updateState(const UIStateSP &s) {
   // TODO: Add toggle variables to cereal, and parse from cereal
   longitudinalPersonality = s.scene.longitudinal_personality;
   dynamicLaneProfile = s.scene.dynamic_lane_profile;
-  mpcMode = QString::fromStdString(lp_sp.getE2eBlended());
-  mpcMode = (mpcMode == "blended") ? mpcMode.replace(0, 1, mpcMode[0].toUpper()) : mpcMode.toUpper();
+  const auto mpc_source = lp_sp.getMpcSource();
+  mpcSource = mpc_source == cereal::MpcSource::BLENDED ? QString(tr("blended")) : QString(tr("acc"));
 
   static int reverse_delay = 0;
   bool reverse_allowed = false;
@@ -1061,7 +1061,7 @@ void AnnotatedCameraWidgetSP::drawFeatureStatusText(QPainter &p, int x, int y) {
     p.setBrush(dec_color);
     p.drawEllipse(dec_btn);
     QString dec_status_text;
-    dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcMode).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
+    dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcSource).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
     p.setPen(QPen(shadow_color, 2));
     p.drawText(x + drop_shadow_size, y + drop_shadow_size, dec_status_text);
     p.setPen(QPen(text_color, 2));
@@ -1177,6 +1177,10 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
 
   // paint path
   QLinearGradient bg(0, height(), 0, 0);
+  const auto long_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+  bool exp_mode_path = (long_plan_sp.getDynamicExperimentalControl() && long_plan_sp.getMpcSource() == cereal::MpcSource::BLENDED) ||
+                       (!long_plan_sp.getDynamicExperimentalControl() && sm["controlsState"].getControlsState().getExperimentalMode());
+
   if (madsEnabled || car_state.getCruiseState().getEnabled()) {
     if (steerOverride && latActive) {
       bg.setColorAt(0.0, QColor::fromHslF(20 / 360., 0.94, 0.51, 0.17));
@@ -1185,7 +1189,7 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
     } else if (!(latActive || car_state.getCruiseState().getEnabled())) {
       bg.setColorAt(0, whiteColor());
       bg.setColorAt(1, whiteColor(0));
-    } else if (sm["controlsState"].getControlsState().getExperimentalMode()) {
+    } else if (exp_mode_path) {
       // The first half of track_vertices are the points for the right side of the path
       const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration().getX();
       const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration.size());

selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h

@@ -192,7 +192,7 @@ private:
   cereal::LongitudinalPlanSP::SpeedLimitControlState slcState;
   int longitudinalPersonality;
   int dynamicLaneProfile;
-  QString mpcMode;
+  QString mpcSource;
 
   int speed_limit_frame;
   bool slcShowSign = true;

sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py

@@ -0,0 +1,127 @@
+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

@@ -0,0 +1,30 @@
+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

@@ -0,0 +1,85 @@
+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

@@ -0,0 +1,32 @@
+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

@@ -0,0 +1,9 @@
+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/ford/custom_stock_longitudinal_controller/controller.py

@@ -0,0 +1,28 @@
+from cereal import car
+from openpilot.selfdrive.car.ford.values import CarControllerParams
+from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
+  SendCan
+from openpilot.sunnypilot.selfdrive.car.ford.custom_stock_longitudinal_controller import fordcan
+
+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.gapAdjustCruise: 0,
+                           ButtonType.resumeCruise: 0, ButtonType.setCruise: 0}
+
+  def create_mock_button_messages(self) -> list[SendCan]:
+    can_sends = []
+
+    if self.cruise_button is not None and (self.car_controller.frame % CarControllerParams.BUTTONS_STEP) == 0:
+      can_sends.append(fordcan.create_button_msg(self.car_controller.packer, self.car_controller.CAN.camera, self.car_state.buttons_stock_values, buttons=self.cruise_button))
+      can_sends.append(fordcan.create_button_msg(self.car_controller.packer, self.car_controller.CAN.main, self.car_state.buttons_stock_values, buttons=self.cruise_button))
+
+    return can_sends

sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/fordcan.py

@@ -0,0 +1,48 @@
+def create_button_msg(packer, bus: int, stock_values: dict, buttons=0):
+  """
+  Creates a CAN message for the Ford SCCM buttons/switches.
+
+  Includes cruise control buttons, turn lights and more.
+
+  Frequency is 10Hz.
+  """
+
+  values = {s: stock_values[s] for s in [
+    "HeadLghtHiFlash_D_Stat",  # SCCM Passthrough the remaining buttons
+    "TurnLghtSwtch_D_Stat",    # SCCM Turn signal switch
+    "WiprFront_D_Stat",
+    "LghtAmb_D_Sns",
+    "AccButtnGapDecPress",
+    "AccButtnGapIncPress",
+    "AslButtnOnOffCnclPress",
+    "AslButtnOnOffPress",
+    "LaSwtchPos_D_Stat",
+    "CcAslButtnCnclResPress",
+    "CcAslButtnDeny_B_Actl",
+    "CcAslButtnIndxDecPress",
+    "CcAslButtnIndxIncPress",
+    "CcAslButtnOffCnclPress",
+    "CcAslButtnOnOffCncl",
+    "CcAslButtnOnPress",
+    "CcAslButtnResDecPress",
+    "CcAslButtnResIncPress",
+    "CcAslButtnSetDecPress",
+    "CcAslButtnSetIncPress",
+    "CcAslButtnSetPress",
+    "CcButtnOffPress",
+    "CcButtnOnOffCnclPress",
+    "CcButtnOnOffPress",
+    "CcButtnOnPress",
+    "HeadLghtHiFlash_D_Actl",
+    "HeadLghtHiOn_B_StatAhb",
+    "AhbStat_B_Dsply",
+    "AccButtnGapTogglePress",
+    "WiprFrontSwtch_D_Stat",
+    "HeadLghtHiCtrl_D_RqAhb",
+  ]}
+
+  values.update({
+    "CcAslButtnSetIncPress": 1 if buttons == 1 else 0,      # CC increase button
+    "CcAslButtnSetDecPress": 1 if buttons == 2 else 0,      # CC decrease button
+  })
+  return packer.make_can_msg("Steering_Data_FD1", bus, values)

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

@@ -0,0 +1,25 @@
+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

@@ -0,0 +1,58 @@
+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.button_counters + 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

@@ -0,0 +1,22 @@
+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

@@ -0,0 +1,25 @@
+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

@@ -0,0 +1,40 @@
+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

@@ -0,0 +1,77 @@
+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

@@ -0,0 +1,18 @@
+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

@@ -0,0 +1,17 @@
+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)