test

This reverts commit a0fe916e4b.

CHANGELOGS.md

@@ -14,6 +14,9 @@ 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
@@ -28,20 +31,33 @@ 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)
@@ -79,6 +95,8 @@ 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
@@ -86,6 +104,8 @@ 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

README.md

@@ -48,6 +48,7 @@ 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
@@ -114,12 +115,40 @@ 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
+  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
   ```
 
 After running the command to install the desired branch, your comma device should reboot.
@@ -194,7 +223,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 sunnypilot completely when `OFF` **(strictly enforced in panda safety code)**
+* `CRUISE (MAIN)` button disables ACC/SCC 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:
@@ -326,7 +355,7 @@ Example:
 
 ---
 
-How-To instructions can be found in [HOW-TOS.md](HOW-TOS.md).
+How-To instructions can be found in [HOW-TOS.md](https://github.com/sunnyhaibin/openpilot/blob/(!)README/HOW-TOS.md).
 
 </details>
 

cereal/car.capnp

@@ -137,6 +137,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     speedLimitPreActive @139;
     speedLimitConfirmed @140;
     torqueNNLoad @141;
+    hyundaiRadarTracksAvailable @142;
+    spAutoBrakeHold @143;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;
@@ -250,7 +252,7 @@ struct CarState {
   struct CustomStockLong {
     cruiseButton @0 :Int16;
     finalSpeedKph @1 :Float32;
-    vCruiseKphPrev @2 :Float32;
+    vCruiseKphPrevDEPRECATED @2 :Float32;
     targetSpeed @3 :Float32;
     vSetDis @4 :Float32;
     speedDiff @5 :Float32;

cereal/custom.capnp

@@ -16,6 +16,7 @@ enum LongitudinalPersonalitySP {
   moderate @1;
   standard @2;
   relaxed @3;
+  overtake @4;
 }
 
 enum AccelerationPersonality {
@@ -34,11 +35,17 @@ 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;
@@ -90,8 +97,10 @@ struct LongitudinalPlanSP @0xaedffd8f31e7b55d {
   desiredTF @13 :Float32;
   notSpeedLimit @14 :Int16;
   e2eX @15 :List(Float32);
-  e2eBlended @18 :Text;
+  e2eBlendedDEPRECATED @18 :Text;
   e2eStatus @22 :Bool;
+  mpcSource @23 :MpcSource;
+  dynamicExperimentalControl @24 :Bool;
 
   distToTurn @7 :Float32;
   turnSpeed @8 :Float32;

common/params.cc

@@ -232,6 +232,7 @@ 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},
@@ -261,9 +262,15 @@ 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},
@@ -280,6 +287,7 @@ 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},
@@ -290,8 +298,11 @@ 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},
@@ -316,10 +327,13 @@ 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},

release/files_pc

@@ -0,0 +1,4 @@
+third_party/libyuv/x86_64/**
+third_party/snpe/x86_64/**
+third_party/snpe/x86_64-linux-clang/**
+third_party/acados/x86_64/**

release/files_tici

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

selfdrive/car/__init__.py

@@ -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)[2]
+  dat = packer.make_can_msg("GAS_COMMAND", 0, values)[1]
 
   checksum = crc8_pedal(dat[:-1])
   values["CHECKSUM_PEDAL"] = checksum

selfdrive/car/card.py

@@ -60,6 +60,7 @@ 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
@@ -67,6 +68,8 @@ 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

selfdrive/car/chrysler/interface.py

@@ -98,10 +98,9 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, 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
@@ -114,16 +113,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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
     else:
       self.CS.madsEnabled = False
-    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)
+    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS.madsEnabled)
 
     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)
+        self.get_sp_cancel_cruise_state()
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       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:
@@ -136,7 +135,7 @@ class CarInterface(CarInterfaceBase):
         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, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,
@@ -146,7 +145,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(self.CS, ret, events)
+    events, ret = self.create_sp_events(ret, events)
 
     # Low speed steer alert hysteresis logic
     if self.CP.carFingerprint in RAM_DT:
@@ -162,9 +161,7 @@ class CarInterface(CarInterfaceBase):
     if self.low_speed_alert:
       events.add(car.CarEvent.EventName.belowSteerSpeed)
 
-    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+    ret.customStockLong = self.update_custom_stock_long()
-                                                           self.CC.target_speed, self.CC.v_set_dis,
-                                                           self.CC.speed_diff, self.CC.button_type)
 
     ret.events = events.to_msg()
 

selfdrive/car/ford/carcontroller.py

@@ -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
+from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags, FordFlagsSP
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -35,6 +35,9 @@ 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 = []
@@ -74,7 +77,10 @@ class CarController(CarControllerBase):
         # TODO: extended mode
         mode = 1 if CC.latActive else 0
         counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
-        can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
+        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))
       else:
         can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))
 

selfdrive/car/ford/carstate.py

@@ -24,6 +24,7 @@ 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}
 
@@ -72,6 +73,10 @@ 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
 
@@ -129,6 +134,16 @@ 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 = [
@@ -185,6 +200,11 @@ 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),

selfdrive/car/ford/interface.py

@@ -3,7 +3,8 @@ 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.selfdrive.car.ford.values import Ecu, FordFlags
+from openpilot.common.params import Params
+from openpilot.selfdrive.car.ford.values import Ecu, FordFlags, FordFlagsSP
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -18,13 +19,15 @@ 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:
@@ -51,6 +54,13 @@ 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:
@@ -79,10 +89,9 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
-                                                            self.CS.button_events, c.vCruise)
 
     if ret.cruiseState.available:
       if self.enable_mads:
@@ -90,15 +99,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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, 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.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+        self.get_sp_cancel_cruise_state()
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       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:
@@ -106,7 +115,7 @@ class CarInterface(CarInterfaceBase):
         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, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,
@@ -115,7 +124,7 @@ class CarInterface(CarInterfaceBase):
 
     events = self.create_common_events(ret, c, extra_gears=[GearShifter.manumatic], pcm_enable=False)
 
-    events, ret = self.create_sp_events(self.CS, ret, events)
+    events, ret = self.create_sp_events(ret, events)
 
     if not self.CS.vehicle_sensors_valid:
       events.add(car.CarEvent.EventName.vehicleSensorsInvalid)

selfdrive/car/ford/radar_interface.py

@@ -11,6 +11,7 @@ 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)
@@ -28,6 +29,9 @@ 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):
@@ -45,6 +49,10 @@ 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}")
 
@@ -68,11 +76,67 @@ 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]

selfdrive/car/ford/values.py

@@ -48,9 +48,14 @@ 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):
@@ -91,7 +96,7 @@ class FordPlatformConfig(PlatformConfig):
 
 @dataclass
 class FordCANFDPlatformConfig(FordPlatformConfig):
-  dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', None))
+  dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', RADAR.STEER_ASSIST_DATA))
 
   def init(self):
     super().init()

selfdrive/car/gm/carcontroller.py

@@ -1,4 +1,6 @@
 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
@@ -6,6 +8,7 @@ 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
@@ -37,6 +40,36 @@ 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
@@ -45,9 +78,40 @@ 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
 
@@ -148,6 +212,15 @@ 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:
@@ -161,3 +234,120 @@ class CarController(CarControllerBase):
 
     self.frame += 1
     return new_actuators, can_sends
+
+  # multikyd methods, sunnyhaibin logic
+  def get_cruise_buttons_status(self, CS):
+    if not CS.out.cruiseState.enabled or CS.cruise_buttons != 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

selfdrive/car/gm/interface.py

@@ -119,6 +119,7 @@ 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
@@ -204,8 +205,6 @@ 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 = [
@@ -214,21 +213,19 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
     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, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
-                                                            self.CS.button_events, c.vCruise)
 
     if ret.cruiseState.available:
       if self.enable_mads:
@@ -236,15 +233,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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
     else:
       self.CS.madsEnabled = False
 
-    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
+    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)
+        self.get_sp_cancel_cruise_state()
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       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:
@@ -252,7 +249,7 @@ class CarInterface(CarInterfaceBase):
         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, gap_button=bool(distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,
@@ -267,7 +264,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(self.CS, ret, events, enable_pressed=self.CS.accEnabled,
+    events, ret = self.create_sp_events(ret, events, enable_pressed=self.CS.accEnabled,
                                         enable_buttons=(ButtonType.decelCruise,))
 
     # Enabling at a standstill with brake is allowed
@@ -281,6 +278,8 @@ 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

selfdrive/car/honda/interface.py

@@ -267,10 +267,9 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
-                                                            self.CS.button_events, c.vCruise)
 
     if ret.cruiseState.available:
       if self.enable_mads:
@@ -278,7 +277,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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
     else:
       self.CS.madsEnabled = False
 
@@ -286,9 +285,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.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+        self.get_sp_cancel_cruise_state()
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       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
@@ -301,7 +300,7 @@ class CarInterface(CarInterfaceBase):
       elif not ret.cruiseState.enabled:
         self.CS.accEnabled = False
 
-    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_setting == 3))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,
@@ -313,7 +312,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(self.CS, ret, events)
+    events, ret = self.create_sp_events(ret, events)
 
     #if self.CP.pcmCruise:
     #  # we engage when pcm is active (rising edge)
@@ -330,9 +329,7 @@ class CarInterface(CarInterfaceBase):
     if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
       events.add(EventName.manualRestart)
 
-    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+    ret.customStockLong = self.update_custom_stock_long()
-                                                           self.CC.target_speed, self.CC.v_set_dis,
-                                                           self.CC.speed_diff, self.CC.button_type)
 
     ret.events = events.to_msg()
 

selfdrive/car/hyundai/carcontroller.py

@@ -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
+from openpilot.common.numpy_fast import clip, interp
 from openpilot.common.params import Params
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import DT_CTRL, 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 = ['longitudinalPlanSP']
+    sub_services = ['longitudinalPlan', 'longitudinalPlanSP']
     if CP.openpilotLongitudinalControl:
       sub_services.append('radarState')
     # TODO: Always true, prep for future conditional refactoring
@@ -94,9 +94,24 @@ class CarController(CarControllerBase):
     self.v_tsc = 0
     self.m_tsc = 0
     self.steady_speed = 0
-    self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
+    self.speeds = 0
+    self.v_target_plan = 0
+    self.custom_stock_planner_speed = self.param_s.get_bool("CustomStockLongPlanner")
     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
@@ -113,6 +128,10 @@ 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
@@ -122,14 +141,21 @@ 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,
@@ -190,7 +216,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, 0
+      addr, bus = 0x7d0, self.CAN.ECAN if self.CP.carFingerprint in CANFD_CAR else 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))
@@ -199,6 +225,9 @@ 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
@@ -225,9 +254,11 @@ 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))
+                                                           set_speed_in_units, hud_control, self.jerk_u, self.jerk_l))
           self.accel_last = accel
       else:
         # button presses
@@ -271,15 +302,13 @@ 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
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
+        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),
                                                         hud_control, set_speed_in_units, stopping,
-                                                        CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance))
+                                                        CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
@@ -446,6 +475,8 @@ 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:
@@ -453,3 +484,66 @@ 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

selfdrive/car/hyundai/fingerprints.py

@@ -1171,6 +1171,17 @@ 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',

selfdrive/car/hyundai/hyundaican.py

@@ -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, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
+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):
+                        CS, escc, CP, lead_distance, cb_lower, cb_upper):
   commands = []
 
   scc11_values = {
@@ -150,8 +150,8 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
   scc12_values = {
     "ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
     "StopReq": 1 if stopping else 0,
-    "aReqRaw": accel,
+    "aReqRaw": accel_raw,
-    "aReqValue": accel,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
+    "aReqValue": accel_val,  # 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, upper_jerk, idx, hud_control, se
   commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
 
   scc14_values = {
-    "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
+    "ComfortBandUpper": cb_upper, # stock usually is 0 but sometimes uses higher values
-    "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
+    "ComfortBandLower": cb_lower, # stock usually is 0 but sometimes uses higher values
     "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
-    "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
+    "JerkLowerLimit": lower_jerk, # 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": 2,
+    "SCCDrvModeRValue": 3,
     "SCC_Equip": 1,
     "Lead_Veh_Dep_Alert_USM": 2,
   }

selfdrive/car/hyundai/hyundaicanfd.py

@@ -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):
+def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, upper_jerk, lower_jerk):
   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": jerk if enabled else 1,
+    "JerkLowerLimit": lower_jerk,
-    "JerkUpperLimit": 3.0,
+    "JerkUpperLimit": upper_jerk,
 
     "ACC_ObjDist": 1,
     "ObjValid": 0,
@@ -172,6 +172,20 @@ 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
@@ -182,15 +196,7 @@ def create_adrv_messages(packer, CAN, frame):
   }
   ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
 
-  if frame % 2 == 0:
+  ret.extend(create_fca_warning_light(packer, CAN, frame))
-    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 = {

selfdrive/car/hyundai/interface.py

@@ -1,6 +1,8 @@
+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, \
@@ -91,7 +93,7 @@ class CarInterface(CarInterfaceBase):
 
     # *** longitudinal control ***
     if candidate in CANFD_CAR:
-      ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR | NON_SCC_CAR)
+      ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | NON_SCC_CAR)
       if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC and not hda2:
         ret.spFlags |= HyundaiFlagsSP.SP_CAMERA_SCC_LEAD.value
     else:
@@ -102,13 +104,20 @@ class CarInterface(CarInterfaceBase):
     ret.pcmCruise = not ret.openpilotLongitudinalControl
 
     ret.stoppingControl = True
-    ret.startingState = True
     ret.vEgoStarting = 0.1
-    ret.startAccel = 1.0
+    ret.startAccel = 1.6
     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 ***
@@ -117,6 +126,8 @@ 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]
 
@@ -125,7 +136,8 @@ class CarInterface(CarInterfaceBase):
 
       if ret.flags & HyundaiFlags.MANDO_RADAR and ret.radarUnavailable:
         ret.spFlags |= HyundaiFlagsSP.SP_RADAR_TRACKS.value
-        ret.radarUnavailable = False
+        if Params().get_bool("HyundaiRadarTracksAvailable"):
+          ret.radarUnavailable = False
 
     # *** panda safety config ***
     if candidate in CANFD_CAR:
@@ -142,6 +154,8 @@ 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
@@ -167,7 +181,8 @@ 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):
+    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):
       ret.flags |= HyundaiFlags.ALT_LIMITS.value
       ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS
 
@@ -177,16 +192,13 @@ 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, 0
+      addr, bus = 0x7d0, CanBus(CP).ECAN if CP.carFingerprint in CANFD_CAR else 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')
@@ -201,7 +213,23 @@ 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 = [
@@ -210,10 +238,9 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
-                                                            self.CS.button_events, c.vCruise)
 
     if ret.cruiseState.available:
       if not self.CP.pcmCruiseSpeed:
@@ -221,12 +248,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 \
+      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):
+        (any(b.type == ButtonType.altButton3 for b in self.CS.button_events) and not self.CP.pcmCruise)):
         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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+      self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
 
     if not ret.cruiseState.available and self.CS.out.cruiseState.available:
       self.CS.madsEnabled = False
@@ -234,15 +261,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.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+          self.get_sp_cancel_cruise_state()
       if not self.CP.pcmCruiseSpeed:
         if not ret.cruiseState.enabled:
-          self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+          self.get_sp_cancel_cruise_state()
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
 
-    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,
@@ -256,7 +283,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(self.CS, ret, events, main_enabled=True, allow_enable=allow_enable)
+    events, ret = self.create_sp_events(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.:
@@ -266,9 +293,10 @@ class CarInterface(CarInterfaceBase):
     if self.low_speed_alert and self.CS.madsEnabled:
       events.add(car.CarEvent.EventName.belowSteerSpeed)
 
-    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+    if self.CS.params_list.hyundai_radar_tracks_available and not self.CS.params_list.hyundai_radar_tracks_available_cache:
-                                                           self.CC.target_speed, self.CC.v_set_dis,
+      events.add(car.CarEvent.EventName.hyundaiRadarTracksAvailable)
-                                                           self.CC.speed_diff, self.CC.button_type)
+
+    ret.customStockLong = self.update_custom_stock_long()
 
     ret.events = events.to_msg()
 

selfdrive/car/hyundai/values.py

@@ -3,7 +3,7 @@ from dataclasses import dataclass, field
 from enum import Enum, IntFlag
 
 from cereal import car
-from panda.python import uds
+from panda.python import uds, Panda
 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):
+  def __init__(self, CP, vEgoRaw=100.):
     self.STEER_DELTA_UP = 3
     self.STEER_DELTA_DOWN = 7
     self.STEER_DRIVER_ALLOWANCE = 50
@@ -26,12 +26,13 @@ class CarControllerParams:
     self.STEER_STEP = 1  # 100 Hz
 
     if CP.carFingerprint in CANFD_CAR:
-      self.STEER_MAX = 270
+      upstream_taco = CP.safetyConfigs[-1].safetyParam & Panda.FLAG_HYUNDAI_UPSTREAM_TACO
-      self.STEER_DRIVER_ALLOWANCE = 250
+      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_DRIVER_MULTIPLIER = 2
-      self.STEER_THRESHOLD = 250
+      self.STEER_THRESHOLD = 250 if not upstream_taco else 350
-      self.STEER_DELTA_UP = 2
+      self.STEER_DELTA_UP = 2 if not upstream_taco else 10 if vEgoRaw < 11. else 2
-      self.STEER_DELTA_DOWN = 3
+      self.STEER_DELTA_DOWN = 3 if not upstream_taco else 10 if vEgoRaw < 11. else 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.
@@ -108,6 +109,7 @@ 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):
@@ -574,6 +576,12 @@ 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),

selfdrive/car/interfaces.py

@@ -244,8 +244,6 @@ 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
@@ -408,7 +406,7 @@ class CarInterfaceBase(ABC):
 
   @staticmethod
   def sp_configure_custom_torque_tune(ret, params):
-    ret.lateralTuning.torque.friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.01
+    ret.lateralTuning.torque.friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.001
     ret.lateralTuning.torque.latAccelFactor = float(params.get("TorqueMaxLatAccel", encoding="utf8")) * 0.01
     return ret
 
@@ -550,20 +548,20 @@ class CarInterfaceBase(ABC):
   def sp_v_cruise_initialized(v_cruise):
     return v_cruise != V_CRUISE_UNSET
 
-  def get_acc_mads(self, cruiseState_enabled, acc_enabled, mads_enabled):
+  def get_acc_mads(self, cs_out, mads_enabled):
     if self.CS.params_list.acc_mads_combo:
-      if not self.prev_acc_mads_combo and (cruiseState_enabled or acc_enabled):
+      if not self.prev_acc_mads_combo and (cs_out.cruiseState.enabled or self.CS.accEnabled):
         mads_enabled = True
-      self.prev_acc_mads_combo = (cruiseState_enabled or acc_enabled)
+      self.prev_acc_mads_combo = (cs_out.cruiseState.enabled or self.CS.accEnabled)
 
     return mads_enabled
 
-  def get_sp_v_cruise_non_pcm_state(self, cs_out, acc_enabled, button_events, vCruise,
+  def get_sp_v_cruise_non_pcm_state(self, cs_out, vCruise, acc_enabled,
                                     enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise),
                                     resume_button=(ButtonType.accelCruise, ButtonType.resumeCruise)):
 
     if cs_out.cruiseState.available:
-      for b in button_events:
+      for b in self.CS.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
@@ -578,9 +576,9 @@ class CarInterfaceBase(ABC):
 
     return acc_enabled
 
-  def get_sp_cancel_cruise_state(self, mads_enabled, acc_enabled=False):
+  def get_sp_cancel_cruise_state(self):
-    mads_enabled = False if not self.enable_mads or self.disengage_on_accelerator else mads_enabled
+    self.CS.madsEnabled = False if not self.enable_mads or self.disengage_on_accelerator else self.CS.madsEnabled
-    return mads_enabled, acc_enabled
+    self.CS.accEnabled = False
 
   def get_sp_pedal_disengage(self, cs_out):
     accel_pedal = cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator
@@ -588,24 +586,22 @@ 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, CS):
+  def get_sp_cruise_main_state(self, cs_out):
-    if not CS.control_initialized:
+    if not self.CS.control_initialized:
-      mads_enabled = False
+      return False
     elif not self.CS.params_list.mads_main_toggle:
-      mads_enabled = False
+      return False
     else:
-      mads_enabled = cs_out.cruiseState.available
+      return cs_out.cruiseState.available
 
-    return mads_enabled
+  def get_sp_started_mads(self, cs_out, mads_enabled):
-
+    if not cs_out.cruiseState.available and self.CS.out.cruiseState.available:
-  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 CS.madsEnabled
+      return mads_enabled
-    if not self.madsEnabledInit and CS.madsEnabled:
+    if not self.madsEnabledInit and self.CS.madsEnabled:
       self.madsEnabledInit = True
       self.last_mads_init = time.monotonic()
     if cs_out.gearShifter not in FORWARD_GEARS:
@@ -616,19 +612,16 @@ class CarInterfaceBase(ABC):
       self.madsEnabledInitPrev = True
       return cs_out.cruiseState.available
     else:
-      return CS.madsEnabled
+      return mads_enabled
 
-  def get_sp_common_state(self, cs_out, CS, gear_allowed=True, gap_button=False):
+  def get_sp_common_state(self, cs_out, gear_allowed=True):
-    cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
+    cs_out.cruiseState.enabled = self.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 CS.out.cruiseState.enabled:
+      if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
-        CS.madsEnabled = True
+        self.CS.madsEnabled = True
-      elif not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled:
+      elif not cs_out.cruiseState.enabled and self.CS.out.cruiseState.enabled:
-        CS.madsEnabled = False
+        self.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)
 
@@ -640,37 +633,24 @@ class CarInterfaceBase(ABC):
 
     cs_out.latActive = gear_allowed
 
-    if not CS.control_initialized:
+    if not self.CS.control_initialized:
-      CS.control_initialized = True
+      self.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 CS.madsEnabled:
+      if self.CS.madsEnabled:
-        CS.disengageByBrake = True
+        self.CS.disengageByBrake = True
 
-    cs_out.madsEnabled = CS.madsEnabled
+    cs_out.madsEnabled = self.CS.madsEnabled
-    cs_out.accEnabled = CS.accEnabled
+    cs_out.accEnabled = self.CS.accEnabled
-    cs_out.disengageByBrake = CS.disengageByBrake
+    cs_out.disengageByBrake = self.CS.disengageByBrake
     cs_out.brakeLightsDEPRECATED |= cs_out.brakePressed or cs_out.brakeHoldActive or cs_out.parkingBrake or cs_out.regenBraking
 
-    return cs_out, CS
+    return cs_out
 
-  # TODO: SP: use upstream's buttonEvents counter checks from controlsd
+  def create_sp_events(self, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
-  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)):
 
@@ -678,7 +658,7 @@ class CarInterfaceBase(ABC):
       if cs_out.disengageByBrake and cs_out.madsEnabled:
         enable_pressed = True
         enable_from_brake = True
-      CS.disengageByBrake = False
+      self.CS.disengageByBrake = False
       cs_out.disengageByBrake = False
 
     for b in cs_out.buttonEvents:
@@ -707,11 +687,11 @@ class CarInterfaceBase(ABC):
     if self.CP.pcmCruise:
       # do disable on button down
       if main_enabled:
-        if any(CS.main_buttons) and not cs_out.cruiseState.enabled:
+        if any(self.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 CS.out.cruiseState.enabled and allow_enable:
+      if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable:
         enable_pressed = True
         enable_pressed_long = True
       elif not cs_out.cruiseState.enabled:
@@ -731,6 +711,16 @@ 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
@@ -834,16 +824,6 @@ 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:
@@ -876,6 +856,13 @@ 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

selfdrive/car/mazda/interface.py

@@ -45,10 +45,9 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
-                                                            self.CS.button_events, c.vCruise)
 
     if ret.cruiseState.available:
       if self.enable_mads:
@@ -56,15 +55,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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, 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)
+        self.get_sp_cancel_cruise_state()
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       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:
@@ -72,7 +71,7 @@ class CarInterface(CarInterfaceBase):
         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, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,
@@ -83,16 +82,14 @@ 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(self.CS, ret, events)
+    events, ret = self.create_sp_events(ret, events)
 
     #if self.CS.lkas_disabled:
     #  events.add(EventName.lkasDisabled)
     if self.CS.low_speed_alert:
       events.add(EventName.belowSteerSpeed)
 
-    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+    ret.customStockLong = self.update_custom_stock_long()
-                                                           self.CC.target_speed, self.CC.v_set_dis,
-                                                           self.CC.speed_diff, self.CC.button_type)
 
     ret.events = events.to_msg()
 

selfdrive/car/nissan/interface.py

@@ -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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
     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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, 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.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
 
-    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     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(self.CS, ret, events)
+    events, ret = self.create_sp_events(ret, events)
 
     if self.CS.lkas_enabled:
       events.add(car.CarEvent.EventName.invalidLkasSetting)

selfdrive/car/param_manager.py

@@ -9,8 +9,11 @@ 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,
@@ -35,8 +38,11 @@ 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"),

selfdrive/car/subaru/interface.py

@@ -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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
     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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, 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.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
 
-    ret, self.CS = self.get_sp_common_state(ret, self.CS)
+    ret = self.get_sp_common_state(ret)
 
     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(self.CS, ret, events)
+    events, ret = self.create_sp_events(ret, events)
 
     ret.events = events.to_msg()
 

selfdrive/car/subaru/values.py

@@ -21,7 +21,7 @@ class CarControllerParams:
     self.STEER_DRIVER_FACTOR = 1       # from dbc
 
     if CP.flags & SubaruFlags.GLOBAL_GEN2:
-      self.STEER_MAX = 1000
+      self.STEER_MAX = 1600
       self.STEER_DELTA_UP = 40
       self.STEER_DELTA_DOWN = 40
     elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020:

selfdrive/car/sunnypilot/fingerprinting.py

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

selfdrive/car/sunnypilot_carname.json

@@ -29,6 +29,9 @@
   "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",
@@ -38,6 +41,8 @@
   "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",
@@ -101,6 +106,7 @@
   "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",

selfdrive/car/tests/routes.py

@@ -17,7 +17,6 @@ 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,
@@ -54,6 +53,7 @@ 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),

selfdrive/car/torque_data/substitute.toml

@@ -86,6 +86,7 @@ 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"

selfdrive/car/toyota/carcontroller.py

@@ -1,4 +1,7 @@
 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, \
@@ -13,6 +16,7 @@ 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
@@ -44,8 +48,11 @@ 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
@@ -55,6 +62,13 @@ 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
 
@@ -133,6 +147,41 @@ 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
@@ -149,7 +198,6 @@ 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:
@@ -168,6 +216,9 @@ 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)
@@ -286,3 +337,25 @@ 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

selfdrive/car/toyota/carstate.py

@@ -1,18 +1,19 @@
 import copy
 
-from cereal import car
+from cereal import car, custom
 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.
@@ -56,6 +57,11 @@ 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
@@ -79,6 +85,17 @@ 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):
@@ -114,6 +131,14 @@ 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:
@@ -176,6 +201,51 @@ 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"]
 
@@ -260,6 +330,9 @@ 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
@@ -391,12 +464,16 @@ 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:
@@ -463,4 +540,7 @@ 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)

selfdrive/car/toyota/interface.py

@@ -160,16 +160,16 @@ class CarInterface(CarInterfaceBase):
 
     sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
 
-    # hand tuned (July 1, 2024)
+    # hand tuned (August 12, 2024)
     def custom_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.01
+      ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.01
-      ret.stoppingDecelRate = 0.35
+      ret.stoppingDecelRate = 0.006
 
     def default_tss2_longitudinal_tuning():
       ret.vEgoStopping = 0.25
       ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+      ret.stoppingDecelRate = 0.002  # 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., 50]
+        tune.kiBP = [0.,  5.,   12.,  20.,  27.,  36.,  40.]
-        tune.kiV = [0.35, 0.23, 0.20, 0.17, 0.10, 0.07, 0.01]
+        tune.kiV = [0.34, 0.234, 0.20, 0.17, 0.105, 0.09, 0.08]
         custom_tss2_longitudinal_tuning()
       else:
         tune.kpV = [0.0]
@@ -195,6 +195,9 @@ 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
@@ -208,13 +211,10 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
     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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
     else:
       self.CS.madsEnabled = False
 
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       if not self.CP.pcmCruise:
         ret.cruiseState.enabled = self.CS.accEnabled
 
-    ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button))
+    ret = self.get_sp_common_state(ret)
 
     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(self.CS, ret, events)
+    events, ret = self.create_sp_events(ret, events)
 
     # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
     # the more accurate angle sensor signal is initialized
@@ -271,6 +271,11 @@ 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

selfdrive/car/toyota/toyotacan.py

@@ -130,3 +130,37 @@ 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)

selfdrive/car/toyota/values.py

@@ -16,7 +16,7 @@ PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
 
 
 class CarControllerParams:
-  ACCEL_MAX = 1.5  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
+  ACCEL_MAX = 2.0  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
   ACCEL_MIN = -3.5  # m/s2
 
   STEER_STEP = 1
@@ -64,6 +64,7 @@ class ToyotaFlagsSP(IntFlag):
   SP_ZSS = 1
   SP_ENHANCED_BSM = 2
   SP_NEED_DEBUG_BSM = 4
+  SP_AUTO_BRAKE_HOLD = 8
 
 
 class Footnote(Enum):

selfdrive/car/volkswagen/interface.py

@@ -114,26 +114,25 @@ 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)
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
-    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
+    self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, 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.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
     else:
       self.CS.madsEnabled = False
-    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)
+    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS.madsEnabled)
 
     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)
+        self.get_sp_cancel_cruise_state()
     if self.get_sp_pedal_disengage(ret):
-      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      self.get_sp_cancel_cruise_state()
       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:
@@ -141,8 +140,7 @@ class CarInterface(CarInterfaceBase):
         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 = self.get_sp_common_state(ret)
-                                            gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events))
 
     ret.buttonEvents = [
       *self.CS.button_events,
@@ -153,7 +151,7 @@ class CarInterface(CarInterfaceBase):
                                        pcm_enable=False,
                                        enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
 
-    events, ret = self.create_sp_events(self.CS, ret, events,
+    events, ret = self.create_sp_events(ret, events,
                                         enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
 
     # Low speed steer alert hysteresis logic
@@ -173,9 +171,7 @@ class CarInterface(CarInterfaceBase):
     if self.CC.eps_timer_soft_disable_alert:
       events.add(EventName.steerTimeLimit)
 
-    ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+    ret.customStockLong = self.update_custom_stock_long()
-                                                           self.CC.target_speed, self.CC.v_set_dis,
-                                                           self.CC.speed_diff, self.CC.button_type)
 
     ret.events = events.to_msg()
 

selfdrive/controls/controlsd.py

@@ -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
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature, CRUISE_LONG_PRESS
 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,17 +174,27 @@ 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()
 
@@ -489,7 +499,9 @@ class Controls:
   def state_transition(self, CS):
     """Compute conditional state transitions and execute actions on state transitions"""
 
-    self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, self.sm['longitudinalPlanSP'])
+    # 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'])
 
     # decrement the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
@@ -638,9 +650,15 @@ 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)
+      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, resume)
 
       # Steering PID loop and lateral MPC
       if self.model_use_lateral_planner:
@@ -717,11 +735,21 @@ 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):
-        self.personality = (self.personality - 1) % 3
+        if not self.experimental_mode_update:
-        self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
+          self.personality = (self.personality - 1) % 3
+          self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
+        self.experimental_mode_update = False
 
     return CC, lac_log
 
@@ -806,11 +834,36 @@ class Controls:
 
     # Curvature & Steering angle
     lp = self.sm['liveParameters']
-    lp_mono_time_svs = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2'
+    dh = '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
@@ -825,7 +878,7 @@ class Controls:
       controlsState.alertSound = current_alert.audible_alert
 
     controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
-    controlsState.lateralPlanMonoTime = self.sm.logMonoTime[lp_mono_time_svs]
+    controlsState.lateralPlanMonoTime = self.sm.logMonoTime[dh]
     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
@@ -863,6 +916,7 @@ 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
@@ -920,7 +974,8 @@ 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
+      self.experimental_mode = self.params.get_bool("ExperimentalMode") and (self.CP.openpilotLongitudinalControl or
+                                                                             (not self.CP.pcmCruiseSpeed and self.custom_stock_planner_speed))
       self.personality = self.read_personality_param()
       self.dynamic_personality = self.params.get_bool("DynamicPersonality")
       self.accel_personality = self.read_accel_personality_param()
@@ -929,9 +984,11 @@ 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):

selfdrive/controls/lib/drive_helpers.py

@@ -65,6 +65,10 @@ 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
 
@@ -75,7 +79,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}
+    self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0}
     self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
 
     self.is_metric_prev = None
@@ -84,12 +88,20 @@ 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, long_plan_sp):
+  def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, sp_override_speed, 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:
@@ -101,13 +113,33 @@ 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:
-        self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+        if enabled and sp_override_speed and CS.cruiseState.speed * CV.MS_TO_KPH < sp_override_speed:
-        self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+          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)
     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
@@ -137,7 +169,7 @@ class VCruiseHelper:
           long_press = True
           break
 
-    if button_type is None:
+    if button_type is None or button_type == ButtonType.gapAdjustCruise:
       return
 
     resume_button = ButtonType.accelCruise
@@ -202,6 +234,8 @@ 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:
@@ -220,8 +254,6 @@ 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":
@@ -234,6 +266,8 @@ 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
 
 

selfdrive/controls/lib/events.py

@@ -744,6 +744,14 @@ 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"),
@@ -1141,6 +1149,9 @@ 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")
+  }
 }
 
 

selfdrive/controls/lib/latcontrol_torque.py

@@ -83,7 +83,8 @@ class LatControlTorque(LatControl):
     self.torqued_override = self.param_s.get_bool("TorquedOverride")
     self._frame = 0
 
-    self.use_lateral_jerk = False  # TODO: make this a parameter in the UI
+    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
 
     # Twilsonco's Lateral Neural Network Feedforward
     self.use_nn = CI.has_lateral_torque_nn
@@ -140,11 +141,13 @@ 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.01
+      self.torque_params.friction = float(self.param_s.get("TorqueFriction", encoding="utf8")) * 0.001
 
   @property
   def pid_long_sp(self):
@@ -197,7 +200,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.use_steering_angle or lookahead_lateral_jerk == 0.0:
+        if self.nnff_no_lateral_jerk or 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
@@ -206,6 +209,7 @@ 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])
@@ -231,7 +235,14 @@ 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

selfdrive/controls/lib/longcontrol.py

@@ -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):
+                             should_stop, brake_pressed, cruise_standstill, resume):
   # Ignore cruise standstill if car has a gas interceptor
   cruise_standstill = cruise_standstill and not CP.enableGasInterceptorDEPRECATED
-  stopping_condition = should_stop
+  stopping_condition = should_stop and not resume
-  starting_condition = (not should_stop and
+  starting_condition = ((not should_stop or resume) 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):
+  def update(self, active, CS, a_target, should_stop, accel_limits, resume):
     """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)
+                                                       CS.cruiseState.standstill, resume)
     if self.long_control_state == LongCtrlState.off:
       self.reset()
       output_accel = 0.

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -3,7 +3,7 @@ import os
 import time
 import numpy as np
 from cereal import custom
-from openpilot.common.numpy_fast import clip
+from openpilot.common.numpy_fast import clip, interp
 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 = 6.0
+STOP_DISTANCE = 5.0
 
 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
@@ -63,45 +63,116 @@ 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.45
+    return 1.65
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 1.25
+    return 1.45
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    return 1.0
+    return 1.25
+  elif personality==custom.LongitudinalPersonalitySP.overtake:
+    return 0.25
   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,    11,   14.5, 15,   20,   20.01,  25,    25.01,  36,  36.01]
+    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.5,  1.5,  1.5,  1.6,  1.76, 1.76,   1.78,  1.78,   1.8, 1.8]
+    y_dist = [1.70, 1.70,  1.80,   1.80]
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    x_vel =  [0,    11,   14.5, 15,   20,   20.01,  25,    25.01,  36,  36.01]
+    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.40, 1.40, 1.40, 1.50, 1.60, 1.76,   1.76,  1.78,   1.8, 1.8]
+    y_dist = [1.65, 1.65,  1.75,   1.75]
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    x_vel =  [0,    11,   14.5, 15,   20,   20.01,  25,    25.01,  36,  36.01]
+    x_vel =  [0.,   22.,   22.01,  36.1]
-    y_dist = [1.3,  1.3,  1.3,  1.35, 1.35, 1.385,  1.385, 1.4,   1.4,  1.45]
+    y_dist = [1.45, 1.45,  1.55,   1.55]
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    x_vel =  [0,     5,     5.01,  11,    14.5,   15,    20,    20.01,  25, 25.01, 36,   36.01]
+    x_vel =  [0.,   19.7,  19.71,  36.1]
-    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]
+    y_dist = [1.00, 1.00,  1.25,   1.25]
   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)
 
@@ -299,12 +370,15 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
+  def set_weights(self, prev_accel_constraint=True, v_lead0 = 0., v_lead1 = 0., personality=custom.LongitudinalPersonalitySP.standard):
+    v_ego = self.x0[1]
     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, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
+      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]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * danger_zone_factor]
     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]
@@ -358,14 +432,18 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False):
+  def update(self, radarstate, v_cruise, prev_accel_constraint, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard,
+             dynamic_personality=False, overtaking_acceleration_assist=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.
@@ -493,4 +571,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)

selfdrive/controls/lib/longitudinal_planner.py

@@ -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
+from cereal import car, log, custom
 
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
@@ -12,6 +12,7 @@ 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
@@ -35,6 +36,7 @@ _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
 
+MpcSource = custom.MpcSource
 EventName = car.CarEvent.EventName
 
 
@@ -82,7 +84,8 @@ class LongitudinalPlanner:
     self.dt = dt
 
     self.a_desired = init_a
-    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
+    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_model_error = 0.0
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
@@ -129,10 +132,13 @@ class LongitudinalPlanner:
       self.read_param()
     self.param_read_counter += 1
     if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
-      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
+      self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
+      self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
+      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode()
     else:
       self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
+
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
@@ -153,8 +159,10 @@ 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=sm['controlsStateSP'].accelPersonality):
+    if self.accel_controller.is_enabled(accel_personality=custom.AccelerationPersonality.sport if overtaking_accel_engaged else
+                                                          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':
@@ -189,11 +197,12 @@ 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=sm['controlsStateSP'].personality)
+    self.mpc.set_weights(prev_accel_constraint, personality=custom.LongitudinalPersonalitySP.overtake if overtaking_accel_engaged else 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, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality)
+    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.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)
@@ -262,7 +271,8 @@ class LongitudinalPlanner:
     longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state
     longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.v_target)
 
-    longitudinalPlanSP.e2eBlended = self.mpc.mode
+    longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc
+    longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled()
 
     pm.send('longitudinalPlanSP', plan_sp_send)
 

selfdrive/controls/lib/sunnypilot/accel_controller.py

@@ -21,23 +21,23 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 
-# Last updated: July 1, 2024
+# Last updated: August 12, 2024
 
 from cereal import custom
 from openpilot.common.numpy_fast import interp
 
 AccelPersonality = custom.AccelerationPersonality
 
-# accel personality by @arne182 modified by cgw
+# accel personality by @arne182 modified by cgw and kumar
-_DP_CRUISE_MIN_V =       [-1.03,  -0.79,  -0.77,  -0.77,  -0.75,  -0.75,  -0.88,  -0.82]
+_DP_CRUISE_MIN_V =       [-1.0,  -1.0]
-_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_ECO =   [-1.0,  -1.0]
-_DP_CRUISE_MIN_V_SPORT = [-1.04,  -0.81,  -0.79,  -0.79,  -0.77,  -0.77,  -0.90,  -0.84]
+_DP_CRUISE_MIN_V_SPORT = [-1.0,  -1.0]
-_DP_CRUISE_MIN_BP =      [0.,     0.05,   0.1,    0.5,    8.33,   16.,    30.,    40.]
+_DP_CRUISE_MIN_BP =      [0., 20.]
 
-_DP_CRUISE_MAX_V =       [2.5, 2.5, 2.5, 1.70, 1.05, .81,  .625, .42,  .348, .12]
+_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.4, .80,   .68,  .53,  .32,  .20,  .085]
+_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 2.0, 1.65, 0.92, .532, .432, .32,  .095]
-_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 2.8, 2.4,  1.4,  1.0,  .89,  .75,  .50,  .2]
+_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.,  15.,  20.,  25.,  30.,  55.]
+_DP_CRUISE_MAX_BP =      [0.,  1.,  6.,  8.,   11.,  20.,  25.,  30.,  55.]
 
 
 class AccelController:

selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py

@@ -21,33 +21,39 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 #
-# Version = 2024-1-29
+# Version = 2024-7-11
 from common.numpy_fast import interp
-from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
+import numpy as np
 
-LEAD_WINDOW_SIZE = 5
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+
+LEAD_WINDOW_SIZE = 4
 LEAD_PROB = 0.6
 
-SLOW_DOWN_WINDOW_SIZE = 5
+SLOW_DOWN_WINDOW_SIZE = 4
 SLOW_DOWN_PROB = 0.6
-SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-SLOW_DOWN_DIST = [20, 30., 50., 70., 80., 90., 105., 120.]
 
-SLOWNESS_WINDOW_SIZE = 20
+SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-SLOWNESS_PROB = 0.6
+SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
+
+SLOWNESS_WINDOW_SIZE = 12
+SLOWNESS_PROB = 0.5
 SLOWNESS_CRUISE_OFFSET = 1.05
 
-DANGEROUS_TTC_WINDOW_SIZE = 5
+DANGEROUS_TTC_WINDOW_SIZE = 3
-DANGEROUS_TTC = 2.0
+DANGEROUS_TTC = 2.3
 
-HIGHWAY_CRUISE_KPH = 75
+HIGHWAY_CRUISE_KPH = 70
 
 STOP_AND_GO_FRAME = 60
 
 SET_MODE_TIMEOUT = 10
 
-MPC_FCW_WINDOW_SIZE = 5
+MPC_FCW_WINDOW_SIZE = 10
-MPC_FCW_PROB = 0.6
+MPC_FCW_PROB = 0.5
+
+V_ACC_MIN = 9.72
 
 
 class SNG_State:
@@ -77,29 +83,51 @@ 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
 
-    self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
+    # Use weighted moving average for filtering leads
+    self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
     self._has_lead_filtered = False
     self._has_lead_filtered_prev = False
 
-    self._slow_down_gmac = GenericMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
+    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
     self._has_slow_down = False
 
     self._has_blinkers = False
 
-    self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
     self._has_slowness = False
 
     self._has_nav_instruction = False
 
-    self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
     self._has_dangerous_ttc = False
 
     self._v_ego_kph = 0.
@@ -113,13 +141,50 @@ class DynamicExperimentalController:
     self._sng_transit_frame = 0
     self._sng_state = SNG_State.off
 
-    self._mpc_fcw_gmac = GenericMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
+    self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
     self._has_mpc_fcw = False
     self._mpc_fcw_crash_cnt = 0
 
     self._set_mode_timeout = 0
     pass
 
+
+  def _adaptive_slowdown_threshold(self):
+    """
+    Adapts the slow down threshold based on vehicle speed and recent behavior.
+    """
+    return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.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
@@ -128,18 +193,27 @@ class DynamicExperimentalController:
 
     # fcw detection
     self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
-    self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() >= MPC_FCW_PROB
+    self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB
 
     # nav enable detection
     self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
 
-    # lead detection
+    # lead detection with smoothing
     self._lead_gmac.add_data(lead_one.status)
-    self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_PROB
+    self._has_lead_filtered = self._lead_gmac.get_weighted_average() > LEAD_PROB
+    #lead_prob = self._lead_gmac.get_weighted_average() or 0
+    #self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
 
-    # slow down detection
+    # adaptive 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))
+    adaptive_threshold = self._adaptive_slowdown_threshold()
-    self._has_slow_down = self._slow_down_gmac.get_moving_average() >= SLOW_DOWN_PROB
+    slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
+    self._slow_down_gmac.add_data(slow_down_trigger)
+    self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB
+
+    # anomaly detection for slow down events
+    if self._anomaly_detection(self._slow_down_gmac.data):
+      # Handle anomaly: potentially log it, adjust behavior, or issue a warning
+      self._has_slow_down = False  # Reset slow down if anomaly detected
 
     # blinker detection
     self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
@@ -161,7 +235,7 @@ class DynamicExperimentalController:
     # slowness detection
     if not self._has_standstill:
       self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
-      self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB
+      self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB
 
     # dangerous TTC detection
     if not self._has_lead_filtered and self._has_lead_filtered_prev:
@@ -171,14 +245,14 @@ class DynamicExperimentalController:
     if self._has_lead and car_state.vEgo >= 0.01:
       self._dangerous_ttc_gmac.add_data(lead_one.dRel/car_state.vEgo)
 
-    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_moving_average() is not None and self._dangerous_ttc_gmac.get_moving_average() <= DANGEROUS_TTC
+    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC
 
     # keep prev values
     self._has_standstill_prev = self._has_standstill
     self._has_lead_filtered_prev = self._has_lead_filtered
     self._frame += 1
 
-  def _blended_priority_mode(self):
+  def _radarless_mode(self):
     # when mpc fcw crash prob is high
     # use blended to slow down quickly
     if self._has_mpc_fcw:
@@ -190,21 +264,21 @@ class DynamicExperimentalController:
       self._set_mode('blended')
       return
 
-    # when blinker is on and speed is driving below highway cruise speed: blended
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
     # we dont want it to switch mode at higher speed, blended may trigger hard brake
-    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
+    #if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
-      self._set_mode('blended')
+    #  self._set_mode('blended')
-      return
+    #  return
 
     # when at highway cruise and SNG: blended
     # ensuring blended mode is used because acc is bad at catching SNG lead car
     # especially those who accel very fast and then brake very hard.
-    if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
+    #if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
-      self._set_mode('blended')
+    #  self._set_mode('blended')
-      return
+    #  return
 
     # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
     if self._has_standstill:
       self._set_mode('blended')
       return
@@ -226,9 +300,9 @@ class DynamicExperimentalController:
       self._set_mode('acc')
       return
 
-    self._set_mode('blended')
+    self._set_mode('acc')
 
-  def _acc_priority_mode(self):
+  def _radar_mode(self):
     # when mpc fcw crash prob is high
     # use blended to slow down quickly
     if self._has_mpc_fcw:
@@ -236,18 +310,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:
+    if self._has_lead_filtered and not self._has_standstill:
-    #  self._set_mode('acc')
+      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 blinker is on and speed is driving below V_ACC_MIN: blended
+    # we dont want it to switch mode at higher speed, blended may trigger hard brake
+    #if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
     # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
     if self._has_standstill:
       self._set_mode('blended')
       return
@@ -270,17 +344,22 @@ class DynamicExperimentalController:
 
     self._set_mode('acc')
 
-  def get_mpc_mode(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
+  def update(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
     if self._is_enabled:
       self._update(car_state, lead_one, md, controls_state, maneuver_distance)
       if radar_unavailable:
-        self._blended_priority_mode()
+        self._radarless_mode()
       else:
-        self._acc_priority_mode()
+        self._radar_mode()
-
+    self._mode_changed = self._mode != self._mode_prev
     self._mode_prev = self._mode
+
+  def get_mpc_mode(self):
     return self._mode
 
+  def has_changed(self):
+    return self._mode_changed
+
   def set_enabled(self, enabled):
     self._is_enabled = enabled
 

selfdrive/controls/plannerd.py

@@ -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'] + lateral_planner_svs,
+                            'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP', 'driverMonitoringState'] + lateral_planner_svs,
                            poll='modelV2', ignore_avg_freq=['radarState'])
 
   while True:

selfdrive/locationd/torqued.py

@@ -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.01
+        self.offline_friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.001
         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)

selfdrive/ui/qt/api.cc

@@ -145,4 +145,4 @@ void HttpRequest::requestFinished() {
 QNetworkAccessManager *HttpRequest::nam() {
   static QNetworkAccessManager *networkAccessManager = new QNetworkAccessManager(qApp);
   return networkAccessManager;
-}
+}

selfdrive/ui/qt/api.h

@@ -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();
-};
+};

selfdrive/ui/qt/home.cc

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

selfdrive/ui/qt/home.h

@@ -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);
-};
+};

selfdrive/ui/qt/offroad/settings.cc

@@ -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

selfdrive/ui/qt/offroad/settings.h

@@ -91,6 +91,7 @@ protected:
   virtual void updateToggles();
 };
 
+
 class SoftwarePanel : public ListWidget {
   Q_OBJECT
 public:

selfdrive/ui/qt/offroad/software_settings.cc

@@ -26,7 +26,7 @@
 
 
 void SoftwarePanel::checkForUpdates() {
-  std::system("pkill -SIGUSR1 -f system.updated.updated");
+  std::system("pkill -SIGUSR1 -f 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 system.updated.updated");
+      std::system("pkill -SIGHUP -f updated");
     }
   });
   addItem(downloadBtn);
@@ -162,4 +162,4 @@ void SoftwarePanel::updateLabels() {
   installBtn->setDescription(QString::fromStdString(params.get("UpdaterNewReleaseNotes")));
 
   update();
-}
+}

selfdrive/ui/qt/offroad_home.cc

@@ -2,6 +2,7 @@
 
 #include <QHBoxLayout>
 #include <QMouseEvent>
+#include <QVBoxLayout>
 
 #include "selfdrive/ui/qt/offroad/experimental_mode.h"
 #include "selfdrive/ui/qt/util.h"

selfdrive/ui/qt/offroad_home.h

@@ -1,12 +1,19 @@
 #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"

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -385,4 +385,4 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
 
   ui_update_params(uiState());
   prev_draw_t = millis_since_boot();
-}
+}

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -50,4 +50,4 @@ protected:
 
   double prev_draw_t = 0;
   FirstOrderFilter fps_filter;
-};
+};

selfdrive/ui/qt/onroad/buttons.cc

@@ -26,7 +26,8 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals
 
 void ExperimentalButton::changeMode() {
   const auto cp = (*uiState()->sm)["carParams"].getCarParams();
-  bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
+  bool can_change = (hasLongitudinalControl(cp) || (!cp.getPcmCruiseSpeed() && params.getBool("CustomStockLongPlanner")))
+                    && params.getBool("ExperimentalModeConfirmed");
   if (can_change) {
     params.putBool("ExperimentalMode", !experimental_mode);
   }

selfdrive/ui/qt/onroad/buttons.h

@@ -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);

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -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));
-}
+}

selfdrive/ui/qt/onroad/onroad_home.h

@@ -26,4 +26,4 @@ protected:
 protected slots:
   virtual void offroadTransition(bool offroad);
   virtual void updateState(const UIState &s);
-};
+};

selfdrive/ui/qt/request_repeater.h

@@ -20,4 +20,4 @@ private:
   Params params;
   QTimer *timer;
   QString prevResp;
-};
+};

selfdrive/ui/qt/sidebar.h

@@ -65,4 +65,4 @@ protected:
 
 private:
   std::unique_ptr<PubMaster> pm;
-};
+};

selfdrive/ui/qt/text.cc

@@ -61,4 +61,4 @@ int main(int argc, char *argv[]) {
   )");
 
   return a.exec();
-}
+}

selfdrive/ui/qt/util.h

@@ -59,4 +59,4 @@ private:
   QFileSystemWatcher *watcher;
   QHash<QString, QString> params_hash;
   Params params;
-};
+};

selfdrive/ui/qt/widgets/ssh_keys.h

@@ -35,4 +35,4 @@ private:
 
   void refresh();
   void getUserKeys(const QString &username);
-};
+};

selfdrive/ui/sunnypilot/SConscript

@@ -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",
 ]
 

selfdrive/ui/sunnypilot/qt/api.cc

@@ -33,6 +33,7 @@ Last updated: July 29, 2024
 #include <QApplication>
 #include <QJsonDocument>
 
+#include <memory>
 #include <string>
 #include <common/swaglog.h>
 

selfdrive/ui/sunnypilot/qt/api.h

@@ -26,6 +26,11 @@ 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"

selfdrive/ui/sunnypilot/qt/home.cc

@@ -28,10 +28,13 @@ 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){    

selfdrive/ui/sunnypilot/qt/home.h

@@ -27,9 +27,14 @@ 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"
@@ -37,6 +42,7 @@ 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"

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

@@ -30,6 +30,7 @@ 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"
 

selfdrive/ui/sunnypilot/qt/offroad/settings/monitoring_settings.h

@@ -28,9 +28,11 @@ 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

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

@@ -57,6 +57,12 @@ 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"),
@@ -223,6 +229,7 @@ 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>"
@@ -261,16 +268,22 @@ 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(false);
+      experimental_mode_toggle->setEnabled(custom_stock_long_planner->isToggled());
-      long_personality_setting->setEnabled(false);
+      experimental_mode_toggle->setDescription(e2e_description);
-      accel_personality_setting->setEnabled(false);
+      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");
+      }
       params.remove("ExperimentalLongitudinalEnabled");
-      params.remove("ExperimentalMode");
     } else {
       // no long for now
       experimental_mode_toggle->setEnabled(false);
@@ -295,6 +308,7 @@ 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");
     }
@@ -302,6 +316,7 @@ 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 {
@@ -310,6 +325,7 @@ void TogglesPanelSP::updateToggles() {
     custom_stock_long_toggle->setVisible(false);
     dec_toggle->setVisible(false);
     dynamic_personality_toggle->setVisible(false);
+    custom_stock_long_planner->setVisible(false);
   }
 }
 

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

@@ -49,6 +49,12 @@ 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)"),
@@ -91,12 +97,24 @@ 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"),
@@ -298,7 +316,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
       list->addItem(dlp_settings);
     }
 
-    if (param == "VisionCurveLaneless") {
+    if (param == "OvertakingAccelerationAssist") {
       list->addItem(laneChangeSettingsLayout);
       list->addItem(horizontal_line());
 
@@ -345,6 +363,17 @@ 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()) {
@@ -366,6 +395,7 @@ 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) {
@@ -470,6 +500,8 @@ 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";
@@ -549,6 +581,7 @@ 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);
@@ -556,18 +589,21 @@ 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);
   }
@@ -577,7 +613,7 @@ void SunnypilotPanel::updateToggles() {
   }
 
   // toggle names to update when EnforceTorqueLateral is flipped
-  std::vector<std::string> torqueLateralGroup{"CustomTorqueLateral", "LiveTorque", "LiveTorqueRelaxed", "TorquedOverride"};
+  std::vector<std::string> torqueLateralGroup{"CustomTorqueLateral", "TorqueLateralJerk", "LiveTorque", "LiveTorqueRelaxed", "TorquedOverride"};
   for (const auto& torqueLateralToggle : torqueLateralGroup) {
     if (toggles.find(torqueLateralToggle) != toggles.end()) {
       if (nnff_toggle->isToggled()) {
@@ -594,6 +630,24 @@ 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());
@@ -622,13 +676,14 @@ 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, 50}) {
+  {0, 500},
+  5) {
 
   refresh();
 }
 
 void TorqueFriction::refresh() {
-  float torqueFrictionVal = QString::fromStdString(params.get("TorqueFriction")).toInt() * 0.01;
+  float torqueFrictionVal = QString::fromStdString(params.get("TorqueFriction")).toInt() * 0.001;
   setTitle("FRICTION - " + (params.getBool("TorquedOverride") ? tr("Real-time and Offline") : tr("Offline Only")));
   setLabel(QString::number(torqueFrictionVal));
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.h

@@ -29,6 +29,7 @@ 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"

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

@@ -101,13 +101,24 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
 
   // Hyundai/Kia/Genesis
   addItem(new LabelControlSP(tr("Hyundai/Kia/Genesis")));
-  auto hkgSmoothStop = new ParamControlSP(
+  auto hkgCustomLongTuning = new ParamControlSP(
-    "HkgSmoothStop",
+    "HkgCustomLongTuning",
-    tr("HKG CAN: Smoother Stopping Performance (Beta)"),
+    tr("HKG: Custom Tuning for New Longitudinal API"),
-    tr("Smoother stopping behind a stopped car or desired stopping event. This is only applicable to HKG CAN platforms using openpilot longitudinal control."),
+    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.")),
     "../assets/offroad/icon_blank.png");
-  hkgSmoothStop->setConfirmation(true, false);
+  hyundaiCruiseMainDefault->setConfirmation(true, false);
-  addItem(hkgSmoothStop);
+  addItem(hyundaiCruiseMainDefault);
 
   // Subaru
   addItem(new LabelControlSP(tr("Subaru")));
@@ -148,6 +159,18 @@ 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"),
@@ -180,6 +203,14 @@ 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(
@@ -193,8 +224,10 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
   // trigger offroadTransition when going onroad/offroad
   connect(uiStateSP(), &UIStateSP::offroadTransition, [=](bool offroad) {
     is_onroad = !offroad;
-    hkgSmoothStop->setEnabled(offroad);
+    hkgCustomLongTuning->setEnabled(offroad);
+    hyundaiCruiseMainDefault->setEnabled(offroad);
     toyotaTss2LongTune->setEnabled(offroad);
+    toyotaAbh->setEnabled(offroad);
     toyotaEnhancedBsm->setEnabled(offroad);
     toyotaSngHack->setEnabled(offroad);
     volkswagenCCOnly->setEnabled(offroad);
@@ -223,6 +256,17 @@ 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") {

selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.h

@@ -28,8 +28,10 @@ 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
@@ -67,6 +69,7 @@ private:
   Params params;
   bool is_onroad = false;
 
+  ParamControlSP *hyundaiCruiseMainDefault;
   ParamControlSP *stockLongToyota;
   ParamControlSP *toyotaEnhancedBsm;
 

selfdrive/ui/sunnypilot/qt/offroad_home.cc

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

selfdrive/ui/sunnypilot/qt/offroad_home.h

@@ -31,6 +31,7 @@ Last updated: July 29, 2024
 #include "selfdrive/ui/qt/maps/map_settings.h"
 #endif
 
+
 class OffroadHomeSP : public OffroadHome {
   Q_OBJECT
 

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

@@ -216,6 +216,7 @@ 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;
@@ -320,8 +321,8 @@ void AnnotatedCameraWidgetSP::updateState(const UIStateSP &s) {
   // TODO: Add toggle variables to cereal, and parse from cereal
   longitudinalPersonality = s.scene.longitudinal_personality;
   dynamicLaneProfile = s.scene.dynamic_lane_profile;
-  mpcMode = QString::fromStdString(lp_sp.getE2eBlended());
+  const auto mpc_source = lp_sp.getMpcSource();
-  mpcMode = (mpcMode == "blended") ? mpcMode.replace(0, 1, mpcMode[0].toUpper()) : mpcMode.toUpper();
+  mpcSource = mpc_source == cereal::MpcSource::BLENDED ? QString(tr("blended")) : QString(tr("acc"));
 
   static int reverse_delay = 0;
   bool reverse_allowed = false;
@@ -523,10 +524,32 @@ void AnnotatedCameraWidgetSP::drawHud(QPainter &p) {
 
   // current speed
   if (!hideVEgoUi) {
+    // Set up the font for the speed text
     p.setFont(InterFont(176, QFont::Bold));
-    drawColoredText(p, rect().center().x(), 210, speedStr, brakeLights ? QColor(0xff, 0, 0, 255) : QColor(0xff, 0xff, 0xff, 255));
+
+    // 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
     p.setFont(InterFont(66));
-    drawText(p, rect().center().x(), 290, speedUnit, 200);
+    drawText(p, centerX, 290, speedUnit, 200);
   }
 
   if (!reversing) {
@@ -1061,7 +1084,7 @@ void AnnotatedCameraWidgetSP::drawFeatureStatusText(QPainter &p, int x, int y) {
     p.setBrush(dec_color);
     p.drawEllipse(dec_btn);
     QString dec_status_text;
-    dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcMode).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
+    dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcSource).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
     p.setPen(QPen(shadow_color, 2));
     p.drawText(x + drop_shadow_size, y + drop_shadow_size, dec_status_text);
     p.setPen(QPen(text_color, 2));
@@ -1165,7 +1188,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.2));
+  painter.setBrush(QColor::fromRgbF(1.0, 0.0, 0.0, 0.4));
   if (left_blindspot) painter.drawPolygon(scene.lane_barrier_vertices[0]);
   if (right_blindspot) painter.drawPolygon(scene.lane_barrier_vertices[1]);
 
@@ -1177,6 +1200,10 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
 
   // paint path
   QLinearGradient bg(0, height(), 0, 0);
+  const auto long_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+  bool exp_mode_path = (long_plan_sp.getDynamicExperimentalControl() && long_plan_sp.getMpcSource() == cereal::MpcSource::BLENDED) ||
+                       (!long_plan_sp.getDynamicExperimentalControl() && sm["controlsState"].getControlsState().getExperimentalMode());
+
   if (madsEnabled || car_state.getCruiseState().getEnabled()) {
     if (steerOverride && latActive) {
       bg.setColorAt(0.0, QColor::fromHslF(20 / 360., 0.94, 0.51, 0.17));
@@ -1185,7 +1212,7 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
     } else if (!(latActive || car_state.getCruiseState().getEnabled())) {
       bg.setColorAt(0, whiteColor());
       bg.setColorAt(1, whiteColor(0));
-    } else if (sm["controlsState"].getControlsState().getExperimentalMode()) {
+    } else if (exp_mode_path) {
       // The first half of track_vertices are the points for the right side of the path
       const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration().getX();
       const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration.size());
@@ -1286,46 +1313,25 @@ 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;
 
-  static const int ct_n = 1;
+  if (accel > 0) {
-  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);
-  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.setBrush(QColor(245, 0, 0, 200));
-    p.drawRect(ra);
+    scannerRect = QRect(0, widgetHeight / 2, scannerWidth, halfHeightAbs);
   }
+
+  p.drawRect(scannerRect);
 }
 
+
 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();

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

@@ -176,6 +176,7 @@ private:
   float latAccelFactorFiltered;
   float frictionCoefficientFiltered;
   bool liveValid;
+  bool ecoMode;
   // ############################## DEV UI END ##############################
 
   float btnPerc;
@@ -192,7 +193,7 @@ private:
   cereal::LongitudinalPlanSP::SpeedLimitControlState slcState;
   int longitudinalPersonality;
   int dynamicLaneProfile;
-  QString mpcMode;
+  QString mpcSource;
 
   int speed_limit_frame;
   bool slcShowSign = true;

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

@@ -26,6 +26,8 @@ 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"

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

@@ -29,6 +29,9 @@ 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
@@ -59,6 +62,7 @@ private:
   void createMapWidget();
   void createOnroadSettingsWidget();
   void mousePressEvent(QMouseEvent* e) override;
+  // AnnotatedCameraWidgetSP *nvg;
   QWidget *map = nullptr;
   QWidget *onroad_settings = nullptr;
 

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

@@ -193,7 +193,7 @@ void OnroadSettings::changeDynamicPersonality() {
 void OnroadSettings::changeDynamicExperimentalControl() {
   UISceneSP &scene = uiStateSP()->scene;
   const auto cp = (*uiStateSP()->sm)["carParams"].getCarParams();
-  bool can_change = hasLongitudinalControl(cp);
+  bool can_change = hasLongitudinalControl(cp) || (!cp.getPcmCruiseSpeed() && params.getBool("CustomStockLongPlanner"));
   if (can_change) {
     scene.dynamic_experimental_control = !scene.dynamic_experimental_control;
     params.putBool("DynamicExperimentalControl", scene.dynamic_experimental_control);

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

@@ -26,6 +26,7 @@ Last updated: July 29, 2024
 
 #include "selfdrive/ui/sunnypilot/qt/onroad/onroad_settings_panel.h"
 
+#include <QHBoxLayout>
 #include <QWidget>
 
 #include "selfdrive/ui/sunnypilot/qt/onroad/onroad_settings.h"

selfdrive/ui/sunnypilot/qt/sidebar.cc

@@ -34,6 +34,7 @@ Last updated: July 29, 2024
 #include "selfdrive/ui/sunnypilot/qt/util.h"
 #include "common/params.h"
 
+
 SidebarSP::SidebarSP(QWidget *parent) : Sidebar(parent) {
   // Because I know that stock sidebar makes this connection, I will disconnect it and connect it to the new updateState function
   QObject::disconnect(uiState(), &UIState::uiUpdate, this, &Sidebar::updateState);
@@ -122,6 +123,7 @@ void SidebarSP::updateState(const UIStateSP &s) {
   setProperty("sunnylinkStatus", QVariant::fromValue(sunnylinkStatus));
 }
 
+
 void SidebarSP::DrawSidebar(QPainter &p){
   Sidebar::DrawSidebar(p);
   // metrics

selfdrive/ui/sunnypilot/qt/sidebar.h

@@ -28,6 +28,8 @@ Last updated: July 29, 2024
 
 #include <memory>
 
+#include <QFrame>
+#include <QMap>
 #include "selfdrive/ui/qt/sidebar.h"
 
 #include "selfdrive/ui/sunnypilot/ui.h"

selfdrive/ui/sunnypilot/qt/ui_scene.h

@@ -25,6 +25,7 @@ Last updated: July 29, 2024
 ***/
 
 #pragma once
+#include "selfdrive/ui/ui.h"
 
 const float DRIVING_PATH_WIDE = 0.9;
 const float DRIVING_PATH_NARROW = 0.25;

selfdrive/ui/sunnypilot/qt/widgets/controls.h

@@ -31,6 +31,13 @@ Last updated: July 29, 2024
 #include <string>
 #include <vector>
 
+#include <QButtonGroup>
+#include <QFrame>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QPainter>
+#include <QPushButton>
+
 #include "common/params.h"
 #include "selfdrive/ui/qt/widgets/controls.h"
 #include "selfdrive/ui/qt/widgets/input.h"

selfdrive/ui/sunnypilot/sunnypilot_main.h

@@ -36,6 +36,7 @@ Last updated: July 29, 2024
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_settings.h"
 
+
 inline void ReplaceWidget(QWidget* old_widget, QWidget* new_widget) {
   if (old_widget && old_widget->parentWidget() && old_widget->parentWidget()->layout()) {
     old_widget->parentWidget()->layout()->replaceWidget(old_widget, new_widget);

selfdrive/ui/sunnypilot/ui.cc

@@ -34,6 +34,7 @@ Last updated: July 29, 2024
 
 #include "common/transformations/orientation.hpp"
 #include "common/params.h"
+#include "common/swaglog.h"
 #include "common/util.h"
 #include "common/watchdog.h"
 #include "selfdrive/ui/sunnypilot/qt/network/sunnylink/models/role_model.h"

selfdrive/ui/sunnypilot/ui.h

@@ -33,9 +33,11 @@ Last updated: July 29, 2024
 #include "selfdrive/ui/ui.h"
 
 #include "cereal/messaging/messaging.h"
+#include "common/timing.h"
 #include "qt/network/sunnylink/models/role_model.h"
 #include "qt/network/sunnylink/models/sponsor_role_model.h"
 #include "qt/network/sunnylink/models/user_model.h"
+#include "selfdrive/ui/sunnypilot/qt/ui_scene.h"
 #include "system/hardware/hw.h"
 
 const int UI_ROAD_NAME_MARGIN_X = 14;