diff --git a/CHANGELOGS.md b/CHANGELOGS.md
index 2fc99ab8d0..7aefc587a9 100644
--- a/CHANGELOGS.md
+++ b/CHANGELOGS.md
@@ -28,6 +28,9 @@ 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
@@ -45,6 +48,10 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
* 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)
@@ -82,6 +89,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
@@ -89,6 +98,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
diff --git a/README.md b/README.md
index 5986adbe33..f8941ed699 100644
--- a/README.md
+++ b/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.
+
+
+
+
+ SSH (More Versatile)
+
+
+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).
diff --git a/cereal/custom.capnp b/cereal/custom.capnp
index 0c32edcf1d..423009f502 100644
--- a/cereal/custom.capnp
+++ b/cereal/custom.capnp
@@ -16,6 +16,7 @@ enum LongitudinalPersonalitySP {
moderate @1;
standard @2;
relaxed @3;
+ overtake @4;
}
enum AccelerationPersonality {
@@ -44,6 +45,7 @@ struct ControlsStateSP @0x81c2f05a394cf4af {
personality @8 :LongitudinalPersonalitySP;
dynamicPersonality @9 :Bool;
accelPersonality @10 :AccelerationPersonality;
+ overtakingAccelerationAssist @11 :Bool;
lateralControlState :union {
indiState @1 :LateralINDIState;
diff --git a/common/params.cc b/common/params.cc
index 39abb8ed28..e2db085932 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -232,6 +232,7 @@ std::unordered_map keys = {
{"CustomMapboxTokenSk", PERSISTENT | BACKUP},
{"CustomOffsets", PERSISTENT | BACKUP},
{"CustomStockLong", PERSISTENT | BACKUP},
+ {"CustomStockLongPlanner", PERSISTENT | BACKUP},
{"CustomTorqueLateral", PERSISTENT | BACKUP},
{"DevUIInfo", PERSISTENT | BACKUP},
{"DisableOnroadUploads", PERSISTENT | BACKUP},
@@ -263,6 +264,7 @@ std::unordered_map keys = {
{"HideVEgoUi", PERSISTENT | BACKUP},
{"HkgCustomLongTuning", PERSISTENT | BACKUP},
{"HkgSmoothStop", PERSISTENT | BACKUP},
+ {"HyundaiCruiseMainDefault", PERSISTENT | BACKUP},
{"HotspotOnBoot", PERSISTENT},
{"HotspotOnBootConfirmed", PERSISTENT},
{"LastCarModel", PERSISTENT | BACKUP},
@@ -281,6 +283,7 @@ std::unordered_map keys = {
{"NavModelUrl", PERSISTENT | BACKUP},
{"NNFF", PERSISTENT | BACKUP},
{"NNFFCarModel", PERSISTENT | BACKUP},
+ {"NNFFNoLateralJerk", PERSISTENT | BACKUP},
{"OnroadScreenOff", PERSISTENT | BACKUP},
{"OnroadScreenOffBrightness", PERSISTENT | BACKUP},
{"OnroadScreenOffEvent", PERSISTENT | BACKUP},
@@ -291,8 +294,11 @@ std::unordered_map 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},
@@ -317,6 +323,7 @@ std::unordered_map keys = {
{"TermsVersionSunnypilot", PERSISTENT},
{"TorqueDeadzoneDeg", PERSISTENT | BACKUP},
{"TorqueFriction", PERSISTENT | BACKUP},
+ {"TorqueLateralJerk", PERSISTENT | BACKUP},
{"TorqueMaxLatAccel", PERSISTENT | BACKUP},
{"TorquedOverride", PERSISTENT | BACKUP},
{"ToyotaAutoHold", PERSISTENT | BACKUP},
diff --git a/release/files_pc b/release/files_pc
new file mode 100644
index 0000000000..f2bf090f2c
--- /dev/null
+++ b/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/**
diff --git a/release/files_tici b/release/files_tici
new file mode 100644
index 0000000000..18860e20af
--- /dev/null
+++ b/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
diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index ba3805b01f..a772d9c16a 100644
--- a/selfdrive/car/__init__.py
+++ b/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
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index d090adf694..e9f59817c9 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/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.))
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index e52454f81d..3b841c5bf9 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/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),
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index a3bbd7ecf5..5461590679 100644
--- a/selfdrive/car/ford/interface.py
+++ b/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:
diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py
index 209bbebae3..7218fc54b6 100644
--- a/selfdrive/car/ford/radar_interface.py
+++ b/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]
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index 04bd592e22..7af6ec607f 100644
--- a/selfdrive/car/ford/values.py
+++ b/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()
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index 350b9a7d9b..7b02e2500f 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/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
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 6ca796755e..35e4654597 100755
--- a/selfdrive/car/gm/interface.py
+++ b/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
@@ -240,7 +241,7 @@ class CarInterface(CarInterfaceBase):
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)
if self.get_sp_pedal_disengage(ret):
@@ -281,6 +282,10 @@ class CarInterface(CarInterfaceBase):
if ret.vEgo < self.CP.minSteerSpeed and self.CS.madsEnabled:
events.add(EventName.belowSteerSpeed)
+ ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
+ self.CC.target_speed, self.CC.v_set_dis,
+ self.CC.speed_diff, self.CC.button_type)
+
ret.events = events.to_msg()
return ret
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index d1c2a8e42d..849f2fced3 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -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,7 +94,10 @@ class CarController(CarControllerBase):
self.v_tsc = 0
self.m_tsc = 0
self.steady_speed = 0
+ self.speeds = 0
+ self.v_target_plan = 0
self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
+ self.custom_stock_planner_speed = self.param_s.get_bool("CustomStockLongPlanner")
self.lead_distance = 0
self.jerk = 0.0
@@ -126,6 +129,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
@@ -135,14 +142,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,
@@ -203,7 +217,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))
@@ -241,6 +255,8 @@ class CarController(CarControllerBase):
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
+ else:
+ can_sends.extend(hyundaicanfd.create_fca_warning_light(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, hud_control, self.jerk_u, self.jerk_l))
@@ -463,6 +479,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:
diff --git a/selfdrive/car/hyundai/fingerprints.py b/selfdrive/car/hyundai/fingerprints.py
index 05b916daf9..749fbadc65 100644
--- a/selfdrive/car/hyundai/fingerprints.py
+++ b/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',
diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py
index e32e462f1b..69ea0c6317 100644
--- a/selfdrive/car/hyundai/hyundaicanfd.py
+++ b/selfdrive/car/hyundai/hyundaicanfd.py
@@ -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:
- 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))
+ ret.extend(create_fca_warning_light(packer, CAN, frame))
if frame % 5 == 0:
values = {
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 20dace7228..1ac7e8cf03 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -91,7 +91,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,12 +102,16 @@ class CarInterface(CarInterfaceBase):
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.stoppingControl = True
- ret.startingState = True
- ret.vEgoStarting = 0.1
- ret.startAccel = 1.6
+ ret.vEgoStarting = 0.5
+ ret.startAccel = 1.0
ret.stopAccel = -1.0
ret.longitudinalActuatorDelay = 0.5
+ if ret.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
+ ret.startingState = False
+ else:
+ ret.startingState = True
+
if DBC[ret.carFingerprint]["radar"] is None:
if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD):
ret.radarUnavailable = False
@@ -118,6 +122,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", "11c1f1909ca37bca"):
+ ret.spFlags |= HyundaiFlagsSP.SP_UPSTREAM_TACO.value
else:
ret.enableBsm = 0x58b in fingerprint[0]
@@ -143,6 +149,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
@@ -168,7 +176,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
@@ -187,7 +196,7 @@ class CarInterface(CarInterfaceBase):
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')
@@ -203,6 +212,10 @@ class CarInterface(CarInterfaceBase):
enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42')
def _update(self, c):
+ if not self.CS.control_initialized:
+ 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 = [
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 45ecb4c712..99f83ccade 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/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
- self.STEER_DRIVER_ALLOWANCE = 250
+ upstream_taco = CP.safetyConfigs[-1].safetyParam & Panda.FLAG_HYUNDAI_UPSTREAM_TACO
+ self.STEER_MAX = 270 if not upstream_taco else 384 if vEgoRaw < 11. else 330
+ self.STEER_DRIVER_ALLOWANCE = 250 if not upstream_taco else 350
self.STEER_DRIVER_MULTIPLIER = 2
- self.STEER_THRESHOLD = 250
- self.STEER_DELTA_UP = 2
- self.STEER_DELTA_DOWN = 3
+ self.STEER_THRESHOLD = 250 if not upstream_taco else 350
+ self.STEER_DELTA_UP = 2 if not upstream_taco else 10 if vEgoRaw < 11. else 2
+ self.STEER_DELTA_DOWN = 3 if not upstream_taco else 10 if vEgoRaw < 11. else 3
# 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),
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 25e8b40a72..aca6398701 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -408,7 +408,7 @@ class CarInterfaceBase(ABC):
@staticmethod
def sp_configure_custom_torque_tune(ret, params):
- ret.lateralTuning.torque.friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.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
diff --git a/selfdrive/car/param_manager.py b/selfdrive/car/param_manager.py
index c7df87b9b7..6f393bd2d2 100644
--- a/selfdrive/car/param_manager.py
+++ b/selfdrive/car/param_manager.py
@@ -11,6 +11,7 @@ class ParamManager:
"experimental_mode": 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,
@@ -37,6 +38,7 @@ class ParamManager:
"experimental_mode": params.get_bool("ExperimentalMode"),
"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"),
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
index d0112748ab..8d33a2d965 100644
--- a/selfdrive/car/subaru/values.py
+++ b/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:
diff --git a/selfdrive/car/sunnypilot_carname.json b/selfdrive/car/sunnypilot_carname.json
index 9a04ea9f1e..16ff623e23 100644
--- a/selfdrive/car/sunnypilot_carname.json
+++ b/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",
diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py
index 4cf32513c8..e8daec9077 100755
--- a/selfdrive/car/tests/routes.py
+++ b/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),
diff --git a/selfdrive/car/torque_data/substitute.toml b/selfdrive/car/torque_data/substitute.toml
index 921846667b..7fb8ec7f3f 100644
--- a/selfdrive/car/torque_data/substitute.toml
+++ b/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"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 519fddaf6a..79d65ffc28 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -174,10 +174,13 @@ 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.custom_model_metadata = CustomModelMetadata(params=self.params, init_only=True)
@@ -185,6 +188,7 @@ class Controls:
self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution
self.dynamic_personality = self.params.get_bool("DynamicPersonality")
+ self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist")
self.accel_personality = self.read_accel_personality_param()
@@ -489,7 +493,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 +644,14 @@ class Controls:
self.LoC.reset()
if not self.joystick_mode:
+ speeds = long_plan.speeds
+ resume = False
+ if len(speeds):
+ resume = self.enabled_long and CS.standstill and speeds[-1] > 0.1 and self.CP.carName == "hyundai"
+
# 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:
@@ -806,11 +817,21 @@ 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)
+ lc_svs = self.sm[dh]
+ long_plan = self.sm['longitudinalPlan'].hasLead
+ dm_state = self.sm['driverMonitoringState']
+ overtaking_accel_allowed = ((lc_svs.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or
+ (lc_svs.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \
+ (lc_svs.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting))
+ overtaking_accel_engaged = self.overtaking_accel and overtaking_accel_allowed and \
+ CS.vEgo > (60 * CV.KPH_TO_MS) if self.is_metric else (40 * CV.MPH_TO_MS) and long_plan.hasLead and \
+ long_plan.aTarget > -0.2 and not (CS.leftBlinker and CS.rightBlinker)
+
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
@@ -825,7 +846,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 +884,7 @@ class Controls:
controlsStateSP.personality = self.personality
controlsStateSP.dynamicPersonality = self.dynamic_personality
controlsStateSP.accelPersonality = self.accel_personality
+ controlsStateSP.overtakingAccelerationAssist = overtaking_accel_engaged
if self.enable_nnff and lat_tuning == 'torque':
controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp
@@ -920,7 +942,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 +952,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):
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 2f191b4a55..3374347a3e 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/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
@@ -84,11 +88,16 @@ 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
+
@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 = long_plan_sp.speedLimitControlState
@@ -103,9 +112,28 @@ class VCruiseHelper:
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
- self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+ if enabled and sp_override_speed and CS.cruiseState.speed * CV.MS_TO_KPH < sp_override_speed:
+ if self.sp_override_v_cruise_kph == V_CRUISE_UNSET:
+ self.sp_override_v_cruise_kph = max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_MIN)
+ else:
+ self.sp_override_v_cruise_kph = V_CRUISE_UNSET
+
+ # when we have an override_speed, use it
+ if self.sp_override_v_cruise_kph != V_CRUISE_UNSET:
+ self.v_cruise_kph = self.sp_override_v_cruise_kph
+ self.v_cruise_cluster_kph = self.sp_override_v_cruise_kph
+ else:
+ self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+ self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+
+ #print("sp_override_v_cruise_kph:", self.sp_override_v_cruise_kph)
+ #print("v_cruise_kph:", self.v_cruise_kph)
+ #print("v_cruise_cluster_kph:", self.v_cruise_cluster_kph)
+
+ self.sp_override_cruise_speed_last = CS.cruiseState.speed
+ self.sp_override_enabled_last = enabled
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
@@ -202,6 +230,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:
@@ -234,6 +264,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
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
index d4763f6c1d..d4594a6a81 100644
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/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
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index e3cd4bb654..1a9e7c2fb7 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/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
- starting_condition = (not should_stop and
+ stopping_condition = should_stop and not resume
+ 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.
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index e520c3bcb2..9542a10078 100755
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -3,8 +3,7 @@ import os
import time
import numpy as np
from cereal import custom
-from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
# WARNING: imports outside of constants will not trigger a rebuild
@@ -67,6 +66,8 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
return 0.8
elif personality==custom.LongitudinalPersonalitySP.aggressive:
return 0.65
+ elif personality==custom.LongitudinalPersonalitySP.overtake:
+ return 0.1
else:
raise NotImplementedError("Longitudinal personality not supported")
@@ -80,6 +81,8 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
return 1.25
elif personality==custom.LongitudinalPersonalitySP.aggressive:
return 1.0
+ elif personality==custom.LongitudinalPersonalitySP.overtake:
+ return 0.5
else:
raise NotImplementedError("Longitudinal personality not supported")
@@ -101,61 +104,6 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
raise NotImplementedError("Dynamic personality not supported")
return np.interp(v_ego, x_vel, y_dist)
-# multiplier for A_CHANGE_COST = 200.
-def get_a_change_cost_multiplier(v_ego, v_lead0, v_lead1, personality=custom.LongitudinalPersonalitySP.standard):
- if personality==custom.LongitudinalPersonalitySP.relaxed:
- a_change_cost_multiplier_follow_distance = 1.0
- elif personality==custom.LongitudinalPersonalitySP.standard:
- a_change_cost_multiplier_follow_distance = 0.5
- elif personality==custom.LongitudinalPersonalitySP.moderate:
- a_change_cost_multiplier_follow_distance = 0.5
- elif personality==custom.LongitudinalPersonalitySP.aggressive:
- a_change_cost_multiplier_follow_distance = 0.1
- else:
- raise NotImplementedError("Longitudinal personality not supported")
-
- # stolen from @KRKeegan
- # values used for interpolation
- # start with a small a_change_multiplier_values during interpolation to allow for faster change in accel
- A_CHANGE_COST_MULTIPLIER_BP = [0., 10.] # vEgo, in m/s
- A_CHANGE_COST_MULTIPLIER_V = [.05, 1.] # multiplier values
-
- # when lead is pulling away, and speed is between 0 and 10 m/s, interpolate a_change_cost_multiplier_v_ego
- a_change_cost_multiplier_v_ego = 1.
- if (v_lead0 - v_ego > 1e-3) and (v_lead1 - v_ego > 1e-3):
- a_change_cost_multiplier_v_ego = interp(v_ego, A_CHANGE_COST_MULTIPLIER_BP, A_CHANGE_COST_MULTIPLIER_V)
-
- # get the minimum between a_change_multiplier based on driving personality, and a_change_multiplier based
- # on v_ego
- a_change_multiplier = min(a_change_cost_multiplier_follow_distance, a_change_cost_multiplier_v_ego)
-
- # and pass it on as the final result
- return a_change_multiplier
-
-# multiplier for DANGER_ZONE_COST = 100.
-def get_danger_zone_cost_multiplier(personality=custom.LongitudinalPersonalitySP.standard):
- if personality==custom.LongitudinalPersonalitySP.relaxed:
- return 1.6
- elif personality==custom.LongitudinalPersonalitySP.standard:
- return 1.3
- elif personality==custom.LongitudinalPersonalitySP.moderate:
- return 1.3
- elif personality==custom.LongitudinalPersonalitySP.aggressive:
- return 1.0
- else:
- raise NotImplementedError("Longitudinal personality not supported")
-
-#def get_STOP_DISTANCE(personality=custom.LongitudinalPersonalitySP.standard):
-# if personality==log.LongitudinalPersonality.relaxed:
-# return 6.0
-# elif personality==log.LongitudinalPersonality.standard:
-# return 5.5
-# elif personality==log.LongitudinalPersonality.aggressive:
-# return 4.5
-# else:
-# raise NotImplementedError("dynamic stop distance not supported")
-
-
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
@@ -354,16 +302,12 @@ class LongitudinalMpc:
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
- def set_weights(self, prev_accel_constraint=True, v_lead0 = 0., v_lead1 = 0., personality=custom.LongitudinalPersonalitySP.standard):
- v_ego = self.x0[1]
+ def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
jerk_factor = get_jerk_factor(personality)
- a_change_cost_multiplier = get_a_change_cost_multiplier(v_ego, v_lead0, v_lead1, personality)
- danger_zone_cost_multiplier = get_danger_zone_cost_multiplier(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_multiplier \
- * a_change_cost, jerk_factor * J_EGO_COST]
- constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * danger_zone_cost_multiplier]
+ cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
+ constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
elif self.mode == 'blended':
a_change_cost = 40.0 if prev_accel_constraint else 0
cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
@@ -417,16 +361,16 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
- def update(self, radarstate, v_cruise, prev_accel_constraint, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False):
+ def update(self, radarstate, v_cruise, 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.
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 04df561c14..9933dbf861 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/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, custom
+from cereal import car, log, custom
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
@@ -83,7 +83,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" 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)
@@ -157,8 +158,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':
@@ -193,11 +196,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, prev_accel_constraint, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality)
+ self.mpc.update(sm['radarState'], v_cruise, 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)
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 6c51b38b13..daef841c19 100755
--- a/selfdrive/controls/plannerd.py
+++ b/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:
diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py
index ce8a7935c8..1c972b2ba2 100755
--- a/selfdrive/locationd/torqued.py
+++ b/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)
diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc
index 80019f406e..2e7c39ea6a 100644
--- a/selfdrive/ui/qt/api.cc
+++ b/selfdrive/ui/qt/api.cc
@@ -145,4 +145,4 @@ void HttpRequest::requestFinished() {
QNetworkAccessManager *HttpRequest::nam() {
static QNetworkAccessManager *networkAccessManager = new QNetworkAccessManager(qApp);
return networkAccessManager;
-}
+}
\ No newline at end of file
diff --git a/selfdrive/ui/qt/api.h b/selfdrive/ui/qt/api.h
index f0e21f56a8..0e5d2abbad 100644
--- a/selfdrive/ui/qt/api.h
+++ b/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();
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc
index 68ab992095..c0e9efaf90 100644
--- a/selfdrive/ui/qt/home.cc
+++ b/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()) {
diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h
index e903dad47d..a0da870dbf 100644
--- a/selfdrive/ui/qt/home.h
+++ b/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);
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index 040b074784..7c821339a1 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/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
diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h
index dcc07aa2f2..975cb82e92 100644
--- a/selfdrive/ui/qt/offroad/settings.h
+++ b/selfdrive/ui/qt/offroad/settings.h
@@ -91,6 +91,7 @@ protected:
virtual void updateToggles();
};
+
class SoftwarePanel : public ListWidget {
Q_OBJECT
public:
diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc
index 47b4a22682..a3f2a8549a 100644
--- a/selfdrive/ui/qt/offroad/software_settings.cc
+++ b/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();
-}
+}
\ No newline at end of file
diff --git a/selfdrive/ui/qt/offroad_home.cc b/selfdrive/ui/qt/offroad_home.cc
index e9d54a4efb..212037a395 100644
--- a/selfdrive/ui/qt/offroad_home.cc
+++ b/selfdrive/ui/qt/offroad_home.cc
@@ -2,6 +2,7 @@
#include
#include
+#include
#include "selfdrive/ui/qt/offroad/experimental_mode.h"
#include "selfdrive/ui/qt/util.h"
diff --git a/selfdrive/ui/qt/offroad_home.h b/selfdrive/ui/qt/offroad_home.h
index 5796c60f52..58366bb0da 100644
--- a/selfdrive/ui/qt/offroad_home.h
+++ b/selfdrive/ui/qt/offroad_home.h
@@ -1,12 +1,19 @@
#pragma once
+#include
+#include
+#include
+#include
#include
+#include
#include
#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"
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
index 484075b3e2..216b897f31 100644
--- a/selfdrive/ui/qt/onroad/annotated_camera.cc
+++ b/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();
-}
+}
\ No newline at end of file
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h
index 465ba23e04..c92c53af46 100644
--- a/selfdrive/ui/qt/onroad/annotated_camera.h
+++ b/selfdrive/ui/qt/onroad/annotated_camera.h
@@ -50,4 +50,4 @@ protected:
double prev_draw_t = 0;
FirstOrderFilter fps_filter;
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc
index 92bcea11b5..862c478e81 100644
--- a/selfdrive/ui/qt/onroad/buttons.cc
+++ b/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);
}
diff --git a/selfdrive/ui/qt/onroad/buttons.h b/selfdrive/ui/qt/onroad/buttons.h
index e999480d5c..6d30c1ac91 100644
--- a/selfdrive/ui/qt/onroad/buttons.h
+++ b/selfdrive/ui/qt/onroad/buttons.h
@@ -31,4 +31,4 @@ private:
QPixmap experimental_img;
};
-void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity);
+void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity);
\ No newline at end of file
diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc
index e93ded6cc2..1b3f9ac8ab 100644
--- a/selfdrive/ui/qt/onroad/onroad_home.cc
+++ b/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));
-}
+}
\ No newline at end of file
diff --git a/selfdrive/ui/qt/onroad/onroad_home.h b/selfdrive/ui/qt/onroad/onroad_home.h
index 4ac37678fa..aee33b9001 100644
--- a/selfdrive/ui/qt/onroad/onroad_home.h
+++ b/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);
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/qt/request_repeater.h b/selfdrive/ui/qt/request_repeater.h
index 32e714b1cb..1b3e96574d 100644
--- a/selfdrive/ui/qt/request_repeater.h
+++ b/selfdrive/ui/qt/request_repeater.h
@@ -20,4 +20,4 @@ private:
Params params;
QTimer *timer;
QString prevResp;
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h
index 62a5486809..eecc439abf 100644
--- a/selfdrive/ui/qt/sidebar.h
+++ b/selfdrive/ui/qt/sidebar.h
@@ -65,4 +65,4 @@ protected:
private:
std::unique_ptr pm;
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/qt/text.cc b/selfdrive/ui/qt/text.cc
index 21ec5eedcf..7b14368589 100644
--- a/selfdrive/ui/qt/text.cc
+++ b/selfdrive/ui/qt/text.cc
@@ -61,4 +61,4 @@ int main(int argc, char *argv[]) {
)");
return a.exec();
-}
+}
\ No newline at end of file
diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h
index 20443db468..94b83f0ab3 100644
--- a/selfdrive/ui/qt/util.h
+++ b/selfdrive/ui/qt/util.h
@@ -59,4 +59,4 @@ private:
QFileSystemWatcher *watcher;
QHash params_hash;
Params params;
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/qt/widgets/ssh_keys.h b/selfdrive/ui/qt/widgets/ssh_keys.h
index 2834164702..80e0f9a729 100644
--- a/selfdrive/ui/qt/widgets/ssh_keys.h
+++ b/selfdrive/ui/qt/widgets/ssh_keys.h
@@ -35,4 +35,4 @@ private:
void refresh();
void getUserKeys(const QString &username);
-};
+};
\ No newline at end of file
diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript
index 945c9ccdb2..f32dec7d98 100644
--- a/selfdrive/ui/sunnypilot/SConscript
+++ b/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",
]
diff --git a/selfdrive/ui/sunnypilot/qt/api.cc b/selfdrive/ui/sunnypilot/qt/api.cc
index fcf925fd1a..125f23a755 100644
--- a/selfdrive/ui/sunnypilot/qt/api.cc
+++ b/selfdrive/ui/sunnypilot/qt/api.cc
@@ -33,6 +33,7 @@ Last updated: July 29, 2024
#include
#include
+#include
#include
#include
diff --git a/selfdrive/ui/sunnypilot/qt/api.h b/selfdrive/ui/sunnypilot/qt/api.h
index b8e835dc91..d437859513 100644
--- a/selfdrive/ui/sunnypilot/qt/api.h
+++ b/selfdrive/ui/sunnypilot/qt/api.h
@@ -26,6 +26,11 @@ Last updated: July 29, 2024
#pragma once
+#include
+#include
+#include
+#include
+
#include "selfdrive/ui/qt/api.h"
#include "selfdrive/ui/sunnypilot/qt/util.h"
#include "common/util.h"
diff --git a/selfdrive/ui/sunnypilot/qt/home.cc b/selfdrive/ui/sunnypilot/qt/home.cc
index 61abbff7dc..fea0913fe8 100644
--- a/selfdrive/ui/sunnypilot/qt/home.cc
+++ b/selfdrive/ui/sunnypilot/qt/home.cc
@@ -28,10 +28,13 @@ Last updated: July 29, 2024
#include
#include
+#include
+#include
#include
#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){
diff --git a/selfdrive/ui/sunnypilot/qt/home.h b/selfdrive/ui/sunnypilot/qt/home.h
index 8bb44d81b4..eb1b94407d 100644
--- a/selfdrive/ui/sunnypilot/qt/home.h
+++ b/selfdrive/ui/sunnypilot/qt/home.h
@@ -27,9 +27,14 @@ Last updated: July 29, 2024
#pragma once
#include
+#include
+#include
+#include
+#include
#include
#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"
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc
index 99964c4681..4b13436abe 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc
@@ -30,6 +30,7 @@ Last updated: July 29, 2024
#include
#include
+#include "common/watchdog.h"
#include "selfdrive/ui/qt/qt_window.h"
#include "selfdrive/ui/qt/widgets/prime.h"
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/monitoring_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/monitoring_settings.h
index 091b2fd9f5..bbea81c0b9 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/monitoring_settings.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/monitoring_settings.h
@@ -28,9 +28,11 @@ Last updated: July 29, 2024
#include