Tesla: set enableBsm flag for 3 and Y (#403 )

Set enableBsm flag for Tesla 3Y
Merge branch 'master' into SP-165-tesla-port
2026-06-20 09:12:05 +08:00 · 2024-08-13 16:18:42 -04:00 · 2024-08-03 17:41:51 -08:00 · 2024-08-03 21:35:31 -04:00 · 2024-08-02 18:54:10 -04:00 · 2024-08-02 18:52:14 -04:00
127 changed files with 763 additions and 2596 deletions
@@ -14,9 +14,6 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
  * Provides a more responsive and tailored driving experience compared to predefined settings
 * UPDATED: Driving Personality: Updated mode names
  * Aggressive, Moderate, Standard, Relaxed
-* NEW❗: Hyundai CAN: Enable Cruise Main by Default
-  * Set CRUISE MAIN to ON by default when the car starts, without engaging MADS
-  * This feature only applies when "openpilot Longitudinal Control (Alpha)" is enabled under the "Toggles" menu
 * NEW❗: Toyota - Enhanced Blind Spot Monitor (BSM) thanks to arne182, rav4kumar, and eFiniLan!
  * Enables Blind Spot Monitor (BSM) signals parsing in sunnypilot using the factory Blind Spot Monitor (BSM)
  * sunnypilot will use debugging CAN messages to receive unfiltered BSM signals, allowing detection of more objects
@@ -31,33 +28,20 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
 * UPDATED: Driving Model Selector v5
  * NEW❗: Driving Model additions
    * Notre Dame (July 01, 2024) - NDv3
-* UPDATED: Neural Network Lateral Control (NNLC)
-  * NEW❗: Remove Lateral Jerk Response (Alpha)
-  * FIXED: Hotfix for "lazy" steering performance in tighter curves thanks to twilsonco!
 * UPDATED: Toyota: Continued support for Smart DSU (SDSU) and Radar CAN Filter
  * In response to the official deprecation of support for Smart DSU (SDSU) and Radar CAN Filter in the upstream ([commaai/openpilot#32777](https://github.com/commaai/openpilot/pull/32777)), sunnypilot will continue maintaining software support for Smart DSU (SDSU) and Radar CAN Filter
 * UPDATED: Continued support for Mapbox navigation
  * In response to the official temporary deprecation of support for Mapbox navigation in the upstream ([commaai/openpilot#32773](https://github.com/commaai/openpilot/pull/32773)), sunnypilot will continue maintaining software support for Mapbox navigation
-* NEW❗: Toyota - Automatic Brake Hold (AHB) thanks to AlexandreSato!
-  * When you stop the vehicle completely by depressing the brake pedal, sunnypilot will activate Auto Brake Hold
-  * NOTE: Only for Toyota/Lexus vehicles with TSS2/LSS2
 * NEW❗: Toyota - Automatic Door Locking and Unlocking thanks to AlexandreSato, cydia2020, and dragonpilot-community!
  * Auto Lock by Speed: All doors are automatically locked when vehicle speed is approximately 6 mph (10 km/h) or higher
  * Auto Unlock by Shift to P: All doors are automatically unlocked when shifting the shift lever to P
 * FIXED: Driving Personality:
  * Maniac mode now correctly enforced when selected
-* FIXED: Experimental Model Distance Button Hold
-  * Experimental Model toggle with distance button hold no longer changes Personality
-  * Personality setting remains consistent when switching between Chill and Experimental Mode
 * UI Updates
  * Display Metrics Below Chevron
    * NEW❗: Time to Lead Car
      * Displays the time to reach the position previously occupied by the lead car
    * NEW❗: Display Distance, Speed, and Time to Lead Car simultaneously
-* Ford F-150 2022-23 support
-* Ford F-150 Lightning 2021-23 support
-* Ford Mustang Mach-E 2021-23 support
-* Hyundai Kona Electric Non-SCC 2019 support thanks to NikitaNekrasov!
 * Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL!

 sunnypilot - 0.9.7.1 (2024-06-13)
@@ -95,8 +79,6 @@ sunnypilot - 0.9.7.1 (2024-06-13)
  * Force sunnypilot in the offroad state even when the car is on
  * When Forced Offroad mode is on, allows changing offroad-only settings even when the car is turned on
  * To engage/disengage Force Offroad, go to Settings -> Device panel
-* NEW❗: Ford CAN-FD longitudinal
-  * NEW❗: Parse speed limit sign recognition from camera for certain supported platforms
 * UPDATED: Auto Lane Change Timer -> Auto Lane Change by Blinker
  * NEW❗: New "Off" option to disable lane change by blinker
 * UPDATED: Pause Lateral Below Speed with Blinker
@@ -104,8 +86,6 @@ sunnypilot - 0.9.7.1 (2024-06-13)
    * Pause lateral actuation with blinker when traveling below the desired speed selected. Default is 20 MPH or 32 km/h.
 * UPDATED: Hyundai CAN Longitudinal
  * Auto-enable radar tracks on platforms with applicable Mando radar
-* UPDATED: Hyundai CAN-FD Radar-based SCC
-  * Longitudinal support for CAN-FD Radar-based SCC cars
 * UPDATED: Hyundai CAN-FD Camera-based SCC
  * NEW❗: Parse lead info for camera-based SCC platforms with longitudinal support
    * Improve lead tracking when using openpilot longitudinal
@@ -48,7 +48,6 @@ Join the official sunnypilot Discord server to stay up to date with all the late
 To use sunnypilot in a car, you need the following:
 * A supported device to run this software
    * a [comma three](https://comma.ai/shop/products/three), or
-    * a comma two (only with older versions below 0.8.13)
 * This software
 * One of [the 250+ supported cars](https://github.com/commaai/openpilot/blob/master/docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run sunnypilot.
 * A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car
@@ -115,40 +114,12 @@ Please refer to [Recommended Branches](#-recommended-branches) to find your pref

 Requires further assistance with software installation? Join the [sunnypilot Discord server](https://discord.sunnypilot.com) and message us in the `#installation-help` channel.

-comma two
------
-
-1. [Factory reset/uninstall](https://github.com/commaai/openpilot/wiki/FAQ#how-can-i-reset-the-device) the previous software if you have another software/fork installed.
-2. After factory reset/uninstall and upon reboot, select `Custom Software` when given the option.
-3. Input the installation URL per [Recommended Branches](#-recommended-branches). Example: ```https://smiskol.com/fork/sunnyhaibin/0.8.12-4-prod```
-4. Complete the rest of the installation following the onscreen instructions.
-
-Requires further assistance with software installation? Join the [sunnypilot Discord server](https://discord.sunnypilot.com) and message us in the `#installation-help` channel.
-
-  </details>
-
-  <details>
-  <summary>SSH (More Versatile)</summary>
-  <br>
-
-Prerequisites: [How to SSH](https://github.com/commaai/openpilot/wiki/SSH)
-
-If you are looking to install sunnypilot via SSH, run the following command in an SSH terminal after connecting to your device:
-
 comma three:
 ------
 * [`release-c3`](https://github.com/sunnyhaibin/openpilot/tree/release-c3):

  ```
-  cd /data; rm -rf ./openpilot; git clone -b release-c3 --recurse-submodules https://github.com/sunnyhaibin/sunnypilot.git openpilot; cd openpilot; sudo reboot
-  ```
-
-comma two:
------
-* [`0.8.12-prod-personal-hkg`](https://github.com/sunnyhaibin/openpilot/tree/0.8.12-prod-personal-hkg):
-
-  ```
-  cd /data; rm -rf ./openpilot; git clone -b 0.8.12-prod-personal-hkg --recurse-submodules https://github.com/sunnyhaibin/sunnypilot.git openpilot; cd openpilot; sudo reboot
+  cd /data && rm -rf ./openpilot && git clone -b release-c3 --recurse-submodules https://github.com/sunnyhaibin/sunnypilot.git openpilot && cd openpilot && sudo reboot
  ```

 After running the command to install the desired branch, your comma device should reboot.
@@ -223,7 +194,7 @@ The goal of Modified Assistive Driving Safety (MADS) is to enhance the user driv
 * `SET-` button enables ACC/SCC
 * `CANCEL` button only disables ACC/SCC
 * `CRUISE (MAIN)` must be `ON` to use ACC/SCC
-* `CRUISE (MAIN)` button disables ACC/SCC completely when `OFF` **(strictly enforced in panda safety code)**
+* `CRUISE (MAIN)` button disables sunnypilot completely when `OFF` **(strictly enforced in panda safety code)**

 ### Disengage Lateral ALC on Brake Press Mode toggle
 Dedicated toggle to handle Lateral state on brake pedal press and release:
@@ -355,7 +326,7 @@ Example:

 ---

-How-To instructions can be found in [HOW-TOS.md](https://github.com/sunnyhaibin/openpilot/blob/(!)README/HOW-TOS.md).
+How-To instructions can be found in [HOW-TOS.md](HOW-TOS.md).

 </details>

@@ -137,8 +137,6 @@ struct CarEvent @0x9b1657f34caf3ad3 {
    speedLimitPreActive @139;
    speedLimitConfirmed @140;
    torqueNNLoad @141;
-    hyundaiRadarTracksAvailable @142;
-    spAutoBrakeHold @143;

    radarCanErrorDEPRECATED @15;
    communityFeatureDisallowedDEPRECATED @62;
@@ -252,7 +250,7 @@ struct CarState {
  struct CustomStockLong {
    cruiseButton @0 :Int16;
    finalSpeedKph @1 :Float32;
-    vCruiseKphPrevDEPRECATED @2 :Float32;
+    vCruiseKphPrev @2 :Float32;
    targetSpeed @3 :Float32;
    vSetDis @4 :Float32;
    speedDiff @5 :Float32;
@@ -16,7 +16,6 @@ enum LongitudinalPersonalitySP {
  moderate @1;
  standard @2;
  relaxed @3;
-  overtake @4;
 }

 enum AccelerationPersonality {
@@ -35,17 +34,11 @@ enum ModelGeneration {
  five @5;
 }

-enum MpcSource {
-  acc @0;
-  blended @1;
-}
-
 struct ControlsStateSP @0x81c2f05a394cf4af {
  lateralState @0 :Text;
  personality @8 :LongitudinalPersonalitySP;
  dynamicPersonality @9 :Bool;
  accelPersonality @10 :AccelerationPersonality;
-  overtakingAccelerationAssist @11 :Bool;

  lateralControlState :union {
    indiState @1 :LateralINDIState;
@@ -97,10 +90,8 @@ struct LongitudinalPlanSP @0xaedffd8f31e7b55d {
  desiredTF @13 :Float32;
  notSpeedLimit @14 :Int16;
  e2eX @15 :List(Float32);
-  e2eBlendedDEPRECATED @18 :Text;
+  e2eBlended @18 :Text;
  e2eStatus @22 :Bool;
-  mpcSource @23 :MpcSource;
-  dynamicExperimentalControl @24 :Bool;

  distToTurn @7 :Float32;
  turnSpeed @8 :Float32;
@@ -232,7 +232,6 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"CustomMapboxTokenSk", PERSISTENT | BACKUP},
    {"CustomOffsets", PERSISTENT | BACKUP},
    {"CustomStockLong", PERSISTENT | BACKUP},
-    {"CustomStockLongPlanner", PERSISTENT | BACKUP},
    {"CustomTorqueLateral", PERSISTENT | BACKUP},
    {"DevUIInfo", PERSISTENT | BACKUP},
    {"DisableOnroadUploads", PERSISTENT | BACKUP},
@@ -262,15 +261,9 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"HandsOnWheelMonitoring", PERSISTENT | BACKUP},
    {"HasAcceptedTermsSP", PERSISTENT},
    {"HideVEgoUi", PERSISTENT | BACKUP},
-    {"HyundaiCruiseMainDefault", PERSISTENT | BACKUP},
-    {"HkgCustomLongTuning", PERSISTENT | BACKUP},
    {"HkgSmoothStop", PERSISTENT | BACKUP},
-    {"HyundaiCruiseMainDefault", PERSISTENT | BACKUP},
    {"HotspotOnBoot", PERSISTENT},
    {"HotspotOnBootConfirmed", PERSISTENT},
-    {"HyundaiRadarTracksAvailable", PERSISTENT},
-    {"HyundaiRadarTracksAvailableCache", PERSISTENT},
-    {"HyundaiRadarTracksAvailablePersistent", PERSISTENT},
    {"LastCarModel", PERSISTENT | BACKUP},
    {"LastSpeedLimitSignTap", PERSISTENT},
    {"LastSunnylinkPingTime", CLEAR_ON_MANAGER_START},
@@ -287,7 +280,6 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"NavModelUrl", PERSISTENT | BACKUP},
    {"NNFF", PERSISTENT | BACKUP},
    {"NNFFCarModel", PERSISTENT | BACKUP},
-    {"NNFFNoLateralJerk", PERSISTENT | BACKUP},
    {"OnroadScreenOff", PERSISTENT | BACKUP},
    {"OnroadScreenOffBrightness", PERSISTENT | BACKUP},
    {"OnroadScreenOffEvent", PERSISTENT | BACKUP},
@@ -298,11 +290,8 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"OsmLocationUrl", PERSISTENT},
    {"OsmWayTest", PERSISTENT},
    {"OsmDownloadedDate", PERSISTENT},
-    {"OvertakingAccelerationAssist", PERSISTENT},
    {"PathOffset", PERSISTENT | BACKUP},
    {"PauseLateralSpeed", PERSISTENT | BACKUP},
-    {"PCMVCruiseOverride", PERSISTENT | BACKUP},
-    {"PCMVCruiseOverrideSpeed", PERSISTENT | BACKUP},
    {"QuietDrive", PERSISTENT | BACKUP},
    {"RoadEdge", PERSISTENT | BACKUP},
    {"ReverseAccChange", PERSISTENT | BACKUP},
@@ -327,13 +316,10 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"TermsVersionSunnypilot", PERSISTENT},
    {"TorqueDeadzoneDeg", PERSISTENT | BACKUP},
    {"TorqueFriction", PERSISTENT | BACKUP},
-    {"TorqueLateralJerk", PERSISTENT | BACKUP},
    {"TorqueMaxLatAccel", PERSISTENT | BACKUP},
    {"TorquedOverride", PERSISTENT | BACKUP},
-    {"ToyotaAutoHold", PERSISTENT | BACKUP},
    {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP},
    {"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP},
-    {"ToyotaDriveMode", PERSISTENT | BACKUP},
    {"ToyotaEnhancedBsm", PERSISTENT | BACKUP},
    {"ToyotaSnG", PERSISTENT | BACKUP},
    {"ToyotaTSS2Long", PERSISTENT | BACKUP},
@@ -1,4 +0,0 @@
-third_party/libyuv/x86_64/**
-third_party/snpe/x86_64/**
-third_party/snpe/x86_64-linux-clang/**
-third_party/acados/x86_64/**
@@ -1,15 +0,0 @@
-third_party/libyuv/larch64/**
-third_party/snpe/larch64**
-third_party/snpe/aarch64-ubuntu-gcc7.5/*
-third_party/acados/larch64/**
-
-system/camerad/cameras/camera_qcom2.cc
-system/camerad/cameras/camera_qcom2.h
-system/camerad/cameras/camera_util.cc
-system/camerad/cameras/camera_util.h
-system/camerad/cameras/process_raw.cl
-
-system/qcomgpsd/*
-
-selfdrive/ui/qt/spinner_larch64
-selfdrive/ui/qt/text_larch64
@@ -218,7 +218,7 @@ def create_gas_interceptor_command(packer, gas_amount, idx):
    values["GAS_COMMAND"] = gas_amount * 255.
    values["GAS_COMMAND2"] = gas_amount * 255.

-  dat = packer.make_can_msg("GAS_COMMAND", 0, values)[1]
+  dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]

  checksum = crc8_pedal(dat[:-1])
  values["CHECKSUM_PEDAL"] = checksum
@@ -60,7 +60,6 @@ class Car:
    self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake")
    self.mads_dlob = self.enable_mads and self.mads_disengage_lateral_on_brake
    self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
-    self.sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
    self.CP.alternativeExperience = 0
    if not self.disengage_on_accelerator:
      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
@@ -68,8 +67,6 @@ class Car:
      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
    elif self.mads_ndlob:
      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_DISABLE_DISENGAGE_LATERAL_ON_BRAKE
-    if self.sp_toyota_auto_brake_hold:
-      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB

    if self.CP.customStockLongAvailable and self.CP.pcmCruise and self.params.get_bool("CustomStockLong"):
      self.CP.pcmCruiseSpeed = False
@@ -98,9 +98,10 @@ class CarInterface(CarInterfaceBase):
      *create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
    ]

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+                                                            self.CS.button_events, c.vCruise,
                                                            enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.resumeCruise) if not self.CP.pcmCruiseSpeed else
                                                                           (ButtonType.accelCruise, ButtonType.decelCruise),
                                                            resume_button=(ButtonType.resumeCruise,) if not self.CP.pcmCruiseSpeed else
@@ -113,16 +114,16 @@ class CarInterface(CarInterfaceBase):
        if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
          self.CS.madsEnabled = not self.CS.madsEnabled
          self.CS.lkas_disabled = not self.CS.lkas_disabled
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False
-    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS.madsEnabled)
+    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)

    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
-        self.get_sp_cancel_cruise_state()
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -135,7 +136,7 @@ class CarInterface(CarInterfaceBase):
        self.CS.accEnabled = False
      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -145,7 +146,7 @@ class CarInterface(CarInterfaceBase):
    # events
    events = self.create_common_events(ret, c, extra_gears=[car.CarState.GearShifter.low], pcm_enable=False)

-    events, ret = self.create_sp_events(ret, events)
+    events, ret = self.create_sp_events(self.CS, ret, events)

    # Low speed steer alert hysteresis logic
    if self.CP.carFingerprint in RAM_DT:
@@ -161,7 +162,9 @@ class CarInterface(CarInterfaceBase):
    if self.low_speed_alert:
      events.add(car.CarEvent.EventName.belowSteerSpeed)

-    ret.customStockLong = self.update_custom_stock_long()
+    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+                                                           self.CC.target_speed, self.CC.v_set_dis,
+                                                           self.CC.speed_diff, self.CC.button_type)

    ret.events = events.to_msg()

@@ -276,9 +276,8 @@ MIGRATION = {
  "SUBARU FORESTER 2022": SUBARU.SUBARU_FORESTER_2022,
  "SUBARU OUTBACK 7TH GEN": SUBARU.SUBARU_OUTBACK_2023,
  "SUBARU ASCENT 2023": SUBARU.SUBARU_ASCENT_2023,
-  'TESLA AP1 MODEL S': TESLA.TESLA_AP1_MODELS,
-  'TESLA AP2 MODEL S': TESLA.TESLA_AP2_MODELS,
-  'TESLA MODEL S RAVEN': TESLA.TESLA_MODELS_RAVEN,
+  'TESLA AP3 MODEL 3': TESLA.TESLA_AP3_MODEL3,
+  'TESLA AP3 MODEL Y': TESLA.TESLA_AP3_MODELY,
  "TOYOTA ALPHARD 2020": TOYOTA.TOYOTA_ALPHARD_TSS2,
  "TOYOTA AVALON 2016": TOYOTA.TOYOTA_AVALON,
  "TOYOTA AVALON 2019": TOYOTA.TOYOTA_AVALON_2019,
@@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker
 from openpilot.common.numpy_fast import clip
 from openpilot.selfdrive.car import apply_std_steer_angle_limits
 from openpilot.selfdrive.car.ford import fordcan
-from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags, FordFlagsSP
+from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags
 from openpilot.selfdrive.car.interfaces import CarControllerBase

 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -35,9 +35,6 @@ class CarController(CarControllerBase):
    self.lkas_enabled_last = False
    self.steer_alert_last = False
    self.lead_distance_bars_last = None
-    self.path_angle = 0.
-    self.path_offset = 0.
-    self.curvature_rate = 0.

  def update(self, CC, CS, now_nanos):
    can_sends = []
@@ -77,10 +74,7 @@ class CarController(CarControllerBase):
        # TODO: extended mode
        mode = 1 if CC.latActive else 0
        counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
-        if self.CP.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value:
-          can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, self.path_offset, self.path_angle, -apply_curvature, self.curvature_rate, counter))
-        else:
-          can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
+        can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
      else:
        can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))

@@ -24,7 +24,6 @@ class CarState(CarStateBase):

    self.lkas_enabled = None
    self.prev_lkas_enabled = None
-    self.v_limit = 0

    self.button_states = {button.event_type: False for button in BUTTONS}

@@ -73,10 +72,6 @@ class CarState(CarStateBase):
    ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
    ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
    ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
-
-    if self.CP.flags & FordFlags.CANFD:
-      ret.cruiseState.speedLimit = self.update_traffic_signals(cp_cam)
-
    if not self.CP.openpilotLongitudinalControl:
      ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1

@@ -134,16 +129,6 @@ class CarState(CarStateBase):

    return ret

-  def update_traffic_signals(self, cp_cam):
-    # TODO: Check if CAN platforms have the same signals
-    if self.CP.flags & FordFlags.CANFD:
-      self.v_limit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"]
-      v_limit_unit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"]
-
-      speed_factor = CV.MPH_TO_MS if v_limit_unit == 2 else CV.KPH_TO_MS if v_limit_unit == 1 else 0
-
-      return self.v_limit * speed_factor if self.v_limit not in (0, 255) else 0
-
  @staticmethod
  def get_can_parser(CP):
    messages = [
@@ -200,11 +185,6 @@ class CarState(CarStateBase):
      ("IPMA_Data", 1),
    ]

-    if CP.flags & FordFlags.CANFD:
-      messages += [
-        ("Traffic_RecognitnData", 1),
-      ]
-
    if CP.enableBsm and CP.flags & FordFlags.CANFD:
      messages += [
        ("Side_Detect_L_Stat", 5),
@@ -3,8 +3,7 @@ from panda import Panda
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.ford.fordcan import CanBus
-from openpilot.common.params import Params
-from openpilot.selfdrive.car.ford.values import Ecu, FordFlags, FordFlagsSP
+from openpilot.selfdrive.car.ford.values import Ecu, FordFlags
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase

 ButtonType = car.CarState.ButtonEvent.Type
@@ -19,15 +18,13 @@ class CarInterface(CarInterfaceBase):
  @staticmethod
  def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
    ret.carName = "ford"
+    ret.dashcamOnly = bool(ret.flags & FordFlags.CANFD)

    ret.radarUnavailable = True
    ret.steerControlType = car.CarParams.SteerControlType.angle
    ret.steerActuatorDelay = 0.2
    ret.steerLimitTimer = 1.0

-    if Params().get("DongleId", encoding='utf8') in ("4fde83db16dc0802", "112e4d6e0cad05e1", "e36b272d5679115f", "24574459dd7fb3e0", "83a4e056c7072678"):
-      ret.spFlags |= FordFlagsSP.SP_ENHANCED_LAT_CONTROL.value
-
    CAN = CanBus(fingerprint=fingerprint)
    cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)]
    if CAN.main >= 4:
@@ -54,13 +51,6 @@ class CarInterface(CarInterfaceBase):
          if config_tja != 0xFF or config_lca != 0xFF:
            ret.dashcamOnly = True

-    if ret.spFlags & FordFlagsSP.SP_ENHANCED_LAT_CONTROL:
-      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_ENHANCED_LAT_CONTROL
-
-    ret.longitudinalTuning.kpBP = [0.]
-    ret.longitudinalTuning.kpV = [0.5]
-    ret.longitudinalTuning.kiV = [0.]
-
    # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
    found_ecus = [fw.ecu for fw in car_fw]
    if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs:
@@ -89,9 +79,10 @@ class CarInterface(CarInterfaceBase):
      *create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
    ]

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+                                                            self.CS.button_events, c.vCruise)

    if ret.cruiseState.available:
      if self.enable_mads:
@@ -99,15 +90,15 @@ class CarInterface(CarInterfaceBase):
          self.CS.madsEnabled = True
        if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
          self.CS.madsEnabled = not self.CS.madsEnabled
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False

    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
-        self.get_sp_cancel_cruise_state()
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -115,7 +106,7 @@ class CarInterface(CarInterfaceBase):
        self.CS.accEnabled = False
      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -124,7 +115,7 @@ class CarInterface(CarInterfaceBase):

    events = self.create_common_events(ret, c, extra_gears=[GearShifter.manumatic], pcm_enable=False)

-    events, ret = self.create_sp_events(ret, events)
+    events, ret = self.create_sp_events(self.CS, ret, events)

    if not self.CS.vehicle_sensors_valid:
      events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
@@ -11,7 +11,6 @@ DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
 DELPHI_MRR_RADAR_START_ADDR = 0x120
 DELPHI_MRR_RADAR_MSG_COUNT = 64

-STEER_ASSIST_DATA_MSGS = 0x3d7

 def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
  msg_n = len(DELPHI_ESR_RADAR_MSGS)
@@ -29,9 +28,6 @@ def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:

  return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)

-def _create_steer_assist_data(CP) -> CANParser:
-  messages = [("Steer_Assist_Data", 20)]
-  return CANParser(RADAR.STEER_ASSIST_DATA, messages, CanBus(CP).camera)

 class RadarInterface(RadarInterfaceBase):
  def __init__(self, CP):
@@ -49,10 +45,6 @@ class RadarInterface(RadarInterfaceBase):
    elif self.radar == RADAR.DELPHI_MRR:
      self.rcp = _create_delphi_mrr_radar_can_parser(CP)
      self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1
-    elif self.radar == RADAR.STEER_ASSIST_DATA:
-      self.rcp = _create_steer_assist_data(CP)
-      self.trigger_msg = STEER_ASSIST_DATA_MSGS
-
    else:
      raise ValueError(f"Unsupported radar: {self.radar}")

@@ -76,67 +68,11 @@ class RadarInterface(RadarInterfaceBase):
      self._update_delphi_esr()
    elif self.radar == RADAR.DELPHI_MRR:
      self._update_delphi_mrr()
-    elif self.radar == RADAR.STEER_ASSIST_DATA:
-      self._update_steer_assist_data()

    ret.points = list(self.pts.values())
    self.updated_messages.clear()
    return ret

-  def _update_steer_assist_data(self):
-    msg = self.rcp.vl["Steer_Assist_Data"]
-    updated_msg = self.updated_messages
-
-    dRel = msg['CmbbObjDistLong_L_Actl']
-    confidence = msg['CmbbObjConfdnc_D_Stat']
-    new_track = False
-
-    # if dRel < 1022:
-    if confidence > 0:
-      if 0 not in self.pts:
-        self.pts[0] = car.RadarData.RadarPoint.new_message()
-        self.pts[0].trackId = self.track_id
-        self.vRelCol[0] = collections.deque(maxlen=20)
-        self.track_id += 1
-        new_track = True
-
-      yRel = msg['CmbbObjDistLat_L_Actl']
-      vRel = msg['CmbbObjRelLong_V_Actl']
-      yvRel = msg['CmbbObjRelLat_V_Actl']
-      calc = 0
-      if not new_track:
-        # if this is a newly created track - we don't have historical data so skip it
-        # if we are on the same track
-        # Let's see if we are moving:
-        #   positive gap - lead is moving faster than us
-        #   negative gap - lead is moving slower than us
-        dDiff = dRel - self.pts[0].dRel
-        if (abs(vRel) < 1.0e-2):
-          self.vRelCol[0].append(dDiff)
-          vRel = sum(self.vRelCol[0])
-          calc = 1
-        else:
-          if len(self.vRelCol[0]) > 0:
-            self.vRelCol[0].clear()
-
-        if abs(self.pts[0].vRel - vRel) > 2 or abs(self.pts[0].dRel - dRel) > 5:
-          self.pts[0].trackId = self.track_id
-          if len(self.vRelCol[0]) > 0:
-            self.vRelCol[0].clear()
-          self.track_id += 1
-
-      self.pts[0].dRel = dRel  # from front of car
-      self.pts[0].yRel = yRel  # in car frame's y axis, left is positive
-      self.pts[0].vRel = vRel
-      self.pts[0].aRel = float('nan')
-      self.pts[0].yvRel = yvRel
-      self.pts[0].measured = True
-    else:
-      if 0 in self.pts:
-        del self.pts[0]
-        del self.vRelCol[0]
-
-
  def _update_delphi_esr(self):
    for ii in sorted(self.updated_messages):
      cpt = self.rcp.vl[ii]
@@ -48,14 +48,9 @@ class FordFlags(IntFlag):
  CANFD = 1


-class FordFlagsSP(IntFlag):
-  SP_ENHANCED_LAT_CONTROL = 1
-
-
 class RADAR:
  DELPHI_ESR = 'ford_fusion_2018_adas'
  DELPHI_MRR = 'FORD_CADS'
-  STEER_ASSIST_DATA = 'ford_lincoln_base_pt'


 class Footnote(Enum):
@@ -96,7 +91,7 @@ class FordPlatformConfig(PlatformConfig):

@dataclass
 class FordCANFDPlatformConfig(FordPlatformConfig):
-  dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', RADAR.STEER_ASSIST_DATA))
+  dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', None))

  def init(self):
    super().init()
@@ -1,6 +1,4 @@
 from cereal import car
-import cereal.messaging as messaging
-from openpilot.common.params import Params
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import interp
 from opendbc.can.packer import CANPacker
@@ -8,7 +6,6 @@ from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits
 from openpilot.selfdrive.car.gm import gmcan
 from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons
 from openpilot.selfdrive.car.interfaces import CarControllerBase
-from selfdrive.controls.lib.drive_helpers import GM_V_CRUISE_MIN

 VisualAlert = car.CarControl.HUDControl.VisualAlert
 NetworkLocation = car.CarParams.NetworkLocation
@@ -40,36 +37,6 @@ class CarController(CarControllerBase):
    self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
    self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis'])

-    self.sm = messaging.SubMaster(['longitudinalPlanSP'])
-    self.param_s = Params()
-    self.is_metric = self.param_s.get_bool("IsMetric")
-    self.speed_limit_control_enabled = False
-    self.last_speed_limit_sign_tap = False
-    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):
    actuators = CC.actuators
    hud_control = CC.hudControl
@@ -78,40 +45,9 @@ class CarController(CarControllerBase):
    if hud_v_cruise > 70:
      hud_v_cruise = 0

-    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
-
-      if self.frame % 200 == 0:
-        self.speed_limit_control_enabled = self.param_s.get_bool("EnableSlc")
-        self.is_metric = self.param_s.get_bool("IsMetric")
-      self.last_speed_limit_sign_tap = self.param_s.get_bool("LastSpeedLimitSignTap")
-      self.v_cruise_min = GM_V_CRUISE_MIN[self.is_metric] * (CV.KPH_TO_MPH if not self.is_metric else 1)
-
    # Send CAN commands.
    can_sends = []

-    if not self.CP.pcmCruiseSpeed:
-      if not self.last_speed_limit_sign_tap_prev and self.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 = self.last_speed_limit_sign_tap
-
-      sl_force_active = self.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
-      sl_inactive = not sl_force_active and (not self.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
-      sl_temp_inactive = not sl_force_active and (self.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 (Active: 50Hz, inactive: 10Hz)
    steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP

@@ -212,15 +148,6 @@ class CarController(CarControllerBase):
          self.last_button_frame = self.frame
          can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))

-      if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and not self.CP.pcmCruiseSpeed and CS.out.cruiseState.enabled:
-        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 % 12 < 6:  # thanks to mochi86420 for the magic numbers
-            can_sends.extend([gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, (CS.buttons_counter + 2) % 4, self.cruise_button)] * 3)
-
    if self.CP.networkLocation == NetworkLocation.fwdCamera:
      # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
      if self.frame % 10 == 0:
@@ -234,120 +161,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 != CruiseButtons.UNPRESS:
-      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 = CruiseButtons.UNPRESS
-    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 self.is_metric else 1))
-    self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not self.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
@@ -119,7 +119,6 @@ class CarInterface(CarInterfaceBase):
        ret.pcmCruise = False
        ret.openpilotLongitudinalControl = True
        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
-      ret.customStockLongAvailable = True

    else:  # ASCM, OBD-II harness
      ret.openpilotLongitudinalControl = True
@@ -205,6 +204,8 @@ class CarInterface(CarInterfaceBase):
  def _update(self, c):
    ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)

+    distance_button = 0
+
    # Don't add event if transitioning from INIT, unless it's to an actual button
    if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
      self.CS.button_events = [
@@ -213,19 +214,21 @@ class CarInterface(CarInterfaceBase):
        *create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
                              {1: ButtonType.gapAdjustCruise})
      ]
+      distance_button = self.CS.distance_button

    self.CS.button_events = [
      *self.CS.button_events,
      *create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
    ]

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

    if not self.CP.pcmCruise:
      if any(b.type == ButtonType.accelCruise and b.pressed for b in self.CS.button_events):
        self.CS.accEnabled = True

-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+                                                            self.CS.button_events, c.vCruise)

    if ret.cruiseState.available:
      if self.enable_mads:
@@ -233,15 +236,15 @@ class CarInterface(CarInterfaceBase):
          self.CS.madsEnabled = True
        if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
          self.CS.madsEnabled = not self.CS.madsEnabled
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False

-    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
+    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
-        self.get_sp_cancel_cruise_state()
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -249,7 +252,7 @@ class CarInterface(CarInterfaceBase):
        self.CS.accEnabled = False
      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -264,7 +267,7 @@ class CarInterface(CarInterfaceBase):
    #  if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
    #    events.add(EventName.buttonEnable)

-    events, ret = self.create_sp_events(ret, events, enable_pressed=self.CS.accEnabled,
+    events, ret = self.create_sp_events(self.CS, ret, events, enable_pressed=self.CS.accEnabled,
                                        enable_buttons=(ButtonType.decelCruise,))

    # Enabling at a standstill with brake is allowed
@@ -278,8 +281,6 @@ class CarInterface(CarInterfaceBase):
    if ret.vEgo < self.CP.minSteerSpeed and self.CS.madsEnabled:
      events.add(EventName.belowSteerSpeed)

-    ret.customStockLong = self.update_custom_stock_long()
-
    ret.events = events.to_msg()

    return ret
@@ -267,9 +267,10 @@ class CarInterface(CarInterfaceBase):
      *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT),
    ]

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+                                                            self.CS.button_events, c.vCruise)

    if ret.cruiseState.available:
      if self.enable_mads:
@@ -277,7 +278,7 @@ class CarInterface(CarInterfaceBase):
          self.CS.madsEnabled = True
        if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
          self.CS.madsEnabled = not self.CS.madsEnabled
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False

@@ -285,9 +286,9 @@ class CarInterface(CarInterfaceBase):

    if not self.CP.pcmCruise or min_enable_speed_pcm or not self.CP.pcmCruiseSpeed:
      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
-        self.get_sp_cancel_cruise_state()
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads or min_enable_speed_pcm \
                                                        else False if self.CP.pcmCruise \
                                                        else self.CS.accEnabled
@@ -300,7 +301,7 @@ class CarInterface(CarInterfaceBase):
      elif not ret.cruiseState.enabled:
        self.CS.accEnabled = False

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_setting == 3))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -312,7 +313,7 @@ class CarInterface(CarInterfaceBase):
    if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed and not self.CS.madsEnabled:
      events.add(EventName.belowEngageSpeed)

-    events, ret = self.create_sp_events(ret, events)
+    events, ret = self.create_sp_events(self.CS, ret, events)

    #if self.CP.pcmCruise:
    #  # we engage when pcm is active (rising edge)
@@ -329,7 +330,9 @@ 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.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+                                                           self.CC.target_speed, self.CC.v_set_dis,
+                                                           self.CC.speed_diff, self.CC.button_type)

    ret.events = events.to_msg()

@@ -1,7 +1,7 @@
 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.numpy_fast import clip
 from openpilot.common.params import Params
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg
@@ -62,7 +62,7 @@ class CarController(CarControllerBase):
    self.lat_disengage_init = False
    self.lat_active_last = False

-    sub_services = ['longitudinalPlan', 'longitudinalPlanSP']
+    sub_services = ['longitudinalPlanSP']
    if CP.openpilotLongitudinalControl:
      sub_services.append('radarState')
    # TODO: Always true, prep for future conditional refactoring
@@ -94,24 +94,9 @@ class CarController(CarControllerBase):
    self.v_tsc = 0
    self.m_tsc = 0
    self.steady_speed = 0
-    self.speeds = 0
-    self.v_target_plan = 0
-    self.custom_stock_planner_speed = self.param_s.get_bool("CustomStockLongPlanner")
+    self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
    self.lead_distance = 0

-    self.jerk = 0.0
-    self.jerk_l = 0.0
-    self.jerk_u = 0.0
-    self.jerkStartLimit = 2.0
-    self.cb_upper = 0.0
-    self.cb_lower = 0.0
-    self.jerk_count = 0.0
-
-    self.accel_raw = 0
-    self.accel_val = 0
-    self.accel_last_jerk = 0
-    self.hkg_custom_long_tuning = self.param_s.get_bool("HkgCustomLongTuning")
-
  def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
    lead_one = self.sm["radarState"].leadOne
    lead_two = self.sm["radarState"].leadTwo
@@ -128,10 +113,6 @@ class CarController(CarControllerBase):
      self.sm.update(0)

    if not self.CP.pcmCruiseSpeed:
-      if self.sm.updated['longitudinalPlan']:
-        _speeds = self.sm['longitudinalPlan'].speeds
-        self.speeds = _speeds[-1] if len(_speeds) else 0
-
      if self.sm.updated['longitudinalPlanSP']:
        self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
        self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
@@ -141,21 +122,14 @@ class CarController(CarControllerBase):
        self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
        self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed

-      if self.frame % 200 == 0:
-        self.custom_stock_planner_speed = self.param_s.get_bool("CustomStockLongPlanner")
      self.v_cruise_min = HYUNDAI_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
-      self.v_target_plan = min(CC.vCruise * CV.KPH_TO_MS, self.speeds)

    actuators = CC.actuators
    hud_control = CC.hudControl

    # steering torque
-    if self.CP.spFlags & HyundaiFlagsSP.SP_UPSTREAM_TACO.value:
-      self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
    new_steer = int(round(actuators.steer * self.params.STEER_MAX))
    apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
-    if self.CP.spFlags & HyundaiFlagsSP.SP_UPSTREAM_TACO.value:
-      apply_steer = clip(apply_steer, -self.params.STEER_MAX, self.params.STEER_MAX)

    # >90 degree steering fault prevention
    self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
@@ -216,7 +190,7 @@ class CarController(CarControllerBase):
    if self.frame % 100 == 0 and not ((self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or escc) and \
      self.CP.carFingerprint not in CAMERA_SCC_CAR and self.CP.openpilotLongitudinalControl:
      # for longitudinal control, either radar or ADAS driving ECU
-      addr, bus = 0x7d0, self.CAN.ECAN if self.CP.carFingerprint in CANFD_CAR else 0
+      addr, bus = 0x7d0, 0
      if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
        addr, bus = 0x730, self.CAN.ECAN
      can_sends.append(make_tester_present_msg(addr, bus, suppress_response=True))
@@ -225,9 +199,6 @@ class CarController(CarControllerBase):
      if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
        can_sends.append(make_tester_present_msg(0x7b1, self.CAN.ECAN, suppress_response=True))

-    if self.CP.openpilotLongitudinalControl:
-      self.make_jerk(CS, accel, actuators)
-
    # CAN-FD platforms
    if self.CP.carFingerprint in CANFD_CAR:
      hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
@@ -254,11 +225,9 @@ class CarController(CarControllerBase):
      if self.CP.openpilotLongitudinalControl:
        if hda2:
          can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
-        else:
-          can_sends.extend(hyundaicanfd.create_fca_warning_light(self.packer, self.CAN, self.frame))
        if self.frame % 2 == 0:
          can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
-                                                           set_speed_in_units, hud_control, self.jerk_u, self.jerk_l))
+                                                           set_speed_in_units, hud_control))
          self.accel_last = accel
      else:
        # button presses
@@ -302,13 +271,15 @@ class CarController(CarControllerBase):
        self.lead_distance = self.calculate_lead_distance(hud_control)

      if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
+        if self.hkg_can_smooth_stop:
+          stopping = stopping and CS.out.vEgoRaw < 0.05
+
        # TODO: unclear if this is needed
        jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
        use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
-        self.make_accel(CS, actuators)
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
                                                        hud_control, set_speed_in_units, stopping,
-                                                        CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
+                                                        CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance))

      # 20 Hz LFA MFA message
      if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
@@ -475,8 +446,6 @@ class CarController(CarControllerBase):
        target_speed_kph = set_speed_kph
      else:
        target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
-      if self.custom_stock_planner_speed:
-        target_speed_kph = self.curve_speed_hysteresis(self.v_target_plan * CV.MS_TO_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:
@@ -484,66 +453,3 @@ class CarController(CarControllerBase):

      cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
    return cruise_button
-
-  # jerk calculations thanks to apilot!
-  def cal_jerk(self, accel, actuators):
-    self.accel_raw = accel
-    if actuators.longControlState == LongCtrlState.off:
-      accel_diff = 0.0
-    elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0:
-      accel_diff = 0.0
-    else:
-      accel_diff = self.accel_raw - self.accel_last_jerk
-
-    accel_diff /= DT_CTRL
-    self.jerk = self.jerk * 0.9 + accel_diff * 0.1
-    return self.jerk
-
-  def make_jerk(self, CS, accel, actuators):
-    jerk = self.cal_jerk(accel, actuators)
-    a_error = accel - CS.out.aEgo
-    jerk = jerk + (a_error * 2.0)
-
-    if not self.hkg_custom_long_tuning:
-      self.jerk_u = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
-      self.jerk_l = 5.0
-    elif True: #self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022:
-      startingJerk = 0.5
-      jerkLimit = 5.0
-      self.jerk_count += DT_CTRL
-      jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit])
-      if actuators.longControlState == LongCtrlState.off:
-        self.jerk_u = jerkLimit
-        self.jerk_l = jerkLimit
-        self.jerk_count = 0
-      else:
-        self.jerk_u = min(max(0.5, jerk * 2.0), jerk_max)
-        self.jerk_l = min(max(1.0, -jerk * 3.0), jerkLimit)
-    else:
-      startingJerk = self.jerkStartLimit
-      jerkLimit = 5.0
-      self.jerk_count += DT_CTRL
-      jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit])
-      self.cb_upper = self.cb_lower = 0
-      if actuators.longControlState == LongCtrlState.off:
-        self.jerk_u = jerkLimit
-        self.jerk_l = jerkLimit
-        self.jerk_count = 0
-      else:
-        self.jerk_u = min(max(0.5, jerk * 2.0), jerk_max)
-        self.jerk_l = min(max(0.5, -jerk * 2.0), jerkLimit)
-        self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2)
-        self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
-
-  def make_accel(self, CS, actuators):
-    long_control = actuators.longControlState
-    is_ice = not self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV)
-    rate_up = 0.1
-    rate_down = 0.1
-    if long_control == LongCtrlState.off or (long_control == LongCtrlState.stopping and CS.out.standstill):
-      self.accel_raw, self.accel_val = 0, 0
-    else:
-      #self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
-      self.accel_val = self.accel_raw
-    self.accel_last = self.accel_val
-    self.accel_last_jerk = self.accel_val
@@ -1171,17 +1171,6 @@ FW_VERSIONS = {
      b'\xf1\x006T6J0_C2\x00\x006T6K1051\x00\x00TOS4N20NS2\x00\x00\x00\x00',
    ],
  },
-  CAR.HYUNDAI_KONA_EV_NON_SCC: {
-    (Ecu.abs, 0x7d1, None): [
-      b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000',
-    ],
-    (Ecu.eps, 0x7d4, None): [
-      b'\xf1\x00OS  MDPS C 1.00 1.04 56310K4000\x00 4OEDC104',
-    ],
-    (Ecu.fwdCamera, 0x7c4, None): [
-      b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4100 W40',
-    ],
-  },
  CAR.KIA_CEED_PHEV_2022_NON_SCC: {
    (Ecu.eps, 0x7D4, None): [
      b'\xf1\x00CD  MDPS C 1.00 1.01 56310-XX000 4CPHC101',
@@ -130,8 +130,8 @@ def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon
  }
  return packer.make_can_msg("LFAHDA_MFC", 0, values)

-def create_acc_commands(packer, enabled, accel_raw, accel_val, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
-                        CS, escc, CP, lead_distance, cb_lower, cb_upper):
+def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
+                        CS, escc, CP, lead_distance):
  commands = []

  scc11_values = {
@@ -150,8 +150,8 @@ def create_acc_commands(packer, enabled, accel_raw, accel_val, lower_jerk, upper
  scc12_values = {
    "ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
    "StopReq": 1 if stopping else 0,
-    "aReqRaw": accel_raw,
-    "aReqValue": accel_val,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
+    "aReqRaw": accel,
+    "aReqValue": accel,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
    "CR_VSM_Alive": idx % 0xF,
  }

@@ -172,10 +172,10 @@ def create_acc_commands(packer, enabled, accel_raw, accel_val, lower_jerk, upper
  commands.append(packer.make_can_msg("SCC12", 0, scc12_values))

  scc14_values = {
-    "ComfortBandUpper": cb_upper, # stock usually is 0 but sometimes uses higher values
-    "ComfortBandLower": cb_lower, # stock usually is 0 but sometimes uses higher values
+    "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
+    "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
    "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
-    "JerkLowerLimit": lower_jerk, # stock usually is 0.5 but sometimes uses higher values
+    "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
    "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
    "ObjGap": get_object_gap(lead_distance), # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
  }
@@ -207,7 +207,7 @@ def create_acc_opt(packer, escc, CS, CP):
  commands = []

  scc13_values = {
-    "SCCDrvModeRValue": 3,
+    "SCCDrvModeRValue": 2,
    "SCC_Equip": 1,
    "Lead_Veh_Dep_Alert_USM": 2,
  }
@@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active, lateral_paused, blin
  return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)


-def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, upper_jerk, lower_jerk):
+def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
  jerk = 5
  jn = jerk / 50
  if not enabled or gas_override:
@@ -137,8 +137,8 @@ def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, ga
    "aReqValue": a_val,
    "aReqRaw": a_raw,
    "VSetDis": set_speed,
-    "JerkLowerLimit": lower_jerk,
-    "JerkUpperLimit": upper_jerk,
+    "JerkLowerLimit": jerk if enabled else 1,
+    "JerkUpperLimit": 3.0,

    "ACC_ObjDist": 1,
    "ObjValid": 0,
@@ -172,20 +172,6 @@ def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
  return ret


-def create_fca_warning_light(packer, CAN, frame):
-  ret = []
-  if frame % 2 == 0:
-    values = {
-      'AEB_SETTING': 0x1,  # show AEB disabled icon
-      'SET_ME_2': 0x2,
-      'SET_ME_FF': 0xff,
-      'SET_ME_FC': 0xfc,
-      'SET_ME_9': 0x9,
-    }
-    ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
-  return ret
-
-
 def create_adrv_messages(packer, CAN, frame):
  # messages needed to car happy after disabling
  # the ADAS Driving ECU to do longitudinal control
@@ -196,7 +182,15 @@ def create_adrv_messages(packer, CAN, frame):
  }
  ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))

-  ret.extend(create_fca_warning_light(packer, CAN, frame))
+  if frame % 2 == 0:
+    values = {
+      'AEB_SETTING': 0x1,  # show AEB disabled icon
+      'SET_ME_2': 0x2,
+      'SET_ME_FF': 0xff,
+      'SET_ME_FC': 0xfc,
+      'SET_ME_9': 0x9,
+    }
+    ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))

  if frame % 5 == 0:
    values = {
@@ -1,8 +1,6 @@
-import cereal.messaging as messaging
 from cereal import car
 from panda import Panda
 from openpilot.common.params import Params
-from openpilot.selfdrive.car.sunnypilot.fingerprinting import can_fingerprint, get_one_can
 from openpilot.selfdrive.car.hyundai.enable_radar_tracks import enable_radar_tracks
 from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
 from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
@@ -93,7 +91,7 @@ class CarInterface(CarInterfaceBase):

    # *** longitudinal control ***
    if candidate in CANFD_CAR:
-      ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | NON_SCC_CAR)
+      ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR | NON_SCC_CAR)
      if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC and not hda2:
        ret.spFlags |= HyundaiFlagsSP.SP_CAMERA_SCC_LEAD.value
    else:
@@ -104,20 +102,13 @@ class CarInterface(CarInterfaceBase):
    ret.pcmCruise = not ret.openpilotLongitudinalControl

    ret.stoppingControl = True
+    ret.startingState = True
    ret.vEgoStarting = 0.1
-    ret.startAccel = 1.6
+    ret.startAccel = 1.0
    ret.longitudinalActuatorDelay = 0.5

-    if ret.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
-      ret.startingState = False
-      ret.stopAccel = -2.0
-    else:
-      ret.startingState = True
-      ret.stopAccel = -1.0
-
    if DBC[ret.carFingerprint]["radar"] is None:
      if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD):
-        ret.radarTimeStep = 0.02
        ret.radarUnavailable = False

    # *** feature detection ***
@@ -126,8 +117,6 @@ class CarInterface(CarInterfaceBase):

      if 0x1fa in fingerprint[CAN.ECAN]:
        ret.spFlags |= HyundaiFlagsSP.SP_NAV_MSG.value
-      if Params().get("DongleId", encoding='utf8') in ("012c95f06918eca4", "68d6a96e703c00c9"):
-        ret.spFlags |= HyundaiFlagsSP.SP_UPSTREAM_TACO.value
    else:
      ret.enableBsm = 0x58b in fingerprint[0]

@@ -136,8 +125,7 @@ class CarInterface(CarInterfaceBase):

      if ret.flags & HyundaiFlags.MANDO_RADAR and ret.radarUnavailable:
        ret.spFlags |= HyundaiFlagsSP.SP_RADAR_TRACKS.value
-        if Params().get_bool("HyundaiRadarTracksAvailable"):
-          ret.radarUnavailable = False
+        ret.radarUnavailable = False

    # *** panda safety config ***
    if candidate in CANFD_CAR:
@@ -154,8 +142,6 @@ class CarInterface(CarInterfaceBase):
        ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
      if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC:
        ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
-      if ret.spFlags & HyundaiFlagsSP.SP_UPSTREAM_TACO:
-        ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_UPSTREAM_TACO
    else:
      if candidate in LEGACY_SAFETY_MODE_CAR:
        # these cars require a special panda safety mode due to missing counters and checksums in the messages
@@ -181,8 +167,7 @@ class CarInterface(CarInterfaceBase):
    elif ret.flags & HyundaiFlags.EV:
      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS

-    if candidate in (CAR.HYUNDAI_KONA, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_KONA_EV_2022,
-                     CAR.HYUNDAI_KONA_NON_SCC, CAR.HYUNDAI_KONA_EV_NON_SCC):
+    if candidate in (CAR.HYUNDAI_KONA, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_KONA_EV_2022, CAR.HYUNDAI_KONA_NON_SCC):
      ret.flags |= HyundaiFlags.ALT_LIMITS.value
      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS

@@ -192,13 +177,16 @@ class CarInterface(CarInterfaceBase):
    if 0x2AA in fingerprint[0]:
      ret.minSteerSpeed = 0.

+    if Params().get_bool("HkgSmoothStop"):
+      ret.vEgoStopping = 0.1
+
    return ret

  @staticmethod
  def init(CP, logcan, sendcan):
    if CP.openpilotLongitudinalControl and not ((CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or (CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC)) and \
      CP.carFingerprint not in CAMERA_SCC_CAR:
-      addr, bus = 0x7d0, CanBus(CP).ECAN if CP.carFingerprint in CANFD_CAR else 0
+      addr, bus = 0x7d0, 0
      if CP.flags & HyundaiFlags.CANFD_HDA2.value:
        addr, bus = 0x730, CanBus(CP).ECAN
      disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
@@ -213,23 +201,7 @@ class CarInterface(CarInterfaceBase):
    if CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS:
      enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42')

-      params = Params()
-      rt_avail = params.get_bool("HyundaiRadarTracksAvailable")
-      rt_avail_persist = params.get_bool("HyundaiRadarTracksAvailablePersistent")
-      params.put_bool_nonblocking("HyundaiRadarTracksAvailableCache", rt_avail)
-      if not rt_avail_persist:
-        messaging.drain_sock_raw(logcan)
-        fingerprint = can_fingerprint(lambda: get_one_can(logcan))
-        radar_unavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[CP.carFingerprint]["radar"] is None
-        params.put_bool_nonblocking("HyundaiRadarTracksAvailable", not radar_unavailable)
-        params.put_bool_nonblocking("HyundaiRadarTracksAvailablePersistent", True)
-
  def _update(self, c):
-    if not self.CS.control_initialized and not self.CP.pcmCruise:
-      can_cruise_main_default = self.CP.spFlags & HyundaiFlagsSP.SP_CAN_LFA_BTN and not self.CP.flags & HyundaiFlags.CANFD and \
-                                self.CS.params_list.hyundai_cruise_main_default
-      self.CS.mainEnabled = True if can_cruise_main_default or self.CP.carFingerprint in CANFD_CAR else False
-
    ret = self.CS.update(self.cp, self.cp_cam)

    self.CS.button_events = [
@@ -238,9 +210,10 @@ class CarInterface(CarInterfaceBase):
      *create_button_events(self.CS.main_buttons[-1], self.CS.prev_main_buttons, {1: ButtonType.altButton3}),
    ]

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+                                                            self.CS.button_events, c.vCruise)

    if ret.cruiseState.available:
      if not self.CP.pcmCruiseSpeed:
@@ -248,12 +221,12 @@ class CarInterface(CarInterfaceBase):
          self.CS.accEnabled = True

    if self.enable_mads:
-      if not self.CS.prev_mads_enabled and self.CS.mads_enabled and (self.CP.pcmCruise or
-        (any(b.type == ButtonType.altButton3 for b in self.CS.button_events) and not self.CP.pcmCruise)):
+      if not self.CS.prev_mads_enabled and self.CS.mads_enabled and \
+        any(b.type == ButtonType.altButton3 for b in self.CS.button_events):
        self.CS.madsEnabled = True
      if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
        self.CS.madsEnabled = not self.CS.madsEnabled
-      self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+      self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)

    if not ret.cruiseState.available and self.CS.out.cruiseState.available:
      self.CS.madsEnabled = False
@@ -261,15 +234,15 @@ class CarInterface(CarInterfaceBase):
    if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
      if not self.CP.pcmCruise:
        if any(b.type == ButtonType.cancel for b in self.CS.button_events):
-          self.get_sp_cancel_cruise_state()
+          self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      if not self.CP.pcmCruiseSpeed:
        if not ret.cruiseState.enabled:
-          self.get_sp_cancel_cruise_state()
+          self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -283,7 +256,7 @@ class CarInterface(CarInterfaceBase):
    events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.manumatic],
                                       pcm_enable=False, allow_enable=allow_enable)

-    events, ret = self.create_sp_events(ret, events, main_enabled=True, allow_enable=allow_enable)
+    events, ret = self.create_sp_events(self.CS, ret, events, main_enabled=True, allow_enable=allow_enable)

    # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
    if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
@@ -293,10 +266,9 @@ class CarInterface(CarInterfaceBase):
    if self.low_speed_alert and self.CS.madsEnabled:
      events.add(car.CarEvent.EventName.belowSteerSpeed)

-    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.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+                                                           self.CC.target_speed, self.CC.v_set_dis,
+                                                           self.CC.speed_diff, self.CC.button_type)

    ret.events = events.to_msg()

@@ -3,7 +3,7 @@ from dataclasses import dataclass, field
 from enum import Enum, IntFlag

 from cereal import car
-from panda.python import uds, Panda
+from panda.python import uds
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict
 from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
@@ -16,7 +16,7 @@ class CarControllerParams:
  ACCEL_MIN = -3.5 # m/s
  ACCEL_MAX = 2.0 # m/s

-  def __init__(self, CP, vEgoRaw=100.):
+  def __init__(self, CP):
    self.STEER_DELTA_UP = 3
    self.STEER_DELTA_DOWN = 7
    self.STEER_DRIVER_ALLOWANCE = 50
@@ -26,13 +26,12 @@ class CarControllerParams:
    self.STEER_STEP = 1  # 100 Hz

    if CP.carFingerprint in CANFD_CAR:
-      upstream_taco = CP.safetyConfigs[-1].safetyParam & Panda.FLAG_HYUNDAI_UPSTREAM_TACO
-      self.STEER_MAX = 270 if not upstream_taco else 384 if vEgoRaw < 11. else 330
-      self.STEER_DRIVER_ALLOWANCE = 250 if not upstream_taco else 350
+      self.STEER_MAX = 270
+      self.STEER_DRIVER_ALLOWANCE = 250
      self.STEER_DRIVER_MULTIPLIER = 2
-      self.STEER_THRESHOLD = 250 if not upstream_taco else 350
-      self.STEER_DELTA_UP = 2 if not upstream_taco else 10 if vEgoRaw < 11. else 2
-      self.STEER_DELTA_DOWN = 3 if not upstream_taco else 10 if vEgoRaw < 11. else 3
+      self.STEER_THRESHOLD = 250
+      self.STEER_DELTA_UP = 2
+      self.STEER_DELTA_DOWN = 3

    # To determine the limit for your car, find the maximum value that the stock LKAS will request.
    # If the max stock LKAS request is <384, add your car to this list.
@@ -109,7 +108,6 @@ class HyundaiFlagsSP(IntFlag):
  SP_CAMERA_SCC_LEAD = 2 ** 6
  SP_LKAS12 = 2 ** 7
  SP_RADAR_TRACKS = 2 ** 8
-  SP_UPSTREAM_TACO = 2 ** 9


 class Footnote(Enum):
@@ -576,12 +574,6 @@ class CAR(Platforms):
    HYUNDAI_KONA.specs,
    spFlags=HyundaiFlagsSP.SP_NON_SCC | HyundaiFlagsSP.SP_NON_SCC_FCA,
  )
-  HYUNDAI_KONA_EV_NON_SCC = HyundaiPlatformConfig(
-    [HyundaiCarDocs("Hyundai Kona Electric Non-SCC 2019", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_g]))],
-    HYUNDAI_KONA.specs,
-    flags=HyundaiFlags.EV,
-    spFlags=HyundaiFlagsSP.SP_NON_SCC | HyundaiFlagsSP.SP_NON_SCC_FCA,
-  )
  KIA_CEED_PHEV_2022_NON_SCC = HyundaiPlatformConfig(
    [HyundaiCarDocs("Kia Ceed PHEV Non-SCC 2022", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_i]))],
    CarSpecs(mass=1650, wheelbase=2.65, steerRatio=13.75, tireStiffnessFactor=0.5),
@@ -244,6 +244,8 @@ class CarInterfaceBase(ABC):
    self.cruise_cancelled_btn = True
    self.prev_acc_mads_combo = False
    self.mads_event_lock = True
+    self.gap_button_counter = 0
+    self.experimental_mode_hold = False
    self.last_mads_init = 0.
    self.madsEnabledInit = False
    self.madsEnabledInitPrev = False
@@ -406,7 +408,7 @@ class CarInterfaceBase(ABC):

  @staticmethod
  def sp_configure_custom_torque_tune(ret, params):
-    ret.lateralTuning.torque.friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.001
+    ret.lateralTuning.torque.friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.01
    ret.lateralTuning.torque.latAccelFactor = float(params.get("TorqueMaxLatAccel", encoding="utf8")) * 0.01
    return ret

@@ -548,20 +550,20 @@ class CarInterfaceBase(ABC):
  def sp_v_cruise_initialized(v_cruise):
    return v_cruise != V_CRUISE_UNSET

-  def get_acc_mads(self, cs_out, mads_enabled):
+  def get_acc_mads(self, cruiseState_enabled, acc_enabled, mads_enabled):
    if self.CS.params_list.acc_mads_combo:
-      if not self.prev_acc_mads_combo and (cs_out.cruiseState.enabled or self.CS.accEnabled):
+      if not self.prev_acc_mads_combo and (cruiseState_enabled or acc_enabled):
        mads_enabled = True
-      self.prev_acc_mads_combo = (cs_out.cruiseState.enabled or self.CS.accEnabled)
+      self.prev_acc_mads_combo = (cruiseState_enabled or acc_enabled)

    return mads_enabled

-  def get_sp_v_cruise_non_pcm_state(self, cs_out, vCruise, acc_enabled,
+  def get_sp_v_cruise_non_pcm_state(self, cs_out, acc_enabled, button_events, vCruise,
                                    enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise),
                                    resume_button=(ButtonType.accelCruise, ButtonType.resumeCruise)):

    if cs_out.cruiseState.available:
-      for b in self.CS.button_events:
+      for b in button_events:
        if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
          if b.type in enable_buttons and not b.pressed:
            acc_enabled = True
@@ -576,9 +578,9 @@ class CarInterfaceBase(ABC):

    return acc_enabled

-  def get_sp_cancel_cruise_state(self):
-    self.CS.madsEnabled = False if not self.enable_mads or self.disengage_on_accelerator else self.CS.madsEnabled
-    self.CS.accEnabled = False
+  def get_sp_cancel_cruise_state(self, mads_enabled, acc_enabled=False):
+    mads_enabled = False if not self.enable_mads or self.disengage_on_accelerator else mads_enabled
+    return mads_enabled, acc_enabled

  def get_sp_pedal_disengage(self, cs_out):
    accel_pedal = cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator
@@ -586,22 +588,24 @@ class CarInterfaceBase(ABC):
    regen = cs_out.regenBraking and (not self.CS.out.regenBraking or not cs_out.standstill)
    return accel_pedal or brake or regen

-  def get_sp_cruise_main_state(self, cs_out):
-    if not self.CS.control_initialized:
-      return False
+  def get_sp_cruise_main_state(self, cs_out, CS):
+    if not CS.control_initialized:
+      mads_enabled = False
    elif not self.CS.params_list.mads_main_toggle:
-      return False
+      mads_enabled = False
    else:
-      return cs_out.cruiseState.available
+      mads_enabled = cs_out.cruiseState.available

-  def get_sp_started_mads(self, cs_out, mads_enabled):
-    if not cs_out.cruiseState.available and self.CS.out.cruiseState.available:
+    return mads_enabled
+
+  def get_sp_started_mads(self, cs_out, CS):
+    if not cs_out.cruiseState.available and CS.out.cruiseState.available:
      self.madsEnabledInit = False
      self.madsEnabledInitPrev = False
      return False
    if not self.CS.params_list.mads_main_toggle or self.prev_acc_mads_combo:
-      return mads_enabled
-    if not self.madsEnabledInit and self.CS.madsEnabled:
+      return CS.madsEnabled
+    if not self.madsEnabledInit and CS.madsEnabled:
      self.madsEnabledInit = True
      self.last_mads_init = time.monotonic()
    if cs_out.gearShifter not in FORWARD_GEARS:
@@ -612,16 +616,19 @@ class CarInterfaceBase(ABC):
      self.madsEnabledInitPrev = True
      return cs_out.cruiseState.available
    else:
-      return mads_enabled
+      return CS.madsEnabled

-  def get_sp_common_state(self, cs_out, gear_allowed=True):
-    cs_out.cruiseState.enabled = self.CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
+  def get_sp_common_state(self, cs_out, CS, gear_allowed=True, gap_button=False):
+    cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled

    if not self.enable_mads:
-      if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
-        self.CS.madsEnabled = True
-      elif not cs_out.cruiseState.enabled and self.CS.out.cruiseState.enabled:
-        self.CS.madsEnabled = False
+      if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled:
+        CS.madsEnabled = True
+      elif not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled:
+        CS.madsEnabled = False
+
+    if self.CP.openpilotLongitudinalControl:
+      self.toggle_exp_mode(gap_button)

    lane_change_speed_min = get_min_lateral_speed(self.CS.params_list.pause_lateral_speed, self.CS.params_list.is_metric)

@@ -633,24 +640,37 @@ class CarInterfaceBase(ABC):

    cs_out.latActive = gear_allowed

-    if not self.CS.control_initialized:
-      self.CS.control_initialized = True
+    if not CS.control_initialized:
+      CS.control_initialized = True

    # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
    if (cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator) or \
      (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)) or \
      (cs_out.regenBraking and (not self.CS.out.regenBraking or not cs_out.standstill)):
-      if self.CS.madsEnabled:
-        self.CS.disengageByBrake = True
+      if CS.madsEnabled:
+        CS.disengageByBrake = True

-    cs_out.madsEnabled = self.CS.madsEnabled
-    cs_out.accEnabled = self.CS.accEnabled
-    cs_out.disengageByBrake = self.CS.disengageByBrake
+    cs_out.madsEnabled = CS.madsEnabled
+    cs_out.accEnabled = CS.accEnabled
+    cs_out.disengageByBrake = CS.disengageByBrake
    cs_out.brakeLightsDEPRECATED |= cs_out.brakePressed or cs_out.brakeHoldActive or cs_out.parkingBrake or cs_out.regenBraking

-    return cs_out
+    return cs_out, CS

-  def create_sp_events(self, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
+  # TODO: SP: use upstream's buttonEvents counter checks from controlsd
+  def toggle_exp_mode(self, gap_pressed):
+    if gap_pressed:
+      if not self.experimental_mode_hold:
+        self.gap_button_counter += 1
+        if self.gap_button_counter > 50:
+          self.gap_button_counter = 0
+          self.experimental_mode_hold = True
+          self.param_s.put_bool_nonblocking("ExperimentalMode", not self.CS.params_list.experimental_mode)
+    else:
+      self.gap_button_counter = 0
+      self.experimental_mode_hold = False
+
+  def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
                       enable_from_brake=False, enable_pressed_long=False,
                       enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):

@@ -658,7 +678,7 @@ class CarInterfaceBase(ABC):
      if cs_out.disengageByBrake and cs_out.madsEnabled:
        enable_pressed = True
        enable_from_brake = True
-      self.CS.disengageByBrake = False
+      CS.disengageByBrake = False
      cs_out.disengageByBrake = False

    for b in cs_out.buttonEvents:
@@ -687,11 +707,11 @@ class CarInterfaceBase(ABC):
    if self.CP.pcmCruise:
      # do disable on button down
      if main_enabled:
-        if any(self.CS.main_buttons) and not cs_out.cruiseState.enabled:
+        if any(CS.main_buttons) and not cs_out.cruiseState.enabled:
          if not cs_out.madsEnabled:
            events.add(EventName.buttonCancel)
      # do enable on both accel and decel buttons
-      if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable:
+      if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled and allow_enable:
        enable_pressed = True
        enable_pressed_long = True
      elif not cs_out.cruiseState.enabled:
@@ -711,16 +731,6 @@ class CarInterfaceBase(ABC):

    return events, cs_out

-  def update_custom_stock_long(self):
-    customStockLong = car.CarState.CustomStockLong.new_message()
-    customStockLong.cruiseButton = 0 if self.CC.cruise_button is None else int(self.CC.cruise_button)
-    customStockLong.finalSpeedKph = float(self.CC.final_speed_kph)
-    customStockLong.targetSpeed = float(self.CC.target_speed)
-    customStockLong.vSetDis = float(self.CC.v_set_dis)
-    customStockLong.speedDiff = float(self.CC.speed_diff)
-    customStockLong.buttonType = int(self.CC.button_type)
-    return customStockLong
-
 class RadarInterfaceBase(ABC):
  def __init__(self, CP):
    self.CP = CP
@@ -824,6 +834,16 @@ class CarStateBase(ABC):

    return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)

+  def update_custom_stock_long(self, cruise_button, final_speed_kph, target_speed, v_set_dis, speed_diff, button_type):
+    customStockLong = car.CarState.CustomStockLong.new_message()
+    customStockLong.cruiseButton = 0 if cruise_button is None else cruise_button
+    customStockLong.finalSpeedKph = final_speed_kph
+    customStockLong.targetSpeed = target_speed
+    customStockLong.vSetDis = v_set_dis
+    customStockLong.speedDiff = speed_diff
+    customStockLong.buttonType = button_type
+    return customStockLong
+
  @staticmethod
  def parse_gear_shifter(gear: str | None) -> car.CarState.GearShifter:
    if gear is None:
@@ -856,13 +876,6 @@ class CarControllerBase(ABC):
    self.CP = CP
    self.frame = 0

-    self.cruise_button = 0
-    self.final_speed_kph = 0.0
-    self.target_speed = 0.0
-    self.v_set_dis = 0.0
-    self.speed_diff = 0.0
-    self.button_type = 0
-
  @abstractmethod
  def update(self, CC: car.CarControl.Actuators, CS: car.CarState, now_nanos: int) -> tuple[car.CarControl.Actuators, list[SendCan]]:
    pass
@@ -45,9 +45,10 @@ class CarInterface(CarInterfaceBase):
      *create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
    ]

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+                                                            self.CS.button_events, c.vCruise)

    if ret.cruiseState.available:
      if self.enable_mads:
@@ -55,15 +56,15 @@ class CarInterface(CarInterfaceBase):
          self.CS.madsEnabled = True
        if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
          self.CS.madsEnabled = not self.CS.madsEnabled
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False

    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
-        self.get_sp_cancel_cruise_state()
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -71,7 +72,7 @@ class CarInterface(CarInterfaceBase):
        self.CS.accEnabled = False
      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -82,14 +83,16 @@ class CarInterface(CarInterfaceBase):
    events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
                                       pcm_enable=False)

-    events, ret = self.create_sp_events(ret, events)
+    events, ret = self.create_sp_events(self.CS, ret, events)

    #if self.CS.lkas_disabled:
    #  events.add(EventName.lkasDisabled)
    if self.CS.low_speed_alert:
      events.add(EventName.belowSteerSpeed)

-    ret.customStockLong = self.update_custom_stock_long()
+    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+                                                           self.CC.target_speed, self.CC.v_set_dis,
+                                                           self.CC.speed_diff, self.CC.button_type)

    ret.events = events.to_msg()

@@ -35,22 +35,22 @@ class CarInterface(CarInterfaceBase):

    self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

    if ret.cruiseState.available:
      if self.enable_mads:
        if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
          self.CS.madsEnabled = True
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False

    if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \
       self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -61,7 +61,7 @@ class CarInterface(CarInterfaceBase):
    events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
                                       pcm_enable=False)

-    events, ret = self.create_sp_events(ret, events)
+    events, ret = self.create_sp_events(self.CS, ret, events)

    if self.CS.lkas_enabled:
      events.add(car.CarEvent.EventName.invalidLkasSetting)
@@ -9,11 +9,8 @@ class ParamManager:
      "acc_mads_combo": False,
      "below_speed_pause": False,
      "experimental_mode": False,
-      "hyundai_radar_tracks_available": False,
-      "hyundai_radar_tracks_available_cache": False,
      "is_metric": False,
      "last_speed_limit_sign_tap": False,
-      "hyundai_cruise_main_default": False,
      "mads_main_toggle": False,
      "pause_lateral_speed": 0,
      "reverse_acc_change": False,
@@ -38,11 +35,8 @@ class ParamManager:
      "acc_mads_combo": params.get_bool("AccMadsCombo"),
      "below_speed_pause": params.get_bool("BelowSpeedPause"),
      "experimental_mode": params.get_bool("ExperimentalMode"),
-      "hyundai_radar_tracks_available": params.get_bool("HyundaiRadarTracksAvailable"),
-      "hyundai_radar_tracks_available_cache": params.get_bool("HyundaiRadarTracksAvailableCache"),
      "is_metric": params.get_bool("IsMetric"),
      "last_speed_limit_sign_tap": params.get_bool("LastSpeedLimitSignTap"),
-      "hyundai_cruise_main_default": params.get_bool("HyundaiCruiseMainDefault"),
      "mads_main_toggle": params.get_bool("MadsCruiseMain"),
      "pause_lateral_speed": int(params.get("PauseLateralSpeed", encoding="utf8")),
      "reverse_acc_change": params.get_bool("ReverseAccChange"),
@@ -117,7 +117,7 @@ class CarInterface(CarInterfaceBase):

    ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

    if ret.cruiseState.available:
      if self.enable_mads:
@@ -128,7 +128,7 @@ class CarInterface(CarInterfaceBase):
            self.CS.madsEnabled = not self.CS.madsEnabled
          elif self.CS.prev_lkas_enabled != self.CS.lkas_enabled and self.CS.prev_lkas_enabled == 2 and self.CS.lkas_enabled != 1:
            self.CS.madsEnabled = not self.CS.madsEnabled
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False

@@ -137,10 +137,10 @@ class CarInterface(CarInterfaceBase):
        if not self.enable_mads:
          self.CS.madsEnabled = False
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS)

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -150,7 +150,7 @@ class CarInterface(CarInterfaceBase):

    events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False)

-    events, ret = self.create_sp_events(ret, events)
+    events, ret = self.create_sp_events(self.CS, ret, events)

    ret.events = events.to_msg()

@@ -21,7 +21,7 @@ class CarControllerParams:
    self.STEER_DRIVER_FACTOR = 1       # from dbc

    if CP.flags & SubaruFlags.GLOBAL_GEN2:
-      self.STEER_MAX = 1600
+      self.STEER_MAX = 1000
      self.STEER_DELTA_UP = 40
      self.STEER_DELTA_DOWN = 40
    elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020:
@@ -1,37 +0,0 @@
-from collections.abc import Callable
-
-import cereal.messaging as messaging
-from openpilot.selfdrive.car import gen_empty_fingerprint
-
-FRAME_FINGERPRINT = 25  # 0.25s
-
-
-def get_one_can(logcan):
-  while True:
-    can = messaging.recv_one_retry(logcan)
-    if len(can.can) > 0:
-      return can
-
-
-def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]:
-  finger = gen_empty_fingerprint()
-  frame = 0
-  done = False
-
-  while not done:
-    a = next_can()
-
-    for can in a.can:
-      # The fingerprint dict is generated for all buses, this way the car interface
-      # can use it to detect a (valid) multipanda setup and initialize accordingly
-      if can.src < 128:
-        if can.src not in finger:
-          finger[can.src] = {}
-        finger[can.src][can.address] = len(can.dat)
-
-    # bail if we've been waiting for more than 2s
-    done = frame > 100
-
-    frame += 1
-
-  return finger
@@ -29,9 +29,6 @@
  "Ford Escape Plug-in Hybrid 2020-22": "FORD_ESCAPE_MK4",
  "Ford Explorer 2020-23": "FORD_EXPLORER_MK6",
  "Ford Explorer Hybrid 2020-23": "FORD_EXPLORER_MK6",
-  "Ford F-150 2022-23": "FORD_F_150_MK14",
-  "Ford F-150 Hybrid 2022-23": "FORD_F_150_MK14",
-  "Ford F-150 Lightning 2021-23": "FORD_F_150_LIGHTNING_MK1",
  "Ford Focus 2018": "FORD_FOCUS_MK4",
  "Ford Focus Hybrid 2018": "FORD_FOCUS_MK4",
  "Ford Kuga 2020-22": "FORD_ESCAPE_MK4",
@@ -41,8 +38,6 @@
  "Ford Maverick 2023-24": "FORD_MAVERICK_MK1",
  "Ford Maverick Hybrid 2022": "FORD_MAVERICK_MK1",
  "Ford Maverick Hybrid 2023-24": "FORD_MAVERICK_MK1",
-  "Ford Mustang Mach-E 2021-23": "FORD_MUSTANG_MACH_E_MK1",
-  "Ford Ranger 2024": "FORD_RANGER_MK2",
  "Genesis G70 2018": "GENESIS_G70",
  "Genesis G70 2019-21": "GENESIS_G70_2020",
  "Genesis G70 2022-23": "GENESIS_G70_2020",
@@ -106,7 +101,6 @@
  "Hyundai Kona Electric 2018-21": "HYUNDAI_KONA_EV",
  "Hyundai Kona Electric 2022-23": "HYUNDAI_KONA_EV_2022",
  "Hyundai Kona Electric (with HDA II, Korea only) 2023": "HYUNDAI_KONA_EV_2ND_GEN",
-  "Hyundai Kona Electric Non-SCC 2019": "HYUNDAI_KONA_EV_NON_SCC",
  "Hyundai Kona Hybrid 2020": "HYUNDAI_KONA_HEV",
  "Hyundai Kona Non-SCC 2019": "HYUNDAI_KONA_NON_SCC",
  "Hyundai Palisade 2020-22": "HYUNDAI_PALISADE",
@@ -230,6 +224,8 @@
  "Škoda Octavia Scout 2017-19": "SKODA_OCTAVIA_MK3",
  "Škoda Scala 2020-23": "SKODA_KAMIQ_MK1",
  "Škoda Superb 2015-22": "SKODA_SUPERB_MK3",
+  "Tesla AP3 Model 3": "TESLA_AP3_MODEL3",
+  "Tesla AP3 Model Y": "TESLA_AP3_MODELY",
  "Toyota Alphard 2019-20": "TOYOTA_ALPHARD_TSS2",
  "Toyota Alphard Hybrid 2021": "TOYOTA_ALPHARD_TSS2",
  "Toyota Avalon 2016": "TOYOTA_AVALON",
@@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_std_steer_angle_limits
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN
-from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
+from openpilot.selfdrive.car.tesla.values import DBC, CarControllerParams


 class CarController(CarControllerBase):
@@ -37,25 +37,26 @@ class CarController(CarControllerBase):
      self.apply_angle_last = apply_angle
      can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, (self.frame // 2) % 16))

-    # Longitudinal control (in sync with stock message, about 40Hz)
+    # Longitudinal control
    if self.CP.openpilotLongitudinalControl:
+      acc_state = CS.das_control["DAS_accState"]
      target_accel = actuators.accel
      target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
      max_accel = 0 if target_accel < 0 else target_accel
      min_accel = 0 if target_accel > 0 else target_accel

-      while len(CS.das_control_counters) > 0:
-        can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft()))
+      counter = CS.das_control["DAS_controlCounter"]
+      can_sends.append(self.tesla_can.create_longitudinal_commands(acc_state, target_speed, min_accel, max_accel, counter))

    # Cancel on user steering override, since there is no steering torque blending
    if hands_on_fault:
      pcm_cancel_cmd = True

-    if self.frame % 10 == 0 and pcm_cancel_cmd:
-      # Spam every possible counter value, otherwise it might not be accepted
-      for counter in range(16):
-        can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter))
-        can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter))
+    # Sent cancel request only if ACC is enabled
+    if self.frame % 10 == 0 and pcm_cancel_cmd and CS.acc_enabled:
+      counter = int(CS.sccm_right_stalk_counter)
+      can_sends.append(self.tesla_can.right_stalk_press((counter + 1) % 16 , 1))  # half up (cancel acc)
+      can_sends.append(self.tesla_can.right_stalk_press((counter + 2) % 16, 0))  # to prevent neutral gear warning

    # TODO: HUD control

@@ -1,8 +1,7 @@
 import copy
-from collections import deque
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
+from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, BUTTONS
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@@ -14,13 +13,13 @@ class CarState(CarStateBase):
    self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis'])

    # Needed by carcontroller
-    self.msg_stw_actn_req = None
    self.hands_on_level = 0
    self.steer_warning = None
-    self.acc_state = 0
-    self.das_control_counters = deque(maxlen=32)
+    self.acc_enabled = None
+    self.sccm_right_stalk_counter = None
+    self.das_control = None

-  def update(self, cp, cp_cam):
+  def update(self, cp, cp_cam, cp_adas):
    ret = car.CarState.new_message()

    # Vehicle speed
@@ -29,78 +28,74 @@ class CarState(CarStateBase):
    ret.standstill = (ret.vEgo < 0.1)

    # Gas pedal
-    ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0
-    ret.gasPressed = (ret.gas > 0)
+    pedal_status = cp.vl["DI_systemStatus"]["DI_accelPedalPos"]
+    ret.gas = pedal_status / 100.0
+    ret.gasPressed = (pedal_status > 0)

    # Brake pedal
    ret.brake = 0
-    ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1)
+    ret.brakePressed = cp.vl["IBST_status"]["IBST_driverBrakeApply"] == 2

    # Steering wheel
-    epas_status = cp_cam.vl["EPAS3P_sysStatus"] if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN else cp.vl["EPAS_sysStatus"]
+    epas_status = cp.vl["EPAS3S_sysStatus"]
+    self.hands_on_level = epas_status["EPAS3S_handsOnLevel"]
+    self.steer_warning = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacErrorCode"].get(int(epas_status["EPAS3S_eacErrorCode"]), None)
+    ret.steeringAngleDeg = -epas_status["EPAS3S_internalSAS"]
+    ret.steeringRateDeg = -cp_adas.vl["SCCM_steeringAngleSensor"]["SCCM_steeringAngleSpeed"]
+    ret.steeringTorque = -epas_status["EPAS3S_torsionBarTorque"]

-    self.hands_on_level = epas_status["EPAS_handsOnLevel"]
-    self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None)
-    steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None)
-
-    ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"]
-    ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate
-    ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"]
    ret.steeringPressed = (self.hands_on_level > 0)
-    ret.steerFaultPermanent = steer_status == "EAC_FAULT"
+    ret.steerFaultPermanent = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacStatus"].get(int(epas_status["EPAS3S_eacStatus"]), None) in ["EAC_FAULT"]
    ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON"))

    # Cruise state
    cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)
    speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None)

-    acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"))
-
-    ret.cruiseState.enabled = acc_enabled
+    self.acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"))
+    ret.cruiseState.enabled = self.acc_enabled
    if speed_units == "KPH":
      ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS
    elif speed_units == "MPH":
      ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS
    ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled)
-    ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special
+    ret.cruiseState.standstill = False  # This needs to be false, since we can resume from stop without sending anything special

    # Gear
-    ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
+    ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_systemStatus"]["DI_gear"].get(int(cp.vl["DI_systemStatus"]["DI_gear"]), "DI_GEAR_INVALID")]

-    # Buttons
-    buttonEvents = []
+    button_events = []
    for button in BUTTONS:
-      state = (cp.vl[button.can_addr][button.can_msg] in button.values)
+      state = (cp_adas.vl[button.can_addr][button.can_msg] in button.values)
      if self.button_states[button.event_type] != state:
        event = car.CarState.ButtonEvent.new_message()
        event.type = button.event_type
        event.pressed = state
-        buttonEvents.append(event)
+        button_events.append(event)
      self.button_states[button.event_type] = state
-    ret.buttonEvents = buttonEvents
+    self.button_events = button_events

    # Doors
-    ret.doorOpen = any((self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS)
+    ret.doorOpen = (cp.vl["UI_warning"]["anyDoorOpen"] == 1)

    # Blinkers
-    ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1)
-    ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1)
+    ret.leftBlinker = (cp_adas.vl["ID3F5VCFRONT_lighting"]["VCFRONT_indicatorLeftRequest"] != 0)
+    ret.rightBlinker = (cp_adas.vl["ID3F5VCFRONT_lighting"]["VCFRONT_indicatorRightRequest"] != 0)

    # Seatbelt
-    if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      ret.seatbeltUnlatched = (cp.vl["DriverSeat"]["buckleStatus"] != 1)
-    else:
-      ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1)
+    ret.seatbeltUnlatched = cp.vl["UI_warning"]["buckleStatus"] != 1

-    # TODO: blindspot
+    # Blindspot
+    if self.CP.enableBsm:
+      ret.leftBlindspot = cp_cam.vl["DAS_status"]["DAS_blindSpotRearLeft"] != 0
+      ret.rightBlindspot = cp_cam.vl["DAS_status"]["DAS_blindSpotRearRight"] != 0

    # AEB
    ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1)

    # Messages needed by carcontroller
-    self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"])
-    self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"]
-    self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"])
+    self.sccm_right_stalk_counter = copy.copy(cp_adas.vl["SCCM_rightStalk"]["SCCM_rightStalkCounter"])
+    self.das_control = copy.copy(cp_cam.vl["DAS_control"])

    return ret

@@ -109,31 +104,33 @@ class CarState(CarStateBase):
    messages = [
      # sig_address, frequency
      ("ESP_B", 50),
-      ("DI_torque1", 100),
-      ("DI_torque2", 100),
-      ("STW_ANGLHP_STAT", 100),
-      ("EPAS_sysStatus", 25),
+      ("DI_systemStatus", 100),
+      ("IBST_status", 25),
      ("DI_state", 10),
-      ("STW_ACTN_RQ", 10),
-      ("GTW_carState", 10),
-      ("BrakeMessage", 50),
+      ("EPAS3S_sysStatus", 100),
+      ("UI_warning", 10)
    ]

-    if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      messages.append(("DriverSeat", 20))
-    else:
-      messages.append(("SDM1", 10))
-
-    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis)
+    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.party)

  @staticmethod
  def get_cam_can_parser(CP):
    messages = [
-      # sig_address, frequency
-      ("DAS_control", 40),
+      ("DAS_control", 25),
+      ("DAS_status", 2)
    ]

-    if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      messages.append(("EPAS3P_sysStatus", 100))
+    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_party)

-    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis)
+  @staticmethod
+  def get_adas_can_parser(CP):  # Vehicle Can on Model 3
+    messages = [
+      ("VCLEFT_switchStatus", 20),
+      ("SCCM_leftStalk", 10),
+      ("SCCM_rightStalk", 10),
+      ("SCCM_steeringAngleSensor", 100),
+      ("DAS_bodyControls", 2),
+      ("ID3F5VCFRONT_lighting", 10),
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CANBUS.vehicle)
@@ -4,29 +4,50 @@ from openpilot.selfdrive.car.tesla.values import CAR
 Ecu = car.CarParams.Ecu

 FW_VERSIONS = {
-  CAR.TESLA_AP2_MODELS: {
-    (Ecu.adas, 0x649, None): [
-      b'\x01\x00\x8b\x07\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x11',
-    ],
-    (Ecu.electricBrakeBooster, 0x64d, None): [
-      b'1037123-00-A',
-    ],
-    (Ecu.fwdRadar, 0x671, None): [
-      b'\x01\x00W\x00\x00\x00\x07\x00\x00\x00\x00\x08\x01\x00\x00\x00\x07\xff\xfe',
-    ],
+  CAR.TESLA_AP3_MODEL3: {
    (Ecu.eps, 0x730, None): [
-      b'\x10#\x01',
+      b'TeM3_E014p10_0.0.0 (16),E014.17.00',
+      b'TeMYG4_DCS_Update_0.0.0 (13),E4014.28.1',
+      b'TeMYG4_DCS_Update_0.0.0 (9),E4014.26.0',
+      b'TeMYG4_SingleECU_0.0.0 (33),E4S014.27',
+    ],
+    (Ecu.engine, 0x606, None): [
+      b'\x01\x00\x05\x18A\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x000\x91',
+      b'\x01\x00\x05\x1cO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\x11',
+      b'\x01\x00\x05\x1cF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\r',
+      b'\x01\x00\x05\x1eK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\xcc',
+      b'\x01\x00\x05\x1eO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\xe0',
+      b'\x01\x00\x05\x1e[\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x05',
+      b'\x01\x00\x05 O\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x99',
+      b'\x01\x00\x05 O\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xbd',
+      b'\x01\x00\x05 D\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xe1',
+      b'\x01\x00\x05 K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xa4',
+      b'\x01\x00\x05 N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xa7',
+      b'\x01\x00\x05 N\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xf2',
+      b'\x01\x00\x05 [\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xd8',
    ],
  },
-  CAR.TESLA_MODELS_RAVEN: {
-    (Ecu.electricBrakeBooster, 0x64d, None): [
-      b'1037123-00-A',
-    ],
-    (Ecu.fwdRadar, 0x671, None): [
-      b'\x01\x00\x99\x02\x01\x00\x10\x00\x00AP8.3.03\x00\x10',
-    ],
+  CAR.TESLA_AP3_MODELY: {
    (Ecu.eps, 0x730, None): [
-      b'SX_0.0.0 (99),SR013.7',
+      b'TeM3_E014p10_0.0.0 (16),Y002.18.00',
+      b'TeM3_ES014p11_0.0.0 (16),YS002.17',
+      b'TeM3_ES014p11_0.0.0 (25),YS002.19.0',
+      b'TeMYG4_DCS_Update_0.0.0 (13),Y4002.27.1',
+      b'TeMYG4_DCS_Update_0.0.0 (9),Y4P002.25.0',
+      b'TeMYG4_DCS_Update_0.0.0 (13),Y4P002.27.1',
+      b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26'
+    ],
+    (Ecu.engine, 0x606, None): [
+      b'\x01\x00\x05\x1cO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\x12',
+      b'\x01\x00\x05\x1eO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\xe2',
+      b'\x01\x00\x05\x1e[\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x06',
+      b'\x01\x00\x05 D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xd3',
+      b'\x01\x00\x05 O\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x9a',
+      b'\x01\x00\x05 m\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xd5',
+      b'\x01\x00\x05 [\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xd7',
+      b'\x01\x00\x05 ^\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xae',
+      b'\x01\x00\x05 \\\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xe7',
+      b'\x01\x00\x055p\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00;\xc5',
    ],
  },
 }
@@ -1,12 +1,17 @@
 #!/usr/bin/env python3
 from cereal import car
 from panda import Panda
-from openpilot.selfdrive.car.tesla.values import CANBUS, CAR
+from openpilot.selfdrive.car.tesla.values import CAR
 from openpilot.selfdrive.car import get_safety_config
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase

+ButtonType = car.CarState.ButtonEvent.Type
+

 class CarInterface(CarInterfaceBase):
+  def __init__(self, CP, CarController, CarState):
+    super().__init__(CP, CarController, CarState)
+
  @staticmethod
  def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
    ret.carName = "tesla"
@@ -14,34 +19,62 @@ class CarInterface(CarInterfaceBase):
    # There is no safe way to do steer blending with user torque,
    # so the steering behaves like autopilot. This is not
    # how openpilot should be, hence dashcamOnly
-    ret.dashcamOnly = True
+    # ret.dashcamOnly = True

    ret.steerControlType = car.CarParams.SteerControlType.angle

    ret.longitudinalActuatorDelay = 0.5 # s
-    ret.radarTimeStep = (1.0 / 8) # 8Hz
+    ret.radarUnavailable = True

-    # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus
-    # If so, we assume that it is connected to the longitudinal harness.
-    flags = (Panda.FLAG_TESLA_RAVEN if candidate == CAR.TESLA_MODELS_RAVEN else 0)
-    if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()):
-      ret.openpilotLongitudinalControl = True
+    if candidate in [CAR.TESLA_AP3_MODEL3, CAR.TESLA_AP3_MODELY]:
+      flags = Panda.FLAG_TESLA_MODEL3_Y
      flags |= Panda.FLAG_TESLA_LONG_CONTROL
-      ret.safetyConfigs = [
-        get_safety_config(car.CarParams.SafetyModel.tesla, flags),
-        get_safety_config(car.CarParams.SafetyModel.tesla, flags | Panda.FLAG_TESLA_POWERTRAIN),
-      ]
-    else:
-      ret.openpilotLongitudinalControl = False
+      ret.openpilotLongitudinalControl = True
      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, flags)]
+      ret.enableBsm = True

    ret.steerLimitTimer = 1.0
    ret.steerActuatorDelay = 0.25
    return ret

  def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret = self.CS.update(self.cp, self.cp_cam, self.cp_adas)

-    ret.events = self.create_common_events(ret).to_msg()
+    if ret.cruiseState.available:
+      if self.enable_mads:
+        for b in self.CS.button_events:
+          if b.type == ButtonType.altButton1 and b.pressed:
+            self.CS.madsEnabled = True
+          elif b.type == ButtonType.altButton2 and b.pressed:
+            self.CS.madsEnabled = False
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = False if self.CS.steer_warning == "EAC_ERROR_HANDS_ON" and self.CS.hands_on_level >= 3 else self.CS.madsEnabled
+    else:
+      self.CS.madsEnabled = False
+
+    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
+      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+    if self.get_sp_pedal_disengage(ret):
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
+
+    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
+      if ret.gasPressed and not ret.cruiseState.enabled:
+        self.CS.accEnabled = False
+      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
+
+    ret, self.CS = self.get_sp_common_state(ret, self.CS)
+
+    ret.buttonEvents = [
+      *self.CS.button_events,
+      *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled)  # MADS BUTTON
+    ]
+
+    events = self.create_common_events(ret, c, pcm_enable=False)
+
+    events, ret = self.create_sp_events(self.CS, ret, events)
+
+    ret.events = events.to_msg()

    return ret
@@ -1,90 +1,4 @@
-#!/usr/bin/env python3
-from cereal import car
-from opendbc.can.parser import CANParser
-from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase

-
 class RadarInterface(RadarInterfaceBase):
-  def __init__(self, CP):
-    super().__init__(CP)
-
-    if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      messages = [('RadarStatus', 16)]
-      self.num_points = 40
-      self.trigger_msg = 1119
-    else:
-      messages = [('TeslaRadarSguInfo', 10)]
-      self.num_points = 32
-      self.trigger_msg = 878
-
-    for i in range(self.num_points):
-      messages.extend([
-        (f'RadarPoint{i}_A', 16),
-        (f'RadarPoint{i}_B', 16),
-      ])
-
-    self.rcp = CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar)
-    self.updated_messages = set()
-    self.track_id = 0
-
-  def update(self, can_strings):
-    if self.rcp is None:
-      return super().update(None)
-
-    values = self.rcp.update_strings(can_strings)
-    self.updated_messages.update(values)
-
-    if self.trigger_msg not in self.updated_messages:
-      return None
-
-    ret = car.RadarData.new_message()
-
-    # Errors
-    errors = []
-    if not self.rcp.can_valid:
-      errors.append('canError')
-
-    if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      radar_status = self.rcp.vl['RadarStatus']
-      if radar_status['sensorBlocked'] or radar_status['shortTermUnavailable'] or radar_status['vehDynamicsError']:
-        errors.append('fault')
-    else:
-      radar_status = self.rcp.vl['TeslaRadarSguInfo']
-      if radar_status['RADC_HWFail'] or radar_status['RADC_SGUFail'] or radar_status['RADC_SensorDirty']:
-        errors.append('fault')
-
-    ret.errors = errors
-
-    # Radar tracks
-    for i in range(self.num_points):
-      msg_a = self.rcp.vl[f'RadarPoint{i}_A']
-      msg_b = self.rcp.vl[f'RadarPoint{i}_B']
-
-      # Make sure msg A and B are together
-      if msg_a['Index'] != msg_b['Index2']:
-        continue
-
-      # Check if it's a valid track
-      if not msg_a['Tracked']:
-        if i in self.pts:
-          del self.pts[i]
-        continue
-
-      # New track!
-      if i not in self.pts:
-        self.pts[i] = car.RadarData.RadarPoint.new_message()
-        self.pts[i].trackId = self.track_id
-        self.track_id += 1
-
-      # Parse track data
-      self.pts[i].dRel = msg_a['LongDist']
-      self.pts[i].yRel = msg_a['LatDist']
-      self.pts[i].vRel = msg_a['LongSpeed']
-      self.pts[i].aRel = msg_a['LongAccel']
-      self.pts[i].yvRel = msg_b['LatSpeed']
-      self.pts[i].measured = bool(msg_a['Meas'])
-
-    ret.points = list(self.pts.values())
-    self.updated_messages.clear()
-    return ret
+  pass
@@ -17,6 +17,15 @@ class TeslaCAN:
    ret += sum(dat)
    return ret & 0xFF

+  @staticmethod
+  def right_stalk_crc(dat):
+    right_stalk_val = [0x7C, 0xB6, 0xF0, 0x2F, 0x69, 0xA3, 0xDD, 0x1C, 0x56, 0x90, 0xCA, 0x09, 0x43, 0x7D, 0xB7, 0xF1]
+    cntr = dat[0] & 0xF
+    crc1_func = crcmod.mkCrcFun(0x12F, initCrc=0x00, xorOut=0xFF, rev=False)
+    crc1 = crc1_func(dat) & 0xFF
+    crc2_func = crcmod.mkCrcFun(0x12F, initCrc=crc1, xorOut=0xFF, rev=False)
+    return crc2_func(bytes([right_stalk_val[cntr]])) & 0xFF
+
  def create_steering_control(self, angle, enabled, counter):
    values = {
      "DAS_steeringAngleRequest": -angle,
@@ -25,56 +34,11 @@ class TeslaCAN:
      "DAS_steeringControlCounter": counter,
    }

-    data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[1]
+    data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)[1]
    values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
-    return self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)
-
-  def create_action_request(self, msg_stw_actn_req, cancel, bus, counter):
-    # We copy this whole message when spamming cancel
-    values = {s: msg_stw_actn_req[s] for s in [
-      "SpdCtrlLvr_Stat",
-      "VSL_Enbl_Rq",
-      "SpdCtrlLvrStat_Inv",
-      "DTR_Dist_Rq",
-      "TurnIndLvr_Stat",
-      "HiBmLvr_Stat",
-      "WprWashSw_Psd",
-      "WprWash_R_Sw_Posn_V2",
-      "StW_Lvr_Stat",
-      "StW_Cond_Flt",
-      "StW_Cond_Psd",
-      "HrnSw_Psd",
-      "StW_Sw00_Psd",
-      "StW_Sw01_Psd",
-      "StW_Sw02_Psd",
-      "StW_Sw03_Psd",
-      "StW_Sw04_Psd",
-      "StW_Sw05_Psd",
-      "StW_Sw06_Psd",
-      "StW_Sw07_Psd",
-      "StW_Sw08_Psd",
-      "StW_Sw09_Psd",
-      "StW_Sw10_Psd",
-      "StW_Sw11_Psd",
-      "StW_Sw12_Psd",
-      "StW_Sw13_Psd",
-      "StW_Sw14_Psd",
-      "StW_Sw15_Psd",
-      "WprSw6Posn",
-      "MC_STW_ACTN_RQ",
-      "CRC_STW_ACTN_RQ",
-    ]}
-
-    if cancel:
-      values["SpdCtrlLvr_Stat"] = 1
-      values["MC_STW_ACTN_RQ"] = counter
-
-    data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[1]
-    values["CRC_STW_ACTN_RQ"] = self.crc(data[:7])
-    return self.packer.make_can_msg("STW_ACTN_RQ", bus, values)
+    return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)

  def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt):
-    messages = []
    values = {
      "DAS_setSpeed": speed * CV.MS_TO_KPH,
      "DAS_accState": acc_state,
@@ -86,9 +50,20 @@ class TeslaCAN:
      "DAS_controlCounter": cnt,
      "DAS_controlChecksum": 0,
    }
+    data = self.packer.make_can_msg("DAS_control", CANBUS.party, values)[1]
+    values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
+    return self.packer.make_can_msg("DAS_control", CANBUS.party, values)

-    for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]:
-      data = packer.make_can_msg("DAS_control", bus, values)[1]
-      values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
-      messages.append(packer.make_can_msg("DAS_control", bus, values))
-    return messages
+  def right_stalk_press(self, counter, position):
+    values = {
+      "SCCM_rightStalkCrc": 0,
+      "SCCM_rightStalkCounter": counter,
+      "SCCM_rightStalkStatus": position,
+      "SCCM_rightStalkReserved1": 0,
+      "SCCM_parkButtonStatus": 0,
+      "SCCM_rightStalkReserved2": 0,
+    }
+
+    data = self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values)[1]
+    values["SCCM_rightStalkCrc"] = self.right_stalk_crc(data[1:])
+    return self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values)
@@ -9,32 +9,22 @@ Ecu = car.CarParams.Ecu

 Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])

+
 class CAR(Platforms):
-  TESLA_AP1_MODELS = PlatformConfig(
-    [CarDocs("Tesla AP1 Model S", "All")],
-    CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0),
-    dbc_dict('tesla_powertrain', 'tesla_radar_bosch_generated', chassis_dbc='tesla_can')
+  TESLA_AP3_MODEL3 = PlatformConfig(
+    [CarDocs("Tesla AP3 Model 3", "All")],
+    CarSpecs(mass=1899., wheelbase=2.875, steerRatio=12.0),
+    dbc_dict('tesla_model3_vehicle', None, chassis_dbc='tesla_model3_party')
  )
-  TESLA_AP2_MODELS = PlatformConfig(
-    [CarDocs("Tesla AP2 Model S", "All")],
-    TESLA_AP1_MODELS.specs,
-    TESLA_AP1_MODELS.dbc_dict
-  )
-  TESLA_MODELS_RAVEN = PlatformConfig(
-    [CarDocs("Tesla Model S Raven", "All")],
-    TESLA_AP1_MODELS.specs,
-    dbc_dict('tesla_powertrain', 'tesla_radar_continental_generated', chassis_dbc='tesla_can')
+  TESLA_AP3_MODELY = PlatformConfig(
+    [CarDocs("Tesla AP3 Model Y", "All")],
+    CarSpecs(mass=2072., wheelbase=2.890, steerRatio=12.0),
+    dbc_dict('tesla_model3_vehicle', None, chassis_dbc='tesla_model3_party')
  )

+
 FW_QUERY_CONFIG = FwQueryConfig(
  requests=[
-    Request(
-      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
-      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
-      whitelist_ecus=[Ecu.eps],
-      rx_offset=0x08,
-      bus=0,
-    ),
    Request(
      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST],
      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE],
@@ -45,23 +35,20 @@ FW_QUERY_CONFIG = FwQueryConfig(
    Request(
      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
-      whitelist_ecus=[Ecu.adas, Ecu.electricBrakeBooster, Ecu.fwdRadar],
+      whitelist_ecus=[Ecu.engine],
      rx_offset=0x10,
-      bus=0,
+      bus=1,
+      obd_multiplexing=False,
    ),
  ]
 )

-class CANBUS:
-  # Lateral harness
-  chassis = 0
-  radar = 1
-  autopilot_chassis = 2

-  # Longitudinal harness
-  powertrain = 4
-  private = 5
-  autopilot_powertrain = 6
+class CANBUS:
+  party = 0
+  vehicle = 1
+  autopilot_party = 2
+

 GEAR_MAP = {
  "DI_GEAR_INVALID": car.CarState.GearShifter.unknown,
@@ -72,18 +59,18 @@ GEAR_MAP = {
  "DI_GEAR_SNA": car.CarState.GearShifter.unknown,
 }

-DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"]
-
-# Make sure the message and addr is also in the CAN parser!
 BUTTONS = [
-  Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]),
-  Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]),
-  Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]),
-  Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]),
-  Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]),
-  Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]),
+  Button(car.CarState.ButtonEvent.Type.leftBlinker, "SCCM_leftStalk", "SCCM_turnIndicatorStalkStatus", [3, 4]),
+  Button(car.CarState.ButtonEvent.Type.rightBlinker, "SCCM_leftStalk", "SCCM_turnIndicatorStalkStatus", [1, 2]),
+  Button(car.CarState.ButtonEvent.Type.accelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(1, 10))),
+  Button(car.CarState.ButtonEvent.Type.decelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(-9, 0))),
+  Button(car.CarState.ButtonEvent.Type.cancel, "SCCM_rightStalk", "SCCM_rightStalkStatus", [1]),
+  Button(car.CarState.ButtonEvent.Type.resumeCruise, "SCCM_rightStalk", "SCCM_rightStalkStatus", [4]),
+  Button(car.CarState.ButtonEvent.Type.altButton1, "SCCM_rightStalk", "SCCM_rightStalkStatus", [3]),  # MADS engaged
+  Button(car.CarState.ButtonEvent.Type.altButton2, "SCCM_rightStalk", "SCCM_rightStalkStatus", [2]),  # MADS disengaged
 ]

+
 class CarControllerParams:
  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 1.6, .3])
  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 7.0, 0.8])
@@ -17,6 +17,7 @@ from openpilot.selfdrive.car.body.values import CAR as COMMA

 # TODO: add routes for these cars
 non_tested_cars = [
+  FORD.FORD_F_150_MK14,
  GM.CADILLAC_ATS,
  GM.HOLDEN_ASTRA,
  GM.CHEVROLET_MALIBU,
@@ -53,7 +54,6 @@ routes = [
  CarTestRoute("e886087f430e7fe7|2023-06-16--23-06-36", FORD.FORD_FOCUS_MK4),
  CarTestRoute("bd37e43731e5964b|2023-04-30--10-42-26", FORD.FORD_MAVERICK_MK1),
  CarTestRoute("112e4d6e0cad05e1|2023-11-14--08-21-43", FORD.FORD_F_150_LIGHTNING_MK1),
-  CarTestRoute("24574459dd7fb3e0|2023-11-06--06-23-44", FORD.FORD_F_150_MK14),
  CarTestRoute("83a4e056c7072678|2023-11-13--16-51-33", FORD.FORD_MUSTANG_MACH_E_MK1),
  CarTestRoute("37998aa0fade36ab/00000000--48f927c4f5", FORD.FORD_RANGER_MK2),
  #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION),
@@ -16,9 +16,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan]

 # Tesla has high torque
-"TESLA_AP1_MODELS" = [nan, 2.5, nan]
-"TESLA_AP2_MODELS" = [nan, 2.5, nan]
-"TESLA_MODELS_RAVEN" = [nan, 2.5, nan]
+"TESLA_AP3_MODEL3" = [nan, 2.5, nan]
+"TESLA_AP3_MODELY" = [nan, 2.5, nan]

 # Guess
 "FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan]
@@ -86,7 +86,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "KIA_FORTE_2021_NON_SCC" = "HYUNDAI_SONATA"
 "KIA_SELTOS_2023_NON_SCC" = "HYUNDAI_SONATA"
 "HYUNDAI_KONA_NON_SCC" = "HYUNDAI_KONA_EV"
-"HYUNDAI_KONA_EV_NON_SCC" = "HYUNDAI_KONA_EV"
 "HYUNDAI_ELANTRA_2022_NON_SCC" = "HYUNDAI_ELANTRA_2021"
 "GENESIS_G70_2021_NON_SCC" = "HYUNDAI_SONATA"
 "KIA_CEED_PHEV_2022_NON_SCC" = "HYUNDAI_SONATA"
@@ -1,7 +1,4 @@
 from cereal import car
-import math
-from openpilot.common.params import Params
-from openpilot.selfdrive.controls.lib.pid import PIDController
 from common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg, make_tester_present_msg, \
@@ -16,7 +13,6 @@ from opendbc.can.packer import CANPacker
 GearShifter = car.CarState.GearShifter
 SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
-#LongCtrlState = car.CarControl.Actuators.LongControlState

 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -48,11 +44,8 @@ class CarController(CarControllerBase):
    self.last_standstill = False
    self.standstill_req = False
    self.steer_rate_counter = 0
-    #self.pcm_accel_comp = 0
    self.distance_button = 0

-    #self.pid = PIDController(k_p=1.0, k_i=0.25, k_f=0)
-
    self.packer = CANPacker(dbc_name)
    self.gas = 0
    self.accel = 0
@@ -62,13 +55,6 @@ class CarController(CarControllerBase):
    self.last_blindspot_frame = 0

    self._auto_lock_speed = 0.0
-
-    if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
-      self.brake_hold_active: bool = False
-      self._brake_hold_counter: int = 0
-      self._brake_hold_reset: bool = False
-      self._prev_brake_pressed: bool = False
-
    self._auto_lock_once = False
    self._gear_prev = GearShifter.park

@@ -147,41 +133,6 @@ class CarController(CarControllerBase):
                                                          lta_active, self.frame // 2, torque_wind_down))

    # *** gas and brake ***
-    sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
-
-    # When sp_tss2_long_tune is True and CC.longActive
-    #if sp_tss2_long_tune:
-      # we will throw out PCM's compensations, but that may be a good thing. for example:
-      # we lose things like pitch compensation, gas to maintain speed, brake to compensate for creeping, etc.
-      # but also remove undesirable "snap to standstill" behavior when not requesting enough accel at low speeds,
-      # lag to start moving, lag to start braking, etc.
-      # PI should compensate for lack of the desirable behaviors, but might be worse than the PCM doing them
-
-      # FIXME? neutral force will only be positive under ~5 mph, which messes up stopping control considerably
-      # not sure why this isn't captured in the PCM accel net, maybe that just ignores creep force + high speed deceleration
-      # it also doesn't seem to capture slightly more braking on downhills (VSC1S07->ASLP (pitch, deg.) might have some clues)
-    #  offset = min(CS.pcm_neutral_force / self.CP.mass, 0.0)
-    #  pitch_offset = math.sin(math.radians(CS.vsc_slope_angle)) * 9.81  # downhill is negative
-      # TODO: these limits are too slow to prevent a jerk when engaging, ramp down on engage?
-      # self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.05, self.pcm_accel_comp + 0.05)
-    #  pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_calc_accel_net)
-    #  self.pcm_accel_comp = clip(pcm_accel_comp, self.pcm_accel_comp - 0.005, self.pcm_accel_comp + 0.005)
-    #  if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping:
-    #    self.pcm_accel_comp = 0.0
-    #    self.pid.reset()
-    #  pcm_accel_cmd = actuators.accel + self.pcm_accel_comp # + offset
-      # pcm_accel_cmd = actuators.accel - pitch_offset
-
-    #  if not CC.longActive:
-    #   self.pid.reset()
-    #    self.pcm_accel_comp = 0.0
-    #    pcm_accel_cmd = 0.0
-
-    #  pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
-    #else:
-    #  pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
-
-
    if self.CP.enableGasInterceptorDEPRECATED and CC.longActive:
      MAX_INTERCEPTOR_GAS = 0.5
      # RAV4 has very sensitive gas pedal
@@ -198,6 +149,7 @@ class CarController(CarControllerBase):
    else:
      interceptor_gas_cmd = 0.
    pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+
    # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
    # than CS.cruiseState.enabled. confirm they're not meaningfully different
    if not (CC.enabled and CS.out.cruiseState.enabled) and CS.pcm_acc_status:
@@ -216,9 +168,6 @@ class CarController(CarControllerBase):

    self.last_standstill = CS.out.standstill

-    if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
-      can_sends.extend(self.create_auto_brake_hold_messages(CS))
-
    # handle UI messages
    fcw_alert = hud_control.visualAlert == VisualAlert.fcw
    steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
@@ -337,25 +286,3 @@ class CarController(CarControllerBase):
        # print("bsm poll right")

    return can_sends
-
-  # auto brake hold (https://github.com/AlexandreSato/)
-  def create_auto_brake_hold_messages(self, CS: car.CarState, brake_hold_allowed_timer: int = 100):
-    can_sends = []
-    disallowed_gears = [GearShifter.park, GearShifter.reverse]
-    brake_hold_allowed = CS.out.standstill and CS.out.cruiseState.available and not CS.out.gasPressed and \
-                         not CS.out.cruiseState.enabled and (CS.out.gearShifter not in disallowed_gears)
-
-    if brake_hold_allowed:
-      self._brake_hold_counter += 1
-      self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer and not self._brake_hold_reset
-      self._brake_hold_reset = not self._prev_brake_pressed and CS.out.brakePressed and not self._brake_hold_reset
-    else:
-      self._brake_hold_counter = 0
-      self.brake_hold_active = False
-      self._brake_hold_reset = False
-    self._prev_brake_pressed = CS.out.brakePressed
-
-    if self.frame % 2 == 0:
-      can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
-
-    return can_sends
@@ -1,19 +1,18 @@
 import copy

-from cereal import car, custom
+from cereal import car
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import mean
 from openpilot.common.filter_simple import FirstOrderFilter
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car import DT_CTRL
-from openpilot.common.params import Params
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.toyota.values import ToyotaFlags, ToyotaFlagsSP, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                  TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR

 SteerControlType = car.CarParams.SteerControlType
-AccelPersonality = custom.AccelerationPersonality
+
 # These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
 # - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds
 # - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3.
@@ -57,11 +56,6 @@ class CarState(CarStateBase):
    self.low_speed_lockout = False
    self.acc_type = 1
    self.lkas_hud = {}
-    #self.pcm_accel_net = 0.0
-    #self.pcm_true_accel_net = 0.0
-    #self.pcm_calc_accel_net = 0.0
-    #self.pcm_neutral_force = 0.0
-    #self.vsc_slope_angle = 0.0

    self.lkas_enabled = None
    self.prev_lkas_enabled = None
@@ -85,17 +79,6 @@ class CarState(CarStateBase):
    self._right_blindspot_d2 = 0
    self._right_blindspot_counter = 0

-    self.signals_checked = False
-    self.sport_signal_seen = False
-    self.eco_signal_seen = False
-    self.accel_profile = None
-    self.prev_accel_profile = None
-    self.accel_profile_init = False
-    self.toyota_drive_mode = Params().get_bool('ToyotaDriveMode')
-
-    if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
-      self.pre_collision_2 = {}
-
    self.frame = 0

  def update(self, cp, cp_cam):
@@ -131,14 +114,6 @@ class CarState(CarStateBase):
    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
    ret.vEgoCluster = ret.vEgo * 1.015  # minimum of all the cars

-    # thought to be the gas/brake as issued by the pcm (0=coasting)
-    #self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]  # this is only accurate for braking * 43
-    #self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]  # this is only accurate for acceleration * 78
-    #self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43
-    #self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]
-    #self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
-    #self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"]
-
    ret.standstill = abs(ret.vEgoRaw) < 1e-3

    if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
@@ -201,51 +176,6 @@ class CarState(CarStateBase):
    ret.leftBlinker = ret.leftBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
    ret.rightBlinker = ret.rightBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2

-    if self.toyota_drive_mode:
-      # Determine sport signal based on car model
-      sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2, CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON'
-
-      # Check signals once
-      if not self.signals_checked:
-        self.signals_checked = True
-
-        # Try to detect sport mode signal, handle missing signal with a fallback
-        try:
-          sport_mode = cp.vl["GEAR_PACKET"][sport_signal]
-          self.sport_signal_seen = True
-        except KeyError:
-          sport_mode = 0
-          self.sport_signal_seen = False
-
-        # Try to detect eco mode signal, handle missing signal with a fallback
-        try:
-          eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON']
-          self.eco_signal_seen = True
-        except KeyError:
-          eco_mode = 0
-          self.eco_signal_seen = False
-      else:
-        # Always re-check the signals to account for mode changes
-        sport_mode = cp.vl["GEAR_PACKET"][sport_signal] if self.sport_signal_seen else 0
-        eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON'] if self.eco_signal_seen else 0
-
-      # Set acceleration profile based on detected modes, with sport mode having higher priority
-      if sport_mode == 1:
-        self.accel_profile = AccelPersonality.sport
-      elif eco_mode == 1:
-        self.accel_profile = AccelPersonality.eco
-      else:
-        self.accel_profile = AccelPersonality.normal
-
-      print(f"Accel profile set to: {self.accel_profile}")
-
-      # If not initialized, sync profile with the current mode on the car
-      if not self.accel_profile_init or self.accel_profile != self.prev_accel_profile:
-        Params().put_nonblocking('AccelPersonality', str(self.accel_profile))
-        self.accel_profile_init = True
-        # Update the previous profile to prevent unnecessary re-syncing
-        self.prev_accel_profile = self.accel_profile
-
    if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
      ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]

@@ -330,9 +260,6 @@ class CarState(CarStateBase):
    if self.CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM and self.frame > 199:
      ret.leftBlindspot, ret.rightBlindspot = self.sp_get_enhanced_bsm(cp)

-    if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
-      self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
-
    self._update_traffic_signals(cp_cam)
    ret.cruiseState.speedLimit = self._calculate_speed_limit()
    self.frame += 1
@@ -464,16 +391,12 @@ class CarState(CarStateBase):
      ("BODY_CONTROL_STATE_2", 2),
      ("ESP_CONTROL", 3),
      ("EPS_STATUS", 25),
-      #("GEAR_PACKET_HYBRID", 60),
-      #("BRAKE", 80),
      ("BRAKE_MODULE", 40),
      ("WHEEL_SPEEDS", 80),
      ("STEER_ANGLE_SENSOR", 80),
      ("PCM_CRUISE", 33),
      ("PCM_CRUISE_SM", 1),
-      #("VSC1S07", 20),
      ("STEER_TORQUE_SENSOR", 50),
-      #("CLUTCH", 16),
    ]

    if CP.carFingerprint != CAR.TOYOTA_MIRAI:
@@ -540,7 +463,4 @@ class CarState(CarStateBase):
        ("PCS_HUD", 1),
      ]

-      if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
-        messages.append(("PRE_COLLISION_2", 33))
-
    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
@@ -160,16 +160,16 @@ class CarInterface(CarInterfaceBase):

    sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")

-    # hand tuned (August 12, 2024)
+    # hand tuned (July 1, 2024)
    def custom_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.25
+      ret.vEgoStopping = 0.01
      ret.vEgoStarting = 0.01
-      ret.stoppingDecelRate = 0.006
+      ret.stoppingDecelRate = 0.35

    def default_tss2_longitudinal_tuning():
      ret.vEgoStopping = 0.25
      ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.002  # reach stopping target smoothly
+      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly

    def default_longitudinal_tuning():
      tune.kiBP = [0., 5., 35.]
@@ -178,8 +178,8 @@ class CarInterface(CarInterfaceBase):
    tune = ret.longitudinalTuning
    if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED:
      if sp_tss2_long_tune:
-        tune.kiBP = [0.,  5.,   12.,  20.,  27.,  36.,  40.]
-        tune.kiV = [0.34, 0.234, 0.20, 0.17, 0.105, 0.09, 0.08]
+        tune.kiBP = [0., 5., 12., 20., 27., 36., 50]
+        tune.kiV = [0.35, 0.23, 0.20, 0.17, 0.10, 0.07, 0.01]
        custom_tss2_longitudinal_tuning()
      else:
        tune.kpV = [0.0]
@@ -195,9 +195,6 @@ class CarInterface(CarInterfaceBase):
    if candidate == CAR.TOYOTA_PRIUS_TSS2:
      ret.spFlags |= ToyotaFlagsSP.SP_NEED_DEBUG_BSM.value

-    if Params().get_bool("ToyotaAutoHold") and candidate in (TSS2_CAR - RADAR_ACC_CAR):
-      ret.spFlags |= ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD.value
-
    return ret

  @staticmethod
@@ -211,10 +208,13 @@ class CarInterface(CarInterfaceBase):
  def _update(self, c):
    ret = self.CS.update(self.cp, self.cp_cam)

+    distance_button = 0
+
    if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
      self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
+      distance_button = self.CS.distance_button

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

    if ret.cruiseState.available:
      if self.enable_mads:
@@ -229,16 +229,16 @@ class CarInterface(CarInterfaceBase):
            if (not self.CS.prev_lkas_enabled and self.CS.lkas_enabled) or \
              (self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled):
              self.CS.madsEnabled = not self.CS.madsEnabled
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False

    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      if not self.CP.pcmCruise:
        ret.cruiseState.enabled = self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -250,7 +250,7 @@ class CarInterface(CarInterfaceBase):
    events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
                                       pcm_enable=False)

-    events, ret = self.create_sp_events(ret, events)
+    events, ret = self.create_sp_events(self.CS, ret, events)

    # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
    # the more accurate angle sensor signal is initialized
@@ -271,11 +271,6 @@ class CarInterface(CarInterfaceBase):
          # while in standstill, send a user alert
          events.add(EventName.manualRestart)

-    # auto brake hold
-    if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
-      if self.CC.brake_hold_active and not ret.brakeHoldActive:
-        events.add(EventName.spAutoBrakeHold)
-
    ret.events = events.to_msg()

    return ret
@@ -130,37 +130,3 @@ def create_set_bsm_debug_mode(lr_blindspot, enabled):

 def create_bsm_polling_status(lr_blindspot):
  return make_can_msg(0x750, lr_blindspot + b"\x02\x21\x69\x00\x00\x00\x00", 0)
-
-
-# auto brake hold
-def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
-  # forward PRE_COLLISION_2 when auto brake hold is not active
-  values = {s: pre_collision_2[s] for s in [
-    "DSS1GDRV",
-    "DS1STAT2",
-    "DS1STBK2",
-    "PCSWAR",
-    "PCSALM",
-    "PCSOPR",
-    "PCSABK",
-    "PBATRGR",
-    "PPTRGR",
-    "IBTRGR",
-    "CLEXTRGR",
-    "IRLT_REQ",
-    "BRKHLD",
-    "AVSTRGR",
-    "VGRSTRGR",
-    "PREFILL",
-    "PBRTRGR",
-    "PCSDIS",
-    "PBPREPMP",
-  ]}
-
-  if brake_hold_active:
-    values = {
-      "DSS1GDRV": 0x3FF,
-      "PBRTRGR": frame % 730 < 727,  # cut actuation for 3 frames
-    }
-
-  return packer.make_can_msg("PRE_COLLISION_2", 0, values)
@@ -16,7 +16,7 @@ PEDAL_TRANSITION = 10. * CV.MPH_TO_MS


 class CarControllerParams:
-  ACCEL_MAX = 2.0  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
+  ACCEL_MAX = 1.5  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
  ACCEL_MIN = -3.5  # m/s2

  STEER_STEP = 1
@@ -64,7 +64,6 @@ class ToyotaFlagsSP(IntFlag):
  SP_ZSS = 1
  SP_ENHANCED_BSM = 2
  SP_NEED_DEBUG_BSM = 4
-  SP_AUTO_BRAKE_HOLD = 8


 class Footnote(Enum):
@@ -114,25 +114,26 @@ class CarInterface(CarInterfaceBase):
  def _update(self, c):
    ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)

-    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)

-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+                                                            self.CS.button_events, c.vCruise,
                                                            enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))

    if ret.cruiseState.available:
      if self.enable_mads:
        if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
          self.CS.madsEnabled = True
-        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False
-    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS.madsEnabled)
+    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)

    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
-        self.get_sp_cancel_cruise_state()
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
    if self.get_sp_pedal_disengage(ret):
-      self.get_sp_cancel_cruise_state()
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled

    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -140,7 +141,8 @@ class CarInterface(CarInterfaceBase):
        self.CS.accEnabled = False
      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled

-    ret = self.get_sp_common_state(ret)
+    ret, self.CS = self.get_sp_common_state(ret, self.CS,
+                                            gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events))

    ret.buttonEvents = [
      *self.CS.button_events,
@@ -151,7 +153,7 @@ class CarInterface(CarInterfaceBase):
                                       pcm_enable=False,
                                       enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))

-    events, ret = self.create_sp_events(ret, events,
+    events, ret = self.create_sp_events(self.CS, ret, events,
                                        enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))

    # Low speed steer alert hysteresis logic
@@ -171,7 +173,9 @@ class CarInterface(CarInterfaceBase):
    if self.CC.eps_timer_soft_disable_alert:
      events.add(EventName.steerTimeLimit)

-    ret.customStockLong = self.update_custom_stock_long()
+    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+                                                           self.CC.target_speed, self.CC.v_set_dis,
+                                                           self.CC.speed_diff, self.CC.button_type)

    ret.events = events.to_msg()

@@ -20,7 +20,7 @@ from openpilot.common.swaglog import cloudlog

 from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event
 from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
-from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature, CRUISE_LONG_PRESS
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature
 from openpilot.selfdrive.controls.lib.events import Events, ET
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -174,27 +174,17 @@ class Controls:

    self.live_torque = self.params.get_bool("LiveTorque")
    self.torqued_override = self.params.get_bool("TorquedOverride")
-    self.custom_stock_planner_speed = self.params.get_bool("CustomStockLongPlanner")

    self.enable_mads = self.params.get_bool("EnableMads")
    self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake")
    self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
-    self.pcm_v_cruise_override = self.params.get_bool("PCMVCruiseOverride")
-    self.pcm_v_cruise_override_speed = int(self.params.get("PCMVCruiseOverrideSpeed", encoding="utf-8"))
    self.process_not_running = False
-    self.experimental_mode_update = False

    self.custom_model_metadata = CustomModelMetadata(params=self.params, init_only=True)
    self.model_use_lateral_planner = self.custom_model_metadata.valid and \
                                     self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution

    self.dynamic_personality = self.params.get_bool("DynamicPersonality")
-    self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist")
-    self.overtaking_accel_engaged = False
-    self.prev_overtaking_accel_engaged = False
-    self.overtaking_accel_allowed = False
-    self.prev_overtaking_accel_allowed = False
-    self.overtaking_accel_blocked = False

    self.accel_personality = self.read_accel_personality_param()

@@ -499,9 +489,7 @@ class Controls:
  def state_transition(self, CS):
    """Compute conditional state transitions and execute actions on state transitions"""

-    # sp - PCM speed override
-    sp_override_speed = self.pcm_v_cruise_override_speed if self.pcm_v_cruise_override else False
-    self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, sp_override_speed, self.sm['longitudinalPlanSP'])
+    self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, self.sm['longitudinalPlanSP'])

    # decrement the soft disable timer at every step, as it's reset on
    # entrance in SOFT_DISABLING state
@@ -650,15 +638,9 @@ class Controls:
      self.LoC.reset()

    if not self.joystick_mode:
-      speeds = long_plan.speeds
-      a_lead = self.sm['radarState'].leadOne.aLeadK
-      resume = False
-      if len(speeds):
-        resume = self.enabled_long and CS.standstill and self.CP.carName == "hyundai" and speeds[-1] > 0.1 and a_lead > 0.1
-
      # accel PID loop
      pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
-      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, resume)
+      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)

      # Steering PID loop and lateral MPC
      if self.model_use_lateral_planner:
@@ -735,21 +717,11 @@ class Controls:
        cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
        setattr(actuators, p, 0.0)

-    # toggle experimental mode once on distance button hold
-    if self.CP.openpilotLongitudinalControl:
-      if self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == CRUISE_LONG_PRESS and \
-            not self.experimental_mode_update:
-        self.experimental_mode = not self.experimental_mode
-        self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
-        self.experimental_mode_update = True
-
    # decrement personality on distance button press
    if self.CP.openpilotLongitudinalControl:
      if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
-        if not self.experimental_mode_update:
-          self.personality = (self.personality - 1) % 3
-          self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
-        self.experimental_mode_update = False
+        self.personality = (self.personality - 1) % 3
+        self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))

    return CC, lac_log

@@ -834,36 +806,11 @@ class Controls:

    # Curvature & Steering angle
    lp = self.sm['liveParameters']
-    dh = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2'
+    lp_mono_time_svs = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2'

    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)

-    lat_plan = self.sm['lateralPlanDEPRECATED']
-    long_plan = self.sm['longitudinalPlan']
-    dm_state = self.sm['driverMonitoringState']
-    blinker_svs = lat_plan if self.model_use_lateral_planner else model_v2.meta
-    self.prev_overtaking_accel_allowed = self.overtaking_accel_allowed
-    if not self.overtaking_accel_allowed and not self.prev_overtaking_accel_allowed:
-      self.overtaking_accel_blocked = False
-    self.overtaking_accel_allowed = ((blinker_svs.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or
-                                     (blinker_svs.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \
-                                    (blinker_svs.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting)) and \
-                                    not self.overtaking_accel_blocked
-    self.prev_overtaking_accel_engaged = self.overtaking_accel_engaged
-    ttc = self.sm['radarState'].leadOne.dRel / CS.vEgo if CS.vEgo > 0 else 255
-    overtaking_accel_engaged = self.overtaking_accel and self.overtaking_accel_allowed and \
-                               (CS.vEgo > ((60 * CV.KPH_TO_MS) if self.is_metric else (40 * CV.MPH_TO_MS))) and \
-                               not (CS.leftBlinker and CS.rightBlinker)
-    if ttc < 0.75 and self.prev_overtaking_accel_engaged and overtaking_accel_engaged:
-      overtaking_accel_engaged = False
-      self.overtaking_accel_blocked = True
-    if overtaking_accel_engaged and not self.prev_overtaking_accel_engaged and \
-      long_plan.hasLead and long_plan.aTarget > -0.1 and (0.75 < ttc < 3.0):
-      self.overtaking_accel_engaged = True
-    elif not overtaking_accel_engaged:
-      self.overtaking_accel_engaged = False
-
    # controlsState
    dat = messaging.new_message('controlsState')
    dat.valid = CS.canValid
@@ -878,7 +825,7 @@ class Controls:
      controlsState.alertSound = current_alert.audible_alert

    controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
-    controlsState.lateralPlanMonoTime = self.sm.logMonoTime[dh]
+    controlsState.lateralPlanMonoTime = self.sm.logMonoTime[lp_mono_time_svs]
    controlsState.enabled = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.enabled or CS.cruiseState.enabled) and CS.gearShifter not in [GearShifter.park, GearShifter.reverse]
    controlsState.active = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.active or CS.cruiseState.enabled)
    controlsState.curvature = curvature
@@ -916,7 +863,6 @@ class Controls:
    controlsStateSP.personality = self.personality
    controlsStateSP.dynamicPersonality = self.dynamic_personality
    controlsStateSP.accelPersonality = self.accel_personality
-    controlsStateSP.overtakingAccelerationAssist = self.overtaking_accel_engaged

    if self.enable_nnff and lat_tuning == 'torque':
      controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp
@@ -974,8 +920,7 @@ class Controls:
  def params_thread(self, evt):
    while not evt.is_set():
      self.is_metric = self.params.get_bool("IsMetric")
-      self.experimental_mode = self.params.get_bool("ExperimentalMode") and (self.CP.openpilotLongitudinalControl or
-                                                                             (not self.CP.pcmCruiseSpeed and self.custom_stock_planner_speed))
+      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
      self.personality = self.read_personality_param()
      self.dynamic_personality = self.params.get_bool("DynamicPersonality")
      self.accel_personality = self.read_accel_personality_param()
@@ -984,11 +929,9 @@ class Controls:

      self.reverse_acc_change = self.params.get_bool("ReverseAccChange")
      self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
-      self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist")

      if self.sm.frame % int(2.5 / DT_CTRL) == 0:
        self.live_torque = self.params.get_bool("LiveTorque")
-        self.custom_stock_planner_speed = self.params.get_bool("CustomStockLongPlanner")
      time.sleep(0.1)

  def controlsd_thread(self):
@@ -65,10 +65,6 @@ VOLKSWAGEN_V_CRUISE_MIN = {
  True: 30,
  False: int(20 * CV.MPH_TO_KPH),
 }
-GM_V_CRUISE_MIN = {
-  True: 30,
-  False: int(20 * CV.MPH_TO_KPH),
-}

 SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState

@@ -79,7 +75,7 @@ class VCruiseHelper:
    self.v_cruise_kph = V_CRUISE_UNSET
    self.v_cruise_cluster_kph = V_CRUISE_UNSET
    self.v_cruise_kph_last = 0
-    self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0}
+    self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
    self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}

    self.is_metric_prev = None
@@ -88,20 +84,12 @@ class VCruiseHelper:
    self.slc_state_prev = SpeedLimitControlState.inactive
    self.slc_speed_limit_offsetted = 0

-    # sp: PCM speed override
-    self.sp_override_v_cruise_kph = V_CRUISE_UNSET
-    self.sp_override_cruise_speed_last = V_CRUISE_UNSET
-    self.sp_override_enabled_last = False
-
-    self.experimental_mode_update = False
-
  @property
  def v_cruise_initialized(self):
    return self.v_cruise_kph != V_CRUISE_UNSET

-  def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, sp_override_speed, long_plan_sp):
+  def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp):
    self.v_cruise_kph_last = self.v_cruise_kph
-    self.slc_state_prev = self.slc_state
    self.slc_state = long_plan_sp.speedLimitControlState

    if not self.CP.pcmCruiseSpeed:
@@ -113,33 +101,13 @@ class VCruiseHelper:
        self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_acc)
        self._update_v_cruise_slc(long_plan_sp)
        self.v_cruise_cluster_kph = self.v_cruise_kph
+        self.update_button_timers(CS, enabled)
      else:
-        if enabled and sp_override_speed and CS.cruiseState.speed * CV.MS_TO_KPH < sp_override_speed:
-          if self.sp_override_v_cruise_kph == V_CRUISE_UNSET:
-            self.sp_override_v_cruise_kph = max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_MIN)
-        else:
-          self.sp_override_v_cruise_kph = V_CRUISE_UNSET
-
-        # when we have an override_speed, use it
-        if self.sp_override_v_cruise_kph != V_CRUISE_UNSET:
-          self.v_cruise_kph = self.sp_override_v_cruise_kph
-          self.v_cruise_cluster_kph = self.sp_override_v_cruise_kph
-        else:
-          self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
-          self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
-
-          #print("sp_override_v_cruise_kph:", self.sp_override_v_cruise_kph)
-          #print("v_cruise_kph:", self.v_cruise_kph)
-          #print("v_cruise_cluster_kph:", self.v_cruise_cluster_kph)
-
-        self.sp_override_cruise_speed_last = CS.cruiseState.speed
-        self.sp_override_enabled_last = enabled
-      self.update_button_timers(CS, enabled)
+        self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+        self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
    else:
-      self.sp_override_v_cruise_kph = V_CRUISE_UNSET
      self.v_cruise_kph = V_CRUISE_UNSET
      self.v_cruise_cluster_kph = V_CRUISE_UNSET
-      self.experimental_mode_update = False

  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, reverse_acc):
    # handle button presses. TODO: this should be in state_control, but a decelCruise press
@@ -169,7 +137,7 @@ class VCruiseHelper:
          long_press = True
          break

-    if button_type is None or button_type == ButtonType.gapAdjustCruise:
+    if button_type is None:
      return

    resume_button = ButtonType.accelCruise
@@ -234,8 +202,6 @@ class VCruiseHelper:
        initial = MAZDA_V_CRUISE_MIN[is_metric]
      elif self.CP.carName == "volkswagen":
        initial = VOLKSWAGEN_V_CRUISE_MIN[is_metric]
-      elif self.CP.carName == "gm":
-        initial = GM_V_CRUISE_MIN[is_metric]

    # 250kph or above probably means we never had a set speed
    if any(b.type in resume_buttons for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
@@ -254,6 +220,8 @@ class VCruiseHelper:
    if self.slc_state == SpeedLimitControlState.active and self.slc_state_prev == SpeedLimitControlState.preActive:
      self.v_cruise_kph = clip(round(self.slc_speed_limit_offsetted, 1), self.v_cruise_min, V_CRUISE_MAX)

+    self.slc_state_prev = self.slc_state
+
  def _update_v_cruise_min(self, is_metric):
    if is_metric != self.is_metric_prev:
      if self.CP.carName == "honda":
@@ -266,8 +234,6 @@ class VCruiseHelper:
        self.v_cruise_min = MAZDA_V_CRUISE_MIN[is_metric]
      elif self.CP.carName == "volkswagen":
        self.v_cruise_min = VOLKSWAGEN_V_CRUISE_MIN[is_metric]
-      elif self.CP.carName == "gm":
-        self.v_cruise_min = GM_V_CRUISE_MIN[is_metric]
    self.is_metric_prev = is_metric


@@ -744,14 +744,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
    ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
  },

-  EventName.spAutoBrakeHold: {
-    ET.PERMANENT: Alert(
-      "sunnypilot Brake Hold Active",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOWEST, VisualAlert.none, AudibleAlert.prompt, 0.),
-  },
-
  EventName.parkBrake: {
    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
    ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
@@ -1149,9 +1141,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
    ET.PERMANENT: torque_nn_load_alert,
  },

-  EventName.hyundaiRadarTracksAvailable: {
-    ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize")
-  }
 }


@@ -83,8 +83,7 @@ class LatControlTorque(LatControl):
    self.torqued_override = self.param_s.get_bool("TorquedOverride")
    self._frame = 0

-    self.use_lateral_jerk = self.param_s.get_bool("TorqueLateralJerk")  # TODO: make this a parameter in the UI
-    self.nnff_no_lateral_jerk = self.param_s.get_bool("NNFFNoLateralJerk")  # TODO: make this a parameter in the UI
+    self.use_lateral_jerk = False  # TODO: make this a parameter in the UI

    # Twilsonco's Lateral Neural Network Feedforward
    self.use_nn = CI.has_lateral_torque_nn
@@ -141,13 +140,11 @@ class LatControlTorque(LatControl):
    if self._frame % 250 == 0:
      self._frame = 0
      self.torqued_override = self.param_s.get_bool("TorquedOverride")
-      self.use_lateral_jerk = self.param_s.get_bool("TorqueLateralJerk")
-      self.nnff_no_lateral_jerk = self.param_s.get_bool("NNFFNoLateralJerk")
      if not self.torqued_override:
        return

      self.torque_params.latAccelFactor = float(self.param_s.get("TorqueMaxLatAccel", encoding="utf8")) * 0.01
-      self.torque_params.friction = float(self.param_s.get("TorqueFriction", encoding="utf8")) * 0.001
+      self.torque_params.friction = float(self.param_s.get("TorqueFriction", encoding="utf8")) * 0.01

  @property
  def pid_long_sp(self):
@@ -200,7 +197,7 @@ class LatControlTorque(LatControl):
        predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs)
        desired_lateral_jerk = (interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, model_data.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time
        lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk)
-        if self.nnff_no_lateral_jerk or self.use_steering_angle or lookahead_lateral_jerk == 0.0:
+        if self.use_steering_angle or lookahead_lateral_jerk == 0.0:
          lookahead_lateral_jerk = 0.0
          actual_lateral_jerk = 0.0
          self.lat_accel_friction_factor = 1.0
@@ -209,7 +206,6 @@ class LatControlTorque(LatControl):

      if self.use_nn and model_good:
        # update past data
-        pitch = 0
        roll = params.roll
        if len(llk.calibratedOrientationNED.value) > 1:
          pitch = self.pitch.update(llk.calibratedOrientationNED.value[1])
@@ -235,14 +231,7 @@ class LatControlTorque(LatControl):
                                 + past_rolls + future_rolls
        torque_from_setpoint = self.torque_from_nn(nnff_setpoint_input)
        torque_from_measurement = self.torque_from_nn(nnff_measurement_input)
-
        pid_log.error = torque_from_setpoint - torque_from_measurement
-        error_blend_factor = interp(abs(desired_lateral_accel), [1.0, 2.0], [0.0, 1.0])
-        if error_blend_factor > 0.0:  # blend in stronger error response when in high lat accel
-          nnff_error_input = [CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0]
-          torque_from_error = self.torque_from_nn(nnff_error_input)
-          if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error):
-            pid_log.error = pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor

        # compute feedforward (same as nn setpoint output)
        error = setpoint - measurement
@@ -11,11 +11,11 @@ LongCtrlState = car.CarControl.Actuators.LongControlState


 def long_control_state_trans(CP, active, long_control_state, v_ego,
-                             should_stop, brake_pressed, cruise_standstill, resume):
+                             should_stop, brake_pressed, cruise_standstill):
  # Ignore cruise standstill if car has a gas interceptor
  cruise_standstill = cruise_standstill and not CP.enableGasInterceptorDEPRECATED
-  stopping_condition = should_stop and not resume
-  starting_condition = ((not should_stop or resume) and
+  stopping_condition = should_stop
+  starting_condition = (not should_stop and
                        not cruise_standstill and
                        not brake_pressed)
  started_condition = v_ego > CP.vEgoStarting
@@ -58,14 +58,14 @@ class LongControl:
  def reset(self):
    self.pid.reset()

-  def update(self, active, CS, a_target, should_stop, accel_limits, resume):
+  def update(self, active, CS, a_target, should_stop, accel_limits):
    """Update longitudinal control. This updates the state machine and runs a PID loop"""
    self.pid.neg_limit = accel_limits[0]
    self.pid.pos_limit = accel_limits[1]

    self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
                                                       should_stop, CS.brakePressed,
-                                                       CS.cruiseState.standstill, resume)
+                                                       CS.cruiseState.standstill)
    if self.long_control_state == LongCtrlState.off:
      self.reset()
      output_accel = 0.
@@ -3,7 +3,7 @@ import os
 import time
 import numpy as np
 from cereal import custom
-from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
@@ -55,7 +55,7 @@ T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
-STOP_DISTANCE = 5.0
+STOP_DISTANCE = 6.0

 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
  if personality==custom.LongitudinalPersonalitySP.relaxed:
@@ -63,116 +63,45 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
  elif personality==custom.LongitudinalPersonalitySP.standard:
    return 1.0
  elif personality==custom.LongitudinalPersonalitySP.moderate:
+    return 0.8
+  elif personality==custom.LongitudinalPersonalitySP.aggressive:
    return 0.6
-  elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 0.2
-  elif personality==custom.LongitudinalPersonalitySP.overtake:
-    return 0.1
  else:
    raise NotImplementedError("Longitudinal personality not supported")

-def get_a_change_factor(v_ego, v_lead0, v_lead1, personality=custom.LongitudinalPersonalitySP.standard):
-  # Set cost multipliers based on driving personality (relaxed, standard, moderate, aggressive).
-  # These values adjust the sensitivity of acceleration change.
-  # Higher value = more cautious (slower reaction), smaller value = quicker response (more aggressive driving)
-  if personality==custom.LongitudinalPersonalitySP.relaxed:
-    a_change_cost_multiplier_follow = 1.2  # Highest cost for changing acceleration, meaning more gradual transitions
-    a_change_cost_high_speed_factor = 2.0  # No extra penalty for high-speed changes (more cautious)
-  elif personality==custom.LongitudinalPersonalitySP.standard:
-    a_change_cost_multiplier_follow = 0.6  # Moderate cost for changing acceleration (quicker transitions compared to relaxed)
-    a_change_cost_high_speed_factor = 2.5  # Higher penalty for changes at higher speeds (more cautious)
-  elif personality==custom.LongitudinalPersonalitySP.moderate:
-    a_change_cost_multiplier_follow = 0.4  # Similar to standard (quicker transitions compared to relaxed)
-    a_change_cost_high_speed_factor = 3.0  # Similar to standard (higher penalty for high speeds)
-  elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    a_change_cost_multiplier_follow = 0.2  # Very low cost for changing acceleration, meaning quicker reactions (less cautious)
-    a_change_cost_high_speed_factor = 5.0  # Much higher penalty for abrupt changes at high speeds (very cautious at high speeds)
-  elif personality==custom.LongitudinalPersonalitySP.overtake:
-    a_change_cost_multiplier_follow = 0.1  # Very low cost for changing acceleration, meaning quicker reactions (less cautious)
-    a_change_cost_high_speed_factor = 5.0  # Much higher penalty for abrupt changes at high speeds (very cautious at high speeds)
-  else:
-    raise NotImplementedError("Longitudinal personality not supported")
-
-  # Variables to modify the acceleration change based on speed and lead vehicle conditions.
-  # LEAD_AUGMENTATION_BP_MAX defines the vEgo threshold for rapid acceleration.
-  LEAD_AUGMENTATION_BP_MAX = 5.  # Maximum speed (5 m/s ~ 18 km/h) where rapid acceleration adjustments are allowed
-
-  # LEAD_AUGMENTATION_BP: breakpoints for ego vehicle speed (vEgo) in m/s
-  # LEAD_AUGMENTATION_V: multiplier values for ego vehicle speed interpolation
-  LEAD_AUGMENTATION_BP = [0., LEAD_AUGMENTATION_BP_MAX]  # vEgo breakpoints: [0 m/s, 5 m/s (~18 km/h)]
-  LEAD_AUGMENTATION_V = [.05, 1.]  # acceleration multipliers: At 0 m/s, allow very small changes (.05), at 5 m/s allow faster acceleration (1.0)
-
-  # SPEED_AUGMENTATION_BP: breakpoints for speed adjustment to reduce abrupt braking at higher speeds
-  # SPEED_AUGMENTATION_V: interpolation values for scaling acceleration cost based on speed
-  # Higher = more cautious (penalizes abrupt braking), smaller = more aggressive (less penalty)
-  SPEED_AUGMENTATION_BP = [0., LEAD_AUGMENTATION_BP_MAX, 10.]  # Speed breakpoints: [0 m/s, 5 m/s, 10 m/s (~36 km/h)]
-  SPEED_AUGMENTATION_V = [1., 1., a_change_cost_high_speed_factor]  # Multiplier: between 0-5 m/s, no change (1.0), after 5 m/s, scale by a_change_cost_high_speed_factor (e.g., 1.5 in standard mode)
-
-  # Calculate a cost for acceleration changes when lead vehicles are pulling away and ego speed is below the threshold.
-  lead_augmented_a_change_cost = 1.0  # Default cost factor
-  if (v_lead0 - v_ego > 1e-3) and (v_lead1 - v_ego > 1e-3):
-    # Interpolate for the acceleration change cost when lead vehicles are increasing speed, based on vEgo
-    lead_augmented_a_change_cost = interp(v_ego, LEAD_AUGMENTATION_BP, LEAD_AUGMENTATION_V)
-
-  # Multiply the lead-based cost with speed-based cost to get a final cost factor, scaling with vehicle speed
-  speed_augmented_a_change_cost = a_change_cost_multiplier_follow * interp(v_ego, SPEED_AUGMENTATION_BP, SPEED_AUGMENTATION_V)
-
-  # Choose the smaller factor between the lead-based cost and the speed-based cost
-  a_change_factor = lead_augmented_a_change_cost if v_ego <= LEAD_AUGMENTATION_BP_MAX else speed_augmented_a_change_cost
-
-  # Return the final acceleration change factor to be applied
-  return a_change_factor
-
-# Function to return a multiplier for a danger zone cost based on driving personality
-def get_danger_zone_factor(personality=custom.LongitudinalPersonalitySP.standard):
-  # Higher values mean more cautious driving in dangerous situations, scaling the cost accordingly
-  if personality==custom.LongitudinalPersonalitySP.relaxed:
-    return 1.8  # Higher danger zone cost for relaxed personality (more cautious)
-  elif personality==custom.LongitudinalPersonalitySP.standard:
-    return 1.5  # Medium danger zone cost for standard personality
-  elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 1.2  # Medium danger zone cost for moderate personality (similar to standard)
-  elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 1.0  # Lowest danger zone cost for aggressive personality (less cautious)
-  elif personality==custom.LongitudinalPersonalitySP.overtake:
-    return 1.0  # Lowest danger zone cost for aggressive personality (less cautious)
-  else:
-    raise NotImplementedError("Longitudinal personality not supported")
-
-

 def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
  if personality==custom.LongitudinalPersonalitySP.relaxed:
    return 1.75
  elif personality==custom.LongitudinalPersonalitySP.standard:
-    return 1.65
-  elif personality==custom.LongitudinalPersonalitySP.moderate:
    return 1.45
-  elif personality==custom.LongitudinalPersonalitySP.aggressive:
+  elif personality==custom.LongitudinalPersonalitySP.moderate:
    return 1.25
-  elif personality==custom.LongitudinalPersonalitySP.overtake:
-    return 0.25
+  elif personality==custom.LongitudinalPersonalitySP.aggressive:
+    return 1.0
  else:
    raise NotImplementedError("Longitudinal personality not supported")


 def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.standard):
  if personality==custom.LongitudinalPersonalitySP.relaxed:
-    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.70, 1.70,  1.80,   1.80]
+    x_vel =  [0,    11,   14.5, 15,   20,   20.01,  25,    25.01,  36,  36.01]
+    y_dist = [1.5,  1.5,  1.5,  1.6,  1.76, 1.76,   1.78,  1.78,   1.8, 1.8]
  elif personality==custom.LongitudinalPersonalitySP.standard:
-    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.65, 1.65,  1.75,   1.75]
+    x_vel =  [0,    11,   14.5, 15,   20,   20.01,  25,    25.01,  36,  36.01]
+    y_dist = [1.40, 1.40, 1.40, 1.50, 1.60, 1.76,   1.76,  1.78,   1.8, 1.8]
  elif personality==custom.LongitudinalPersonalitySP.moderate:
-    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.45, 1.45,  1.55,   1.55]
+    x_vel =  [0,    11,   14.5, 15,   20,   20.01,  25,    25.01,  36,  36.01]
+    y_dist = [1.3,  1.3,  1.3,  1.35, 1.35, 1.385,  1.385, 1.4,   1.4,  1.45]
  elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    x_vel =  [0.,   19.7,  19.71,  36.1]
-    y_dist = [1.00, 1.00,  1.25,   1.25]
+    x_vel =  [0,     5,     5.01,  11,    14.5,   15,    20,    20.01,  25, 25.01, 36,   36.01]
+    y_dist = [1.12,  1.12,  1.12,  1.12,  1.12,  1.105, 1.105, 1.15, 1.15, 1.18, 1.20,  1.23]
  else:
    raise NotImplementedError("Dynamic personality not supported")
+
  return np.interp(v_ego, x_vel, y_dist)

+
 def get_stopped_equivalence_factor(v_lead):
  return (v_lead**2) / (2 * COMFORT_BRAKE)

@@ -370,15 +299,12 @@ class LongitudinalMpc:
    for i in range(N):
      self.solver.cost_set(i, 'Zl', Zl)

-  def set_weights(self, prev_accel_constraint=True, v_lead0 = 0., v_lead1 = 0., personality=custom.LongitudinalPersonalitySP.standard):
-    v_ego = self.x0[1]
+  def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
    jerk_factor = get_jerk_factor(personality)
-    a_change_factor = get_a_change_factor(v_ego, v_lead0, v_lead1, personality)
-    danger_zone_factor = get_danger_zone_factor(personality)
    if self.mode == 'acc':
      a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
-      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_factor * a_change_cost, jerk_factor * J_EGO_COST]
-      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * danger_zone_factor]
+      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
    elif self.mode == 'blended':
      a_change_cost = 40.0 if prev_accel_constraint else 0
      cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
@@ -432,18 +358,14 @@ class LongitudinalMpc:
    self.cruise_min_a = min_a
    self.max_a = max_a

-  def update(self, radarstate, v_cruise, prev_accel_constraint, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard,
-             dynamic_personality=False, overtaking_acceleration_assist=False):
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False):
    v_ego = self.x0[1]
    t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality)
-    t_follow = get_T_FOLLOW(custom.LongitudinalPersonalitySP.overtake) if overtaking_acceleration_assist else t_follow
    self.status = radarstate.leadOne.status or radarstate.leadTwo.status

    lead_xv_0 = self.process_lead(radarstate.leadOne)
    lead_xv_1 = self.process_lead(radarstate.leadTwo)

-    self.set_weights(prev_accel_constraint=prev_accel_constraint, v_lead0=lead_xv_0[0, 1], v_lead1=lead_xv_1[0, 1], personality=personality)
-
    # To estimate a safe distance from a moving lead, we calculate how much stopping
    # distance that lead needs as a minimum. We can add that to the current distance
    # and then treat that as a stopped car/obstacle at this new distance.
@@ -571,4 +493,4 @@ class LongitudinalMpc:
 if __name__ == "__main__":
  ocp = gen_long_ocp()
  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
-  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
@@ -3,7 +3,7 @@ import math
 import numpy as np
 from openpilot.common.numpy_fast import clip, interp
 from openpilot.common.params import Params
-from cereal import car, log, custom
+from cereal import car

 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
@@ -12,7 +12,6 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.sunnypilot.common import Source
 from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import SpeedLimitController
-from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
 from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
@@ -36,7 +35,6 @@ _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]


-MpcSource = custom.MpcSource
 EventName = car.CarEvent.EventName


@@ -84,8 +82,7 @@ class LongitudinalPlanner:
    self.dt = dt

    self.a_desired = init_a
-    v_ego_sec = 0.6 if CP.carName == "hyundai" and not CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV) else 2.0
-    self.v_desired_filter = FirstOrderFilter(init_v, v_ego_sec, self.dt)
+    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
    self.v_model_error = 0.0

    self.v_desired_trajectory = np.zeros(CONTROL_N)
@@ -132,13 +129,10 @@ class LongitudinalPlanner:
      self.read_param()
    self.param_read_counter += 1
    if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
-      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()
+      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)
    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
@@ -159,10 +153,8 @@ class LongitudinalPlanner:
      accel_limits = [ACCEL_MIN, ACCEL_MAX]
      accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]

-    overtaking_accel_engaged = sm['controlsStateSP'].overtakingAccelerationAssist
    # override accel using Accel Controller
-    if self.accel_controller.is_enabled(accel_personality=custom.AccelerationPersonality.sport if overtaking_accel_engaged else
-                                                          sm['controlsStateSP'].accelPersonality):
+    if self.accel_controller.is_enabled(accel_personality=sm['controlsStateSP'].accelPersonality):
      # get min, max from accel controller
      min_limit, max_limit = self.accel_controller.get_accel_limits(v_ego, accel_limits)
      if self.mpc.mode == 'acc':
@@ -197,12 +189,11 @@ class LongitudinalPlanner:
    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)

-    self.mpc.set_weights(prev_accel_constraint, personality=custom.LongitudinalPersonalitySP.overtake if overtaking_accel_engaged else sm['controlsStateSP'].personality)
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsStateSP'].personality)
    self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
    x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], v_cruise, prev_accel_constraint, x, v, a, j, personality=sm['controlsStateSP'].personality,
-                    dynamic_personality=sm['controlsStateSP'].dynamicPersonality, overtaking_acceleration_assist=overtaking_accel_engaged)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality)

    self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
    self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -271,8 +262,7 @@ class LongitudinalPlanner:
    longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state
    longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.v_target)

-    longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc
-    longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled()
+    longitudinalPlanSP.e2eBlended = self.mpc.mode

    pm.send('longitudinalPlanSP', plan_sp_send)

@@ -21,23 +21,23 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.

-# Last updated: August 12, 2024
+# Last updated: July 1, 2024

 from cereal import custom
 from openpilot.common.numpy_fast import interp

 AccelPersonality = custom.AccelerationPersonality

-# accel personality by @arne182 modified by cgw and kumar
-_DP_CRUISE_MIN_V =       [-1.0,  -1.0]
-_DP_CRUISE_MIN_V_ECO =   [-1.0,  -1.0]
-_DP_CRUISE_MIN_V_SPORT = [-1.0,  -1.0]
-_DP_CRUISE_MIN_BP =      [0., 20.]
+# accel personality by @arne182 modified by cgw
+_DP_CRUISE_MIN_V =       [-1.03,  -0.79,  -0.77,  -0.77,  -0.75,  -0.75,  -0.88,  -0.82]
+_DP_CRUISE_MIN_V_ECO =   [-1.02,  -0.78,  -0.75,  -0.75,  -0.73,  -0.73,  -0.80,  -0.80]
+_DP_CRUISE_MIN_V_SPORT = [-1.04,  -0.81,  -0.79,  -0.79,  -0.77,  -0.77,  -0.90,  -0.84]
+_DP_CRUISE_MIN_BP =      [0.,     0.05,   0.1,    0.5,    8.33,   16.,    30.,    40.]

-_DP_CRUISE_MAX_V =       [2.0, 2.0, 2.0, 1.80, 1.03, .62,  .47,  .36,  .11]
-_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 2.0, 1.65, 0.92, .532, .432, .32,  .095]
-_DP_CRUISE_MAX_V_SPORT = [2.0, 2.0, 2.0, 2.00, 1.25, .71,  .54,  .46,  .2]
-_DP_CRUISE_MAX_BP =      [0.,  1.,  6.,  8.,   11.,  20.,  25.,  30.,  55.]
+_DP_CRUISE_MAX_V =       [2.5, 2.5, 2.5, 1.70, 1.05, .81,  .625, .42,  .348, .12]
+_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 2.0, 1.4, .80,   .68,  .53,  .32,  .20,  .085]
+_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 2.8, 2.4,  1.4,  1.0,  .89,  .75,  .50,  .2]
+_DP_CRUISE_MAX_BP =      [0.,  1.,  6.,  8.,   11.,  15.,  20.,  25.,  30.,  55.]


 class AccelController:
@@ -21,39 +21,33 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 #
-# Version = 2024-7-11
+# Version = 2024-1-29
 from common.numpy_fast import interp
-import numpy as np
+from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE

-# d-e2e, from modeldata.h
-TRAJECTORY_SIZE = 33
-
-LEAD_WINDOW_SIZE = 4
+LEAD_WINDOW_SIZE = 5
 LEAD_PROB = 0.6

-SLOW_DOWN_WINDOW_SIZE = 4
+SLOW_DOWN_WINDOW_SIZE = 5
 SLOW_DOWN_PROB = 0.6
-
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
+SLOW_DOWN_DIST = [20, 30., 50., 70., 80., 90., 105., 120.]

-SLOWNESS_WINDOW_SIZE = 12
-SLOWNESS_PROB = 0.5
+SLOWNESS_WINDOW_SIZE = 20
+SLOWNESS_PROB = 0.6
 SLOWNESS_CRUISE_OFFSET = 1.05

-DANGEROUS_TTC_WINDOW_SIZE = 3
-DANGEROUS_TTC = 2.3
+DANGEROUS_TTC_WINDOW_SIZE = 5
+DANGEROUS_TTC = 2.0

-HIGHWAY_CRUISE_KPH = 70
+HIGHWAY_CRUISE_KPH = 75

 STOP_AND_GO_FRAME = 60

 SET_MODE_TIMEOUT = 10

-MPC_FCW_WINDOW_SIZE = 10
-MPC_FCW_PROB = 0.5
-
-V_ACC_MIN = 9.72
+MPC_FCW_WINDOW_SIZE = 5
+MPC_FCW_PROB = 0.6


 class SNG_State:
@@ -83,51 +77,29 @@ 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, 3, 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

-    # Use weighted moving average for filtering leads
-    self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
+    self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
    self._has_lead_filtered = False
    self._has_lead_filtered_prev = False

-    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
+    self._slow_down_gmac = GenericMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
    self._has_slow_down = False

    self._has_blinkers = False

-    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
    self._has_slowness = False

    self._has_nav_instruction = False

-    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
    self._has_dangerous_ttc = False

    self._v_ego_kph = 0.
@@ -141,50 +113,13 @@ class DynamicExperimentalController:
    self._sng_transit_frame = 0
    self._sng_state = SNG_State.off

-    self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
+    self._mpc_fcw_gmac = GenericMovingAverageCalculator(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.03 * np.log(1 + len(self._slow_down_gmac.data)))
-
-  def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True):
-    """
-    Basic anomaly detection using standard deviation.
-    """
-    if len(recent_data) < 5:
-      return False
-    mean = np.mean(recent_data)
-    std_dev = np.std(recent_data)
-    anomaly = recent_data[-1] > mean + threshold * std_dev
-
-    # Context check to ensure repeated anomaly
-    if context_check:
-      return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1
-    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
@@ -193,27 +128,18 @@ 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_weighted_average() > MPC_FCW_PROB
+    self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() >= MPC_FCW_PROB

    # nav enable detection
    self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13

-    # lead detection with smoothing
+    # lead detection
    self._lead_gmac.add_data(lead_one.status)
-    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)
+    self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_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
+    # 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

    # blinker detection
    self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
@@ -235,7 +161,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_weighted_average() > SLOWNESS_PROB
+      self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB

    # dangerous TTC detection
    if not self._has_lead_filtered and self._has_lead_filtered_prev:
@@ -245,14 +171,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_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC
+    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_moving_average() is not None and self._dangerous_ttc_gmac.get_moving_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 _radarless_mode(self):
+  def _blended_priority_mode(self):
    # when mpc fcw crash prob is high
    # use blended to slow down quickly
    if self._has_mpc_fcw:
@@ -264,21 +190,21 @@ class DynamicExperimentalController:
      self._set_mode('blended')
      return

-    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # 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 < V_ACC_MIN:
-    #  self._set_mode('blended')
-    #  return
+    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
+      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 >= V_ACC_MIN:
-    #  self._set_mode('blended')
-    #  return
+    if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
+      self._set_mode('blended')
+      return

    # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
    if self._has_standstill:
      self._set_mode('blended')
      return
@@ -300,9 +226,9 @@ class DynamicExperimentalController:
      self._set_mode('acc')
      return

-    self._set_mode('acc')
+    self._set_mode('blended')

-  def _radar_mode(self):
+  def _acc_priority_mode(self):
    # when mpc fcw crash prob is high
    # use blended to slow down quickly
    if self._has_mpc_fcw:
@@ -310,18 +236,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 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')
+    #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')
+      return
+
    # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
    if self._has_standstill:
      self._set_mode('blended')
      return
@@ -344,22 +270,17 @@ class DynamicExperimentalController:

    self._set_mode('acc')

-  def update(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
+  def get_mpc_mode(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._radarless_mode()
+        self._blended_priority_mode()
      else:
-        self._radar_mode()
-    self._mode_changed = self._mode != self._mode_prev
+        self._acc_priority_mode()
+
    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

@@ -32,7 +32,7 @@ def plannerd_thread():
  pm = messaging.PubMaster(['longitudinalPlan', 'longitudinalPlanSP'] + lateral_planner_svs)
  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2',
                            'longitudinalPlan', 'navInstruction', 'longitudinalPlanSP',
-                            'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP', 'driverMonitoringState'] + lateral_planner_svs,
+                            'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP'] + lateral_planner_svs,
                           poll='modelV2', ignore_avg_freq=['radarState'])

  while True:
@@ -83,7 +83,7 @@ class TorqueEstimator(ParameterEstimator):
    params = Params()
    if params.get_bool("EnforceTorqueLateral"):
      if params.get_bool("CustomTorqueLateral"):
-        self.offline_friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.001
+        self.offline_friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.01
        self.offline_latAccelFactor = float(params.get("TorqueMaxLatAccel", encoding="utf8")) * 0.01
      if params.get_bool("LiveTorqueRelaxed"):
        self.min_bucket_points = np.array([0, 200, 300, 500, 500, 300, 200, 0]) / (10 if decimated else 1)
@@ -145,4 +145,4 @@ void HttpRequest::requestFinished() {
 QNetworkAccessManager *HttpRequest::nam() {
  static QNetworkAccessManager *networkAccessManager = new QNetworkAccessManager(qApp);
  return networkAccessManager;
-}
+}
@@ -24,7 +24,7 @@ class HttpRequest : public QObject {
  Q_OBJECT

 public:
-  enum class Method { GET, DELETE, POST, PUT };
+  enum class Method {GET, DELETE, POST, PUT};

  explicit HttpRequest(QObject* parent, bool create_jwt = true, int timeout = 20000);
  virtual void sendRequest(const QString &requestURL, Method method);
@@ -47,4 +47,4 @@ protected:
 protected slots:
  void requestTimeout();
  void requestFinished();
-};
+};
@@ -86,8 +86,7 @@ void HomeWindow::mousePressEvent(QMouseEvent* e) {
 }

 void HomeWindow::mouseDoubleClickEvent(QMouseEvent* e) {
-  // By removing the static call to HomeWindow::mousePressEvent, we can now rely on child classes to handle the event
-  mousePressEvent(e);
+  HomeWindow::mousePressEvent(e);
  const SubMaster &sm = *(uiState()->sm);
  if (sm["carParams"].getCarParams().getNotCar()) {
    if (onroad->isVisible()) {
@@ -47,7 +47,7 @@ protected:
  void mouseDoubleClickEvent(QMouseEvent* e) override;

  Sidebar *sidebar;
-  OffroadHome* home;
+  OffroadHome *home;
  OnroadWindow *onroad;
  BodyWindow *body;
  DriverViewWindow *driver_view;
@@ -55,4 +55,4 @@ protected:

 protected slots:
  virtual void updateState(const UIState &s);
-};
+};
@@ -16,7 +16,7 @@
 #include "selfdrive/ui/qt/widgets/ssh_keys.h"
 #ifdef SUNNYPILOT
 #include "selfdrive/ui/sunnypilot/sunnypilot_main.h"
-#endif
+#endif 

 TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
  RETURN_IF_SUNNYPILOT
@@ -91,7 +91,6 @@ protected:
  virtual void updateToggles();
 };

-
 class SoftwarePanel : public ListWidget {
  Q_OBJECT
 public:
@@ -26,7 +26,7 @@


 void SoftwarePanel::checkForUpdates() {
-  std::system("pkill -SIGUSR1 -f updated");
+  std::system("pkill -SIGUSR1 -f system.updated.updated");
 }

 SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
@@ -45,7 +45,7 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
    if (downloadBtn->text() == tr("CHECK")) {
      checkForUpdates();
    } else {
-      std::system("pkill -SIGHUP -f updated");
+      std::system("pkill -SIGHUP -f system.updated.updated");
    }
  });
  addItem(downloadBtn);
@@ -162,4 +162,4 @@ void SoftwarePanel::updateLabels() {
  installBtn->setDescription(QString::fromStdString(params.get("UpdaterNewReleaseNotes")));

  update();
-}
+}
@@ -2,7 +2,6 @@

 #include <QHBoxLayout>
 #include <QMouseEvent>
-#include <QVBoxLayout>

 #include "selfdrive/ui/qt/offroad/experimental_mode.h"
 #include "selfdrive/ui/qt/util.h"
@@ -1,19 +1,12 @@
 #pragma once

-#include <QFrame>
-#include <QLabel>
-#include <QPushButton>
-#include <QStackedLayout>
 #include <QStackedWidget>
-#include <QTimer>
 #include <QWidget>

 #include "common/params.h"
-#include "selfdrive/ui/qt/offroad/driverview.h"
 #include "selfdrive/ui/qt/body.h"
 #include "selfdrive/ui/qt/onroad/onroad_home.h"
 #include "selfdrive/ui/qt/widgets/offroad_alerts.h"
-#include "selfdrive/ui/ui.h"

 #ifdef SUNNYPILOT
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
@@ -385,4 +385,4 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) {

  ui_update_params(uiState());
  prev_draw_t = millis_since_boot();
-}
+}
@@ -50,4 +50,4 @@ protected:

  double prev_draw_t = 0;
  FirstOrderFilter fps_filter;
-};
+};
@@ -26,8 +26,7 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals

 void ExperimentalButton::changeMode() {
  const auto cp = (*uiState()->sm)["carParams"].getCarParams();
-  bool can_change = (hasLongitudinalControl(cp) || (!cp.getPcmCruiseSpeed() && params.getBool("CustomStockLongPlanner")))
-                    && params.getBool("ExperimentalModeConfirmed");
+  bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
  if (can_change) {
    params.putBool("ExperimentalMode", !experimental_mode);
  }
@@ -31,4 +31,4 @@ private:
  QPixmap experimental_img;
 };

-void drawIcon(QPainter &p, const QPoint &center, const QPixmap &img, const QBrush &bg, float opacity);
+void drawIcon(QPainter &p, const QPoint &center, const QPixmap &img, const QBrush &bg, float opacity);
@@ -66,4 +66,4 @@ void OnroadWindow::offroadTransition(bool offroad) {
 void OnroadWindow::paintEvent(QPaintEvent *event) {
  QPainter p(this);
  p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
-}
+}
@@ -26,4 +26,4 @@ protected:
 protected slots:
  virtual void offroadTransition(bool offroad);
  virtual void updateState(const UIState &s);
-};
+};
@@ -20,4 +20,4 @@ private:
  Params params;
  QTimer *timer;
  QString prevResp;
-};
+};
@@ -65,4 +65,4 @@ protected:

 private:
  std::unique_ptr<PubMaster> pm;
-};
+};
@@ -61,4 +61,4 @@ int main(int argc, char *argv[]) {
  )");

  return a.exec();
-}
+}
@@ -59,4 +59,4 @@ private:
  QFileSystemWatcher *watcher;
  QHash<QString, QString> params_hash;
  Params params;
-};
+};
@@ -35,4 +35,4 @@ private:

  void refresh();
  void getUserKeys(const QString &username);
-};
+};
@@ -30,7 +30,7 @@ sp_maps_widgets_src = [
 ]

 sp_qt_util = [
-  # "#selfdrive/ui/sunnypilot/qt/api.cc",
+  # "#selfdrive/ui/sunnypilot/qt/api.cc", 
  "#selfdrive/ui/sunnypilot/qt/util.cc",
 ]

@@ -33,7 +33,6 @@ Last updated: July 29, 2024
 #include <QApplication>
 #include <QJsonDocument>

-#include <memory>
 #include <string>
 #include <common/swaglog.h>

@@ -26,11 +26,6 @@ Last updated: July 29, 2024

 #pragma once

-#include <QJsonObject>
-#include <QNetworkReply>
-#include <QString>
-#include <QTimer>
-
 #include "selfdrive/ui/qt/api.h"
 #include "selfdrive/ui/sunnypilot/qt/util.h"
 #include "common/util.h"
@@ -28,13 +28,10 @@ Last updated: July 29, 2024

 #include <QHBoxLayout>
 #include <QMouseEvent>
-#include <QStackedWidget>
-#include <QVBoxLayout>
 #include <common/swaglog.h>

 #include "selfdrive/ui/qt/offroad/experimental_mode.h"
 #include "selfdrive/ui/qt/util.h"
-#include "selfdrive/ui/qt/widgets/prime.h"

 // HomeWindowSP: the container for the offroad and onroad UIs
 HomeWindowSP::HomeWindowSP(QWidget* parent) : HomeWindow(parent){    
@@ -27,14 +27,9 @@ Last updated: July 29, 2024
 #pragma once

 #include <QFrame>
-#include <QLabel>
-#include <QPushButton>
-#include <QStackedLayout>
-#include <QTimer>
 #include <QWidget>

 #include "common/params.h"
-#include "selfdrive/ui/qt/offroad/driverview.h"
 #include "selfdrive/ui/qt/body.h"
 #include "selfdrive/ui/qt/widgets/offroad_alerts.h"
 #include "selfdrive/ui/sunnypilot/ui.h"
@@ -42,7 +37,6 @@ Last updated: July 29, 2024

 #ifdef SUNNYPILOT
 #include "selfdrive/ui/sunnypilot/qt/sidebar.h"
-#include "selfdrive/ui/sunnypilot/qt/onroad/onroad_home.h"
 #define OnroadWindow OnroadWindowSP
 #else
 #include "selfdrive/ui/qt/sidebar.h"
@@ -30,7 +30,6 @@ Last updated: July 29, 2024
 #include <vector>
 #include <QFileInfo>

-#include "common/watchdog.h"
 #include "selfdrive/ui/qt/qt_window.h"
 #include "selfdrive/ui/qt/widgets/prime.h"

@@ -28,11 +28,9 @@ Last updated: July 29, 2024

 #include <map>
 #include <string>
-#include <tuple>
 #include <vector>

 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"

 class MonitoringPanel : public QFrame {
  Q_OBJECT
@@ -57,12 +57,6 @@ TogglesPanelSP::TogglesPanelSP(SettingsWindow *parent) : TogglesPanel(parent) {
      tr("When enabled, sunnypilot will attempt to control stock longitudinal control with ACC button presses.\nThis feature must be used along with SLC, and/or V-TSC, and/or M-TSC."),
      "../assets/offroad/icon_blank.png",
    },
-    {
-      "CustomStockLongPlanner",
-      tr("Use Planner Speed"),
-      "",
-      "../assets/offroad/icon_blank.png",
-    },
    {
      "ExperimentalMode",
      tr("Experimental Mode"),
@@ -229,7 +223,6 @@ void TogglesPanelSP::updateToggles() {
  auto custom_stock_long_toggle = toggles["CustomStockLong"];
  auto dec_toggle = toggles["DynamicExperimentalControl"];
  auto dynamic_personality_toggle = toggles["DynamicPersonality"];
-  auto custom_stock_long_planner = toggles["CustomStockLongPlanner"];
  const QString e2e_description = QString("%1<br>"
                                          "<h4>%2</h4><br>"
                                          "%3<br>"
@@ -268,22 +261,16 @@ void TogglesPanelSP::updateToggles() {
      accel_personality_setting->setEnabled(true);
      op_long_toggle->setEnabled(true);
      custom_stock_long_toggle->setEnabled(false);
-      custom_stock_long_planner->setEnabled(false);
      params.remove("CustomStockLong");
      dec_toggle->setEnabled(true);
      dynamic_personality_toggle->setEnabled(true);
    } else if (custom_stock_long_toggle->isToggled()) {
      op_long_toggle->setEnabled(false);
-      experimental_mode_toggle->setEnabled(custom_stock_long_planner->isToggled());
-      experimental_mode_toggle->setDescription(e2e_description);
-      custom_stock_long_planner->setEnabled(true);
-      long_personality_setting->setEnabled(custom_stock_long_planner->isToggled());
-      accel_personality_setting->setEnabled(custom_stock_long_planner->isToggled());
-      dec_toggle->setEnabled(experimental_mode_toggle->isToggled());
-      if (!custom_stock_long_planner->isToggled()) {
-        params.remove("ExperimentalMode");
-      }
+      experimental_mode_toggle->setEnabled(false);
+      long_personality_setting->setEnabled(false);
+      accel_personality_setting->setEnabled(false);
      params.remove("ExperimentalLongitudinalEnabled");
+      params.remove("ExperimentalMode");
    } else {
      // no long for now
      experimental_mode_toggle->setEnabled(false);
@@ -308,7 +295,6 @@ void TogglesPanelSP::updateToggles() {
      custom_stock_long_toggle->setEnabled(CP.getCustomStockLongAvailable());
      dec_toggle->setEnabled(false);
      dynamic_personality_toggle->setEnabled(false);
-      custom_stock_long_planner->setEnabled(false);
      params.remove("DynamicExperimentalControl");
      params.remove("DynamicPersonality");
    }
@@ -316,7 +302,6 @@ void TogglesPanelSP::updateToggles() {
    experimental_mode_toggle->refresh();
    op_long_toggle->refresh();
    custom_stock_long_toggle->refresh();
-    custom_stock_long_planner->refresh();
    dec_toggle->refresh();
    dynamic_personality_toggle->refresh();
  } else {
@@ -325,7 +310,6 @@ void TogglesPanelSP::updateToggles() {
    custom_stock_long_toggle->setVisible(false);
    dec_toggle->setVisible(false);
    dynamic_personality_toggle->setVisible(false);
-    custom_stock_long_planner->setVisible(false);
  }
 }

@@ -49,12 +49,6 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
      tr("While in Auto Lane, switch to Laneless for current/future curves."),
      "../assets/offroad/icon_blank.png",
    },
-    {
-      "OvertakingAccelerationAssist",
-      tr("Overtaking Acceleration Assist"),
-      tr("Overtaking Acceleration Assist will operate when the turn signal indicator is turned on to the left (left-hand drive) or turned on to the right (right-hand drive) while openpilot Longitudinal Control is operating."),
-      "../assets/offroad/icon_blank.png",
-    },
    {
      "EnableSlc",
      tr("Speed Limit Control (SLC)"),
@@ -97,24 +91,12 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
      "",
      "../assets/offroad/icon_blank.png",
    },
-    {
-      "NNFFNoLateralJerk",
-      tr("NNLC: Remove Lateral Jerk Response (Alpha)"),
-      tr("When NNLC is active, enable this to disables the use of lateral jerk in steering torque calculations, focusing solely on lateral acceleration for a simplified control response."),
-      "../assets/offroad/icon_blank.png",
-    },
    {
      "EnforceTorqueLateral",
      tr("Enforce Torque Lateral Control"),
      tr("Enable this to enforce sunnypilot to steer with Torque lateral control."),
      "../assets/offroad/icon_blank.png",
    },
-    {
-      "TorqueLateralJerk",
-      tr("Lateral Jerk with Torque Lateral Control (Alpha)"),
-      tr("Utilizes limited lateral jerk control for improved steering response, leveraging stock torque lateral controller capabilities. Designed to mimic NNLC behavior without training models or data collection."),
-      "../assets/offroad/icon_blank.png",
-    },
    {
      "LiveTorque",
      tr("Enable Self-Tune"),
@@ -316,7 +298,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
      list->addItem(dlp_settings);
    }

-    if (param == "OvertakingAccelerationAssist") {
+    if (param == "VisionCurveLaneless") {
      list->addItem(laneChangeSettingsLayout);
      list->addItem(horizontal_line());

@@ -363,17 +345,6 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
    }
  });

-  connect(toggles["EnforceTorqueLateral"], &ToggleControlSP::toggleFlipped, [=](bool state) {
-    if (state) {
-      toggles["NNFF"]->setEnabled(false);
-      params.putBool("NNFF", false);
-      toggles["NNFF"]->refresh();
-    } else {
-      toggles["NNFF"]->setEnabled(true);
-      toggles["NNFF"]->refresh();
-    }
-  });
-
  // trigger updateToggles() when toggleFlipped
  for (const auto& updateToggleName : updateTogglesNames) {
    if (toggles.find(updateToggleName) != toggles.end()) {
@@ -395,7 +366,6 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
  toggles["EnableMads"]->setConfirmation(true, false);
  toggles["EndToEndLongAlertLight"]->setConfirmation(true, false);
  toggles["CustomOffsets"]->showDescription();
-  toggles["OvertakingAccelerationAssist"]->setConfirmation(true, false);

  connect(toggles["EnableMads"], &ToggleControlSP::toggleFlipped, mads_settings, &MadsSettings::updateToggles);
  connect(toggles["EnableMads"], &ToggleControlSP::toggleFlipped, [=](bool state) {
@@ -500,8 +470,6 @@ void SunnypilotPanel::updateToggles() {
  toggles["VisionCurveLaneless"]->setEnabled(dynamic_lane_profile_param == "2");
  toggles["VisionCurveLaneless"]->refresh();

-  auto overtaking_acceleration_assist = toggles["OvertakingAccelerationAssist"];
-
  bool custom_driving_model_valid = params.getBool("CustomDrivingModel");
  auto driving_model_generation = QString::fromStdString(params.get("DrivingModelGeneration"));
  bool model_use_lateral_planner = custom_driving_model_valid && driving_model_generation == "1";
@@ -581,7 +549,6 @@ void SunnypilotPanel::updateToggles() {
      if (!slcSettings->isVisible() && enable_slc_param) {
        slcSettings->setVisible(true);
      }
-      overtaking_acceleration_assist->setEnabled(hasLongitudinalControl(CP));
    } else {
      v_tsc->setEnabled(false);
      m_tsc->setEnabled(false);
@@ -589,21 +556,18 @@ void SunnypilotPanel::updateToggles() {
      slc_toggle->setEnabled(false);
      params.remove("EnableSlc");
      slcSettings->setEnabled(false);
-      overtaking_acceleration_assist->setEnabled(false);
    }

    enforce_torque_lateral->refresh();
    slc_toggle->refresh();
    nnff_toggle->refresh();
    m_tsc->refresh();
-    overtaking_acceleration_assist->refresh();
  } else {
    v_tsc->setEnabled(false);
    m_tsc->setEnabled(false);
    reverse_acc->setEnabled(false);
    slc_toggle->setEnabled(false);
    slcSettings->setEnabled(false);
-    overtaking_acceleration_assist->setEnabled(false);

    nnff_toggle->setDescription(nnff_toggle->isToggled() ? nnffDescriptionBuilder(nnff_status_init) : nnff_description);
  }
@@ -613,7 +577,7 @@ void SunnypilotPanel::updateToggles() {
  }

  // toggle names to update when EnforceTorqueLateral is flipped
-  std::vector<std::string> torqueLateralGroup{"CustomTorqueLateral", "TorqueLateralJerk", "LiveTorque", "LiveTorqueRelaxed", "TorquedOverride"};
+  std::vector<std::string> torqueLateralGroup{"CustomTorqueLateral", "LiveTorque", "LiveTorqueRelaxed", "TorquedOverride"};
  for (const auto& torqueLateralToggle : torqueLateralGroup) {
    if (toggles.find(torqueLateralToggle) != toggles.end()) {
      if (nnff_toggle->isToggled()) {
@@ -630,24 +594,6 @@ void SunnypilotPanel::updateToggles() {
    }
  }

-  // toggle names to update when EnforceTorqueLateral is flipped
-  std::vector<std::string> nnffGroup{"NNFFNoLateralJerk"};
-  for (const auto& nnffToggle : nnffGroup) {
-    if (toggles.find(nnffToggle) != toggles.end()) {
-      if (enforce_torque_lateral->isToggled()) {
-        toggles[nnffToggle]->setVisible(false);
-        toggles[nnffToggle]->setEnabled(false);
-      }
-    }
-  }
-
-  for (const auto& nnffToggle : nnffGroup) {
-    if (toggles.find(nnffToggle) != toggles.end()) {
-      toggles[nnffToggle]->setVisible(nnff_toggle->isToggled());
-      toggles[nnffToggle]->setEnabled(nnff_toggle->isToggled());
-    }
-  }
-
  if (enforce_torque_lateral->isToggled()) {
    live_torque_relaxed->setVisible(live_torque->isToggled());
    torqued_override->setVisible(custom_torque_lateral->isToggled());
@@ -676,14 +622,13 @@ TorqueFriction::TorqueFriction() : OptionControlSP(
  tr("FRICTION"),
  tr("Adjust Friction for the Torque Lateral Controller. <b>Live</b>: Override self-tune values; <b>Offline</b>: Override self-tune offline values at car restart."),
  "../assets/offroad/icon_blank.png",
-  {0, 500},
-  5) {
+  {0, 50}) {

  refresh();
 }

 void TorqueFriction::refresh() {
-  float torqueFrictionVal = QString::fromStdString(params.get("TorqueFriction")).toInt() * 0.001;
+  float torqueFrictionVal = QString::fromStdString(params.get("TorqueFriction")).toInt() * 0.01;
  setTitle("FRICTION - " + (params.getBool("TorquedOverride") ? tr("Real-time and Offline") : tr("Offline Only")));
  setLabel(QString::number(torqueFrictionVal));
 }
@@ -29,7 +29,6 @@ Last updated: July 29, 2024
 #include <map>
 #include <string>

-#include "common/model.h"
 #include "selfdrive/ui/sunnypilot/ui.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/custom_offsets_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/lane_change_settings.h"
@@ -101,24 +101,13 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge

  // Hyundai/Kia/Genesis
  addItem(new LabelControlSP(tr("Hyundai/Kia/Genesis")));
-  auto hkgCustomLongTuning = new ParamControlSP(
-    "HkgCustomLongTuning",
-    tr("HKG: Custom Tuning for New Longitudinal API"),
-    tr(""),
-    "../assets/offroad/icon_blank.png"
-  );
-  hkgCustomLongTuning->setConfirmation(true, false);
-  addItem(hkgCustomLongTuning);
-
-  hyundaiCruiseMainDefault = new ParamControlSP(
-    "HyundaiCruiseMainDefault",
-    tr("HKG CAN: Enable Cruise Main by Default"),
-    QString("<b>%1</b><br><br>%2")
-    .arg(tr("WARNING: This feature only applies when \"openpilot Longitudinal Control (Alpha)\" is enabled under \"Toggles\""))
-    .arg(tr("Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.")),
+  auto hkgSmoothStop = new ParamControlSP(
+    "HkgSmoothStop",
+    tr("HKG CAN: Smoother Stopping Performance (Beta)"),
+    tr("Smoother stopping behind a stopped car or desired stopping event. This is only applicable to HKG CAN platforms using openpilot longitudinal control."),
    "../assets/offroad/icon_blank.png");
-  hyundaiCruiseMainDefault->setConfirmation(true, false);
-  addItem(hyundaiCruiseMainDefault);
+  hkgSmoothStop->setConfirmation(true, false);
+  addItem(hkgSmoothStop);

  // Subaru
  addItem(new LabelControlSP(tr("Subaru")));
@@ -159,18 +148,6 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
  toyotaTss2LongTune->setConfirmation(true, false);
  addItem(toyotaTss2LongTune);

-  auto toyotaAbh = new ParamControlSP(
-    "ToyotaAutoHold",
-    tr("Enable Automatic Brake Hold (AHB)"),
-    QString("<b>%1</b><br><br>%2<br><br><b>%3</b>")
-    .arg(tr("WARNING: Only for Toyota/Lexus vehicles with TSS2/LSS2. USE AT YOUR OWN RISK."))
-    .arg(tr("When you stop the vehicle completely by depressing the brake pedal, sunnypilot will activate Auto Brake Hold."))
-    .arg(tr("Changing this setting takes effect when the car is powered off.")),
-    "../assets/offroad/icon_blank.png"
-  );
-  toyotaAbh->setConfirmation(true, false);
-  addItem(toyotaAbh);
-
  toyotaEnhancedBsm = new ParamControlSP(
    "ToyotaEnhancedBsm",
    tr("Enable Enhanced Blind Spot Monitor"),
@@ -203,14 +180,6 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
  toyotaAutoUnlock->setConfirmation(true, false);
  addItem(toyotaAutoUnlock);

-  auto toyotaDriveMode = new ParamControlSP(
-    "ToyotaDriveMode",
-    tr("Enable Toyota Drive Mode Button"),
-    tr("Sunnypilot will link the Acceleration Personality to the car's physical drive mode selector.\nReboot Required."),
-    "../assets/offroad/icon_blank.png");
-  toyotaDriveMode->setConfirmation(true, false);
-  addItem(toyotaDriveMode);
-
  // Volkswagen
  addItem(new LabelControlSP(tr("Volkswagen")));
  auto volkswagenCCOnly = new ParamControlSP(
@@ -224,10 +193,8 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
  // trigger offroadTransition when going onroad/offroad
  connect(uiStateSP(), &UIStateSP::offroadTransition, [=](bool offroad) {
    is_onroad = !offroad;
-    hkgCustomLongTuning->setEnabled(offroad);
-    hyundaiCruiseMainDefault->setEnabled(offroad);
+    hkgSmoothStop->setEnabled(offroad);
    toyotaTss2LongTune->setEnabled(offroad);
-    toyotaAbh->setEnabled(offroad);
    toyotaEnhancedBsm->setEnabled(offroad);
    toyotaSngHack->setEnabled(offroad);
    volkswagenCCOnly->setEnabled(offroad);
@@ -256,17 +223,6 @@ void SPVehiclesTogglesPanel::updateToggles() {
    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();

-    // Hyundai/Kia/Genesis
-    {
-      if (CP.getCarName() == "hyundai") {
-        if ((CP.getSpFlags() & 2) and !(CP.getFlags() & 8192)) {
-          hyundaiCruiseMainDefault->setEnabled(true);
-        } else {
-          hyundaiCruiseMainDefault->setEnabled(false);
-        }
-      }
-    }
-
    // Toyota: Enhanced Blind Spot Monitor
    {
      if (CP.getCarName() == "toyota") {
@@ -28,10 +28,8 @@ Last updated: July 29, 2024

 #include <QApplication>

-#include "common/watchdog.h"
 #include "selfdrive/ui/sunnypilot/ui.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"

 class VehiclePanel : public QWidget {
  Q_OBJECT
@@ -69,7 +67,6 @@ private:
  Params params;
  bool is_onroad = false;

-  ParamControlSP *hyundaiCruiseMainDefault;
  ParamControlSP *stockLongToyota;
  ParamControlSP *toyotaEnhancedBsm;

@@ -30,7 +30,6 @@ Last updated: July 29, 2024

 #include "selfdrive/ui/qt/offroad/experimental_mode.h"
 #include "selfdrive/ui/qt/widgets/prime.h"
-#include <QStackedWidget>
 #include "selfdrive/ui/sunnypilot/sunnypilot_main.h"

 #ifdef ENABLE_MAPS
@@ -31,7 +31,6 @@ Last updated: July 29, 2024
 #include "selfdrive/ui/qt/maps/map_settings.h"
 #endif

-
 class OffroadHomeSP : public OffroadHome {
  Q_OBJECT

@@ -216,7 +216,6 @@ void AnnotatedCameraWidgetSP::updateState(const UIStateSP &s) {
  latAccelFactorFiltered = ltp.getLatAccelFactorFiltered();
  frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered();
  liveValid = ltp.getLiveValid();
-  ecoMode = vEgo > 0 && car_state.getEngineRpm() == 0;
  // ############################## DEV UI END ##############################

  btnPerc = s.scene.sleep_btn_opacity * 0.05;
@@ -321,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;
-  const auto mpc_source = lp_sp.getMpcSource();
-  mpcSource = mpc_source == cereal::MpcSource::BLENDED ? QString(tr("blended")) : QString(tr("acc"));
+  mpcMode = QString::fromStdString(lp_sp.getE2eBlended());
+  mpcMode = (mpcMode == "blended") ? mpcMode.replace(0, 1, mpcMode[0].toUpper()) : mpcMode.toUpper();

  static int reverse_delay = 0;
  bool reverse_allowed = false;
@@ -524,32 +523,10 @@ void AnnotatedCameraWidgetSP::drawHud(QPainter &p) {

  // current speed
  if (!hideVEgoUi) {
-    // Set up the font for the speed text
    p.setFont(InterFont(176, QFont::Bold));
-
-    // Define text coordinates
-    int centerX = rect().center().x();
-    int centerY = 210;
-
-    // Draw a red border if brakeLights is active
-    if (brakeLights) {
-      for (int offsetX = -6; offsetX <= 6; offsetX++) {
-        for (int offsetY = -6; offsetY <= 6; offsetY++) {
-          // Avoid drawing at the original text position
-          if (offsetX != 0 || offsetY != 0) {
-            drawColoredText(p, centerX + offsetX, centerY + offsetY, speedStr, QColor(255, 0, 0, 255)); // Red border
-          }
-        }
-      }
-    }
-
-    // Draw the main speed text: green if ecoMode is on, otherwise white
-    QColor speedTextColor = ecoMode ? QColor(0, 245, 0) : QColor(255, 255, 255, 255);
-    drawColoredText(p, centerX, centerY, speedStr, speedTextColor);
-
-    // Draw the speed unit below the main text
+    drawColoredText(p, rect().center().x(), 210, speedStr, brakeLights ? QColor(0xff, 0, 0, 255) : QColor(0xff, 0xff, 0xff, 255));
    p.setFont(InterFont(66));
-    drawText(p, centerX, 290, speedUnit, 200);
+    drawText(p, rect().center().x(), 290, speedUnit, 200);
  }

  if (!reversing) {
@@ -1084,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(mpcSource).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
+    dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcMode).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));
@@ -1188,7 +1165,7 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
  }

  // TODO: Fix empty spaces when curiving back on itself
-  painter.setBrush(QColor::fromRgbF(1.0, 0.0, 0.0, 0.4));
+  painter.setBrush(QColor::fromRgbF(1.0, 0.0, 0.0, 0.2));
  if (left_blindspot) painter.drawPolygon(scene.lane_barrier_vertices[0]);
  if (right_blindspot) painter.drawPolygon(scene.lane_barrier_vertices[1]);

@@ -1200,10 +1177,6 @@ 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));
@@ -1212,7 +1185,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 (exp_mode_path) {
+    } else if (sm["controlsState"].getControlsState().getExperimentalMode()) {
      // 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());
@@ -1313,25 +1286,46 @@ void AnnotatedCameraWidgetSP::drawDriverState(QPainter &painter, const UIStateSP
 }

 void AnnotatedCameraWidgetSP::rocketFuel(QPainter &p) {
-  UIState *s = uiState();
-  float accel = (*s->sm)["carControl"].getCarControl().getActuators().getAccel();
-  int widgetHeight = rect().height();
-  float halfHeightAbs = std::abs(accel) * widgetHeight / 2.0f;
-  const float scannerWidth = 15;
-  QRect scannerRect;

-  if (accel > 0) {
+  static const int ct_n = 1;
+  static float ct;
+
+  int rect_w = rect().width();
+  int rect_h = rect().height();
+
+  const int n = 15 + 1; //Add one off screen due to timing issues
+  static float t[n];
+  int dim_n = (sin(ct / 5) + 1) * (n - 0.01);
+  t[dim_n] = 1.0;
+  t[(int)(ct/ct_n)] = 1.0;
+
+  UIStateSP *s = uiStateSP();
+  float vc_accel0 = (*s->sm)["carState"].getCarState().getAEgo();
+  static float vc_accel;
+  vc_accel = vc_accel + (vc_accel0 - vc_accel) / 5;
+  float hha = 0;
+  if (vc_accel > 0) {
+    hha = 0.85 - 0.1 / vc_accel;  // only extend up to 85%
    p.setBrush(QColor(0, 245, 0, 200));
-    scannerRect = QRect(0, widgetHeight / 2 - halfHeightAbs, scannerWidth, halfHeightAbs);
-  } else {
-    p.setBrush(QColor(245, 0, 0, 200));
-    scannerRect = QRect(0, widgetHeight / 2, scannerWidth, halfHeightAbs);
  }
-
-  p.drawRect(scannerRect);
+  if (vc_accel < 0) {
+    hha = 0.85 + 0.1 / vc_accel; // only extend up to 85%
+    p.setBrush(QColor(245, 0, 0, 200));
+  }
+  if (hha < 0) {
+    hha = 0;
+  }
+  hha = hha * rect_h;
+  float wp = 28;
+  if (vc_accel > 0) {
+    QRect ra = QRect(rect_w - wp, rect_h / 2 - hha / 2, wp, hha / 2);
+    p.drawRect(ra);
+  } else {
+    QRect ra = QRect(rect_w - wp, rect_h / 2, wp, hha / 2);
+    p.drawRect(ra);
+  }
 }

-
 void AnnotatedCameraWidgetSP::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd,
                                     int num, const cereal::CarState::Reader &car_data, int chevron_data) {
  painter.save();
@@ -176,7 +176,6 @@ private:
  float latAccelFactorFiltered;
  float frictionCoefficientFiltered;
  bool liveValid;
-  bool ecoMode;
  // ############################## DEV UI END ##############################

  float btnPerc;
@@ -193,7 +192,7 @@ private:
  cereal::LongitudinalPlanSP::SpeedLimitControlState slcState;
  int longitudinalPersonality;
  int dynamicLaneProfile;
-  QString mpcSource;
+  QString mpcMode;

  int speed_limit_frame;
  bool slcShowSign = true;
@@ -26,8 +26,6 @@ Last updated: July 29, 2024

 #include "selfdrive/ui/sunnypilot/qt/onroad/onroad_home.h"

-#include <QPainter>
-#include <QStackedLayout>

 #ifdef ENABLE_MAPS
 #include "selfdrive/ui/sunnypilot/qt/maps/map_helpers.h"
@@ -29,9 +29,6 @@ Last updated: July 29, 2024
 #include "selfdrive/ui/qt/onroad/onroad_home.h"

 #include "common/params.h"
-#include "selfdrive/ui/qt/onroad/alerts.h"
-// #include "selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h"
-

 class OnroadWindowSP : public OnroadWindow {
  Q_OBJECT
@@ -62,7 +59,6 @@ private:
  void createMapWidget();
  void createOnroadSettingsWidget();
  void mousePressEvent(QMouseEvent* e) override;
-  // AnnotatedCameraWidgetSP *nvg;
  QWidget *map = nullptr;
  QWidget *onroad_settings = nullptr;

--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Carlin Hefner	3126714df6	Tesla: set enableBsm flag for 3 and Y (#403 ) Set enableBsm flag for Tesla 3Y	2024-08-13 16:18:42 -04:00
Jason Wen	86220b4327	Merge branch 'master' into SP-165-tesla-port	2024-08-03 17:41:51 -08:00
Jason Wen	54c257bcc8	Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into SP-165-tesla-port # Conflicts: # panda # selfdrive/car/tesla/radar_interface.py # selfdrive/car/tesla/teslacan.py	2024-08-03 21:35:31 -04:00
Jason Wen	6552f836ab	oops	2024-08-02 18:54:10 -04:00
Jason Wen	917cdd97d7	disengage MADS if user overriding	2024-08-02 18:52:14 -04:00
Jason Wen	81830c8133	MADS? MADS!	2024-08-02 17:53:20 -04:00
Jason Wen	1fe12dbd7b	Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into SP-165-tesla-port	2024-08-02 16:41:50 -04:00
Jason Wen	f0424c8b8e	not needed probably	2024-08-02 14:47:36 -04:00
Carlin Hefner	b23fd97254	Remove dangling imports (#369 )	2024-08-01 06:25:09 -04:00
Carlin Hefner	982293e99b	Disable cruise main state detection for Tesla (#371 )	2024-07-31 19:09:48 -04:00
Carlin Hefner	6ea93cfb75	Add more Tesla fingerprints (#370 ) Add 3Y fingerprints from lukas fork	2024-07-31 19:08:57 -04:00
Jason Wen	c003298fd7	new button events	2024-07-31 00:18:31 -04:00
Jason Wen	ca6356f614	Merge branch 'master' into SP-165-tesla-port # Conflicts: # panda	2024-07-31 00:12:24 -04:00
Jason Wen	a874c839f6	Merge branch 'master-priv' into SP-165-tesla-port	2024-07-11 11:46:43 -04:00
Jason Wen	0ca1c9f85a	Tesla: Car Port for Model 3 and Model Y	2024-07-10 13:04:35 -04:00