sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed 2026-07-21 07:44:02 +00:00
commit de77c89cf6
3995 files changed
+1260688

No files matched your search

Whitespace-only changes.
@@ -0,0 +1,86 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from enum import StrEnum
from opendbc.car import Bus, structs
from opendbc.can.parser import CANParser
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
class CarStateExt:
def __init__(self, CP, CP_SP):
self.CP = CP
self.CP_SP = CP_SP
self.aBasis = 0.0
def update_speed_limit(self, cp, cp_cam) -> float:
speed_limit = 0
if self.CP.flags & HyundaiFlags.CANFD:
if self.CP_SP.flags & HyundaiFlagsSP.SPEED_LIMIT_AVAILABLE:
bus = cp if self.CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG else cp_cam
speed_limit = bus.vl["FR_CMR_02_100ms"]["ISLW_SpdCluMainDis"]
else:
nav, cam = 0, 0
if self.CP_SP.flags & HyundaiFlagsSP.SPEED_LIMIT_AVAILABLE:
nav = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
if self.CP_SP.flags & HyundaiFlagsSP.HAS_LKAS12:
cam = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"]
speed_limit = cam if cam not in (0, 255) else nav
if speed_limit in (0, 255):
speed_limit = 0
return speed_limit
def update(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser], speed_conv: float) -> None:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
self.aBasis = cp.vl["TCS13"]["aBasis"]
if self.CP_SP.flags & HyundaiFlagsSP.NON_SCC:
cruise_msg = "LABEL11" if self.CP.flags & HyundaiFlags.EV else \
"E_CRUISE_CONTROL" if self.CP.flags & HyundaiFlags.HYBRID else \
"EMS16"
cruise_available_sig = "CC_React" if self.CP.flags & HyundaiFlags.EV else "CRUISE_LAMP_M"
cruise_enabled_sig = "CC_ACT" if self.CP.flags & HyundaiFlags.EV else "CRUISE_LAMP_S"
cruise_speed_msg = "E_EMS11" if self.CP.flags & HyundaiFlags.EV else \
"ELECT_GEAR" if self.CP.flags & HyundaiFlags.HYBRID else \
"LVR12"
cruise_speed_sig = "Cruise_Limit_Target" if self.CP.flags & HyundaiFlags.EV else \
"SLC_SET_SPEED" if self.CP.flags & HyundaiFlags.HYBRID else \
"CF_Lvr_CruiseSet"
ret.cruiseState.available = cp.vl[cruise_msg][cruise_available_sig] != 0
ret.cruiseState.enabled = cp.vl[cruise_msg][cruise_enabled_sig] != 0
ret.cruiseState.speed = cp.vl[cruise_speed_msg][cruise_speed_sig] * speed_conv
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
if not self.CP_SP.flags & HyundaiFlagsSP.NON_SCC_NO_FCA:
cp_cruise = cp if self.CP_SP.flags & HyundaiFlagsSP.NON_SCC_RADAR_FCA else cp_cam
aeb_src = "FCA11"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src]["FCA_CmdAct"] != 0
ret.stockFcw = aeb_warning and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
ret_sp.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_conv
def update_canfd_ext(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser],
speed_factor: float) -> None:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
self.aBasis = cp.vl["TCS"]["aBasis"]
ret_sp.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_factor
@@ -0,0 +1,74 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from opendbc.car import uds
from opendbc.car.carlog import carlog
from opendbc.car.isotp_parallel_query import IsoTpParallelQuery
DEVELOPER_DIAGNOSTIC = 0x07
CUSTOM_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, DEVELOPER_DIAGNOSTIC])
CUSTOM_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, DEVELOPER_DIAGNOSTIC])
READ_DATA_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER])
READ_DATA_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
WRITE_DATA_REQUEST = bytes([uds.SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER])
WRITE_DATA_RESPONSE = bytes([uds.SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER + 0x40])
CONFIG_DATA_ID = bytes([0x01, 0x42])
DEFAULT_CONFIG = bytes([0x00, 0x00, 0x00, 0x01, 0x00, 0x00])
TRACKS_ENABLED_CONFIG = bytes([0x00, 0x00, 0x00, 0x01, 0x00, 0x01])
TRACKS_ENABLED_CONFIG_BYTES = b"\x00\x00\x01\x00\x01"
def enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, timeout=0.1, retry=2):
carlog.error("radar_tracks: enabling ...")
for i in range(retry):
try:
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [CUSTOM_DIAGNOSTIC_REQUEST], [CUSTOM_DIAGNOSTIC_RESPONSE])
for _, _ in query.get_data(timeout).items():
carlog.error("radar_tracks: check current config ...")
request = READ_DATA_REQUEST + CONFIG_DATA_ID
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [request], [READ_DATA_RESPONSE])
for _, data in query.get_data(timeout).items():
current_config = data[3:]
carlog.error(f"radar_tracks: current config: {current_config.hex()}")
if current_config == TRACKS_ENABLED_CONFIG_BYTES:
carlog.error("radar_tracks: already enabled, skipping ...")
else:
carlog.error("radar_tracks: reconfigure radar to output radar points ...")
request = WRITE_DATA_REQUEST + CONFIG_DATA_ID + TRACKS_ENABLED_CONFIG
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [request], [WRITE_DATA_RESPONSE])
query.get_data(0)
carlog.error("radar_tracks: successfully enabled")
return True
except Exception as e:
carlog.exception(f"radar_tracks exception: {e}")
carlog.error(f"radar_tracks retry ({i + 1}) ...")
carlog.error("radar_tracks: failed")
return False
if __name__ == "__main__":
import time
import openpilot.cereal.messaging as messaging
sendcan = messaging.pub_sock('sendcan')
logcan = messaging.sub_sock('can')
time.sleep(1)
enabled = enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, timeout=0.1)
print(f"enabled: {enabled}")
@@ -0,0 +1,68 @@
from opendbc.car import structs
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
ESCC_MSG = 0x2AB
class EnhancedSmartCruiseControl:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
self.CP = CP
self.CP_SP = CP_SP
@property
def enabled(self):
return self.CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC
@property
def trigger_msg(self):
return ESCC_MSG
def update_car_state(self, car_state):
"""
This method is invoked by the CarController to update the car state on the ESCC object.
The updated state is then used to update SCC12 with the current car state values received through ESCC.
:param car_state:
:return:
"""
self.car_state = car_state
def update_scc12(self, values):
"""
Update SCC12 with the current car state values received through ESCC.
These values are sourced directly from the car's SCC radar and provide a more reliable source for AEB and FCA alerts.
:param values: SCC12 to be sent in dictionary form before being packed
:return: Nothing. SCC12 is updated in place.
"""
values["AEB_CmdAct"] = self.car_state.escc_cmd_act
values["CF_VSM_Warn"] = self.car_state.escc_aeb_warning
values["CF_VSM_DecCmdAct"] = self.car_state.escc_aeb_dec_cmd_act
values["CR_VSM_DecCmd"] = self.car_state.escc_aeb_dec_cmd
# TODO-SP: we should read it from the car's settings and use that value.
# It may not be ideal to set this here directly.
# Observed flickering on the dashboard settings switching between "deactivated" and "active assistance" when sending AEB_Status = 1.
# These values could differ from the user's configuration from the car's settings.
# This indicates that SCC12 likely displays it on the dashboard, and another FCA message may also cause it to appear.
values["AEB_Status"] = 2 # AEB enabled
class EsccCarStateBase:
def __init__(self):
self.escc_aeb_warning = 0
self.escc_aeb_dec_cmd_act = 0
self.escc_cmd_act = 0
self.escc_aeb_dec_cmd = 0
class EsccCarController:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
self.ESCC = EnhancedSmartCruiseControl(CP, CP_SP)
def update(self, car_state):
self.ESCC.update_car_state(car_state)
class EsccRadarInterfaceBase:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
self.ESCC = EnhancedSmartCruiseControl(CP, CP_SP)
self.use_escc = False
@@ -0,0 +1,126 @@
from opendbc.car.structs import CarParams
from opendbc.car.hyundai.values import CAR
Ecu = CarParams.Ecu
FW_VERSIONS_EXT = {
CAR.KIA_CEED_PHEV_2022_NON_SCC: {
(Ecu.eps, 0x7D4, None): [
b'\xf1\x00CD MDPS C 1.00 1.01 56310-XX000 4CPHC101',
],
(Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\x00CDH LKAS AT EUR LHD 1.00 1.01 99211-CR700 931',
],
},
# TODO-SP: HYUNDAI_KONA_EV_NON_SCC has the same FW versions as HYUNDAI_KONA_EV, in the future we may
# allow similar FW versions across different platforms
# 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.GENESIS_G70_2021_NON_SCC: {
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9200 4I2CL108',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00IK__ SCC --CUP 1.00 1.02 96400-G9100 ',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00IK MFC MT USA LHD 1.00 1.01 95740-G9000 170920',
],
},
CAR.HYUNDAI_KONA_NON_SCC: {
# (Ecu.abs, 0x7d1, None): [
# b'\xf1\x816V5RAJ00040.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
# ],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105',
b'\xf1\x00OS MDPS C 1.00 1.04 56310J9030\x00 4OSDC104',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9200 g30',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x006T6J0_C2\x00\x006T6K1051\x00\x00TOS4N20NS2\x00\x00\x00\x00',
],
},
CAR.KIA_FORTE_2019_NON_SCC: {
(Ecu.eps, 0x7D4, None): [
b'\xf1\x00BD MDPS C 1.00 1.04 56310/M6000 4BDDC104',
b'\xf1\x00BD MDPS C 1.00 1.05 56310/M6000 4BDDC105',
],
(Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\x00BD LKAS AT USA LHD 1.00 1.02 95740-M6000 J31',
],
# (Ecu.abs, 0x7d1, None): [
# b'\xf1\x816VFRAF00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
# ],
# (Ecu.transmission, 0x7e1, None): [
# b'\xf1\x87CXJQAM4966515JB0x\xa9\x98\x9b\x99fff\x98feg\x88\x88w\x88Ff\x8f\xff{\xff\xff\xff\xa8\xf6\xf1\x816V2C1051\x00\x00\xf1\x006V2B0_C2\x00\x006V2C1051\x00\x00CBD0N20NS8q\xc1&\xd2', # noqa: E501
# ],
},
CAR.KIA_FORTE_2021_NON_SCC: {
(Ecu.eps, 0x7D4, None): [
b'\xf1\x00BD MDPS C 1.00 1.08 56310M6000\x00 4BDDC108',
],
(Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\x00BD LKAS AT USA LHD 1.00 1.04 95740-M6000 J33',
],
# (Ecu.abs, 0x7d1, None): [
# b'\xf1\x816VFRAL00010.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
# ],
# (Ecu.transmission, 0x7e1, None): [
# b'\xf1\x87CXLQAM0906975JB0\x89\x88\xa6\x8aVfug\xba\x87\x94yffuxgfo\xff\x8b\xff\xff\xff\x91\x82\xf1\x816V2C1051\x00\x00\xf1\x006V2B0_C2\x00\x006V2C1051\x00\x00CBD0N20NS8q\xc1&\xd2', # noqa: E501
# ],
},
CAR.KIA_SELTOS_2023_NON_SCC: {
(Ecu.abs, 0x7d1, None): [
b'\xf1\x00SP ESC \t 101"\t\x01 58910-Q5510',
b'\xf1\x00SP ESC \r 100"\x04\x01 58910-Q5510',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00SP2 MDPS C 1.00 1.04 56310Q5240 4SPSC104',
b'\xf1\x00SP2 MDPS C 1.00 1.01 56300Q5920 ',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00SP2 MFC AT USA LHD 1.00 1.03 99210-Q5500 230208',
b'\xf1\x00SP2 MFC AT AUS RHD 1.00 1.02 99210-Q5500 220624',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x006V2B0_C2\x00\x006V2D5051\x00\x00CSP2N20NL0\x00\x00\x00\x00',
b'\xf1\x006V2B0_C2\x00\x006V2D4051\x00\x00CSP2N20KL1\x00\x00\x00\x00',
],
},
CAR.HYUNDAI_ELANTRA_2022_NON_SCC: {
(Ecu.eps, 0x7d4, None): [
# b'\xf1\x8756310AA030\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA030\x00 4CNDC106',
b'\xf1\x00CN7 MDPS R 1.00 1.04 57700-IB000 4CNNP104',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205',
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-IB000 210531',
],
(Ecu.abs, 0x7d1, None): [
# b'\xf1\x8758910-AB500\xf1\x00CN ESC \t 100 \x06\x01 58910-AB500',
b'\xf1\x00CN ESC \t 100!\x05\x01 58910-IB000',
],
(Ecu.transmission, 0x7e1, None): [
# b'\xf1\x87CXNQEM4091445JB3g\x98\x98\x89\x99\x87gv\x89wuwgwv\x89hD_\xffx\xff\xff\xff\x86\xeb\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', # noqa: E501
b'\xf1\x00T02601BL T02900A1 WCN7T20XXX900NS4\xf7\xccz\xf6',
],
},
CAR.HYUNDAI_BAYON_1ST_GEN_NON_SCC: {
# TODO: Check working route for more FW
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00BC3 LKA AT EUR LHD 1.00 1.01 99211-Q0100 261'
],
},
}
@@ -0,0 +1,78 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import numpy as np
from opendbc.car import DT_CTRL, structs
from opendbc.car.can_definitions import CanData
from opendbc.car.hyundai import hyundaican, hyundaicanfd
from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CANFD_CAR
from opendbc.sunnypilot.car.intelligent_cruise_button_management_interface_base import IntelligentCruiseButtonManagementInterfaceBase
ButtonType = structs.CarState.ButtonEvent.Type
SendButtonState = structs.IntelligentCruiseButtonManagement.SendButtonState
BUTTON_COPIES = 2
BUTTON_COPIES_TIME = 7
BUTTON_COPIES_TIME_IMPERIAL = [BUTTON_COPIES_TIME + 3, 70]
BUTTON_COPIES_TIME_METRIC = [BUTTON_COPIES_TIME, 40]
BUTTONS = {
SendButtonState.increase: Buttons.RES_ACCEL,
SendButtonState.decrease: Buttons.SET_DECEL,
}
class IntelligentCruiseButtonManagementInterface(IntelligentCruiseButtonManagementInterfaceBase):
def __init__(self, CP, CP_SP):
super().__init__(CP, CP_SP)
def create_can_mock_button_messages(self, packer, CS, send_button) -> list[CanData]:
can_sends = []
copies_xp = BUTTON_COPIES_TIME_METRIC if CS.is_metric else BUTTON_COPIES_TIME_IMPERIAL
copies = int(np.interp(BUTTON_COPIES_TIME, copies_xp, [1, BUTTON_COPIES]))
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([hyundaican.create_clu11(packer, self.frame, CS.clu11, send_button, self.CP)] * copies)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
return can_sends
def create_canfd_mock_button_messages(self, packer, CS, CAN, send_button) -> list[CanData]:
can_sends = []
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# TODO: resume for alt button cars
pass
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.2:
self.button_frame += 1
button_counter_offset = [1, 1, 0, None][self.button_frame % 4]
if button_counter_offset is not None:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(packer, self.CP, CAN, (CS.buttons_counter + button_counter_offset) % 0xF, send_button))
self.last_button_frame = self.frame
return can_sends
def update(self, CS, CC_SP, packer, frame, last_button_frame, CAN) -> list[CanData]:
can_sends = []
self.CC_SP = CC_SP
self.ICBM = CC_SP.intelligentCruiseButtonManagement
self.frame = frame
self.last_button_frame = last_button_frame
if self.ICBM.sendButton != SendButtonState.none:
send_button = BUTTONS[self.ICBM.sendButton]
if self.CP.carFingerprint in CANFD_CAR:
can_sends.extend(self.create_canfd_mock_button_messages(packer, CS, CAN, send_button))
else:
can_sends.extend(self.create_can_mock_button_messages(packer, CS, send_button))
return can_sends
@@ -0,0 +1,117 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from abc import ABC, abstractmethod
from opendbc.car import structs
from opendbc.car.hyundai.values import HyundaiFlags
class LeadData(ABC):
def __init__(self, object_gap: int, lead_distance: float, lead_rel_speed: float, lead_visible: bool):
self.object_gap = object_gap
self.lead_distance = lead_distance
self.lead_rel_speed = lead_rel_speed
self.lead_visible = lead_visible
@property
@abstractmethod
def object_rel_gap(self) -> int:
raise NotImplementedError("Subclasses must implement this method")
class CanLeadData(LeadData):
@property
def object_rel_gap(self) -> int:
return 0 if self.lead_distance == 0 else 2 if self.lead_rel_speed < -0.2 else 1
class CanFdLeadData(LeadData):
@property
def object_rel_gap(self) -> int:
return 0 if not self.lead_visible else 2 if self.lead_rel_speed < 0 else 1
def _hysteresis_update(current, new_value, counter, threshold):
"""
Updates a value based on a hysteresis threshold mechanism. This function
compares a new value against the current value and uses a counter to detect
when a transition should occur, avoiding rapid oscillations between states.
A new value will only be adopted if it differs from the current value and
the counter reaches the specified threshold.
:param current: The current value being tracked.
:param new_value: The potential new value to compare against the current.
:param counter: The count of consecutive different values encountered.
:param threshold: The minimum count required before switching to the new value.
:return: A tuple containing:
- The updated current value, which is either the original current
value or the new value if the hysteresis condition was met.
- The updated counter, reset to 0 if the new value was adopted,
or incremented by 1 otherwise.
"""
if new_value == current:
return current, 0
counter += 1
return (new_value, 0) if counter >= threshold else (current, counter)
class LeadDataCarController:
# Hysteresis parameters
LEAD_HYSTERESIS_FRAMES: int = 50
def __init__(self, CP: structs.CarParams):
self.CP = CP
self.lead_one = {}
self.lead_two = {}
self._lead_on_counter = 0
self._lead_off_counter = 0
self.lead_visible = False
self.gap_counter = 0
self.object_gap = 0
self.lead_distance = 0
self.lead_rel_speed = 0
def _update_object_gap(self, lead_distance: float | None):
new_gap = 5 # Default gap value if no lead distance is provided
if lead_distance is None or lead_distance == 0:
new_gap = 0
elif lead_distance < 20:
new_gap = 2
elif lead_distance < 25:
new_gap = 3
elif lead_distance < 30:
new_gap = 4
self.object_gap, self.gap_counter = _hysteresis_update(self.object_gap, new_gap, self.gap_counter, self.LEAD_HYSTERESIS_FRAMES)
def _update_lead_visible_hysteresis(self, raw_lead_visible: bool):
counter = self._lead_on_counter if raw_lead_visible else self._lead_off_counter
self.lead_visible, counter = _hysteresis_update(self.lead_visible, raw_lead_visible, counter, self.LEAD_HYSTERESIS_FRAMES)
if raw_lead_visible:
self._lead_on_counter = counter
self._lead_off_counter = 0 # reset opposite counter
else:
self._lead_off_counter = counter
self._lead_on_counter = 0 # reset opposite counter
def update(self, CC_SP: structs.CarControlSP) -> None:
self.lead_one = CC_SP.leadOne
self.lead_two = CC_SP.leadTwo
self.lead_distance = self.lead_one.dRel
self.lead_rel_speed = self.lead_one.vRel
self._update_lead_visible_hysteresis(self.lead_one.status)
self._update_object_gap(self.lead_distance)
@property
def lead_data(self) -> CanLeadData | CanFdLeadData:
if self.CP.flags & HyundaiFlags.CANFD:
return CanFdLeadData(self.object_gap, self.lead_distance, self.lead_rel_speed, self.lead_visible)
return CanLeadData(self.object_gap, self.lead_distance, self.lead_rel_speed, self.lead_visible)
@@ -0,0 +1,54 @@
# **How custom longitudinal tuning works for Hyundai/Kia/Genesis vehicles in sunnypilot.**
This document explains the custom longitudinal control tuning implemented in sunnypilot for Hyundai, Kia, and Genesis vehicles.
**Safety First: ISO 15622:2018**
Our primary safety guideline is [ISO 15622:2018](https://www.iso.org/obp/ui/en/#iso:std:iso:15622:ed-3:v1:en).
This standard provides the foundation for the safety limits enforced by this tuning, particularly concerning maximum allowable jerk based on speed.
**Core Concepts**
1. **Jerk-Limited Acceleration:** Instead of directly applying the planner's target acceleration (`CC.actuators.accel`), we use a jerk-limited integrator. This ensures smoother transitions by limiting the rate of change of acceleration (jerk).
* `desired_accel`: The target acceleration from the planner, clipped within safe limits (`CarControllerParams.ACCEL_MIN`, `CarControllerParams.ACCEL_MAX`). During stopping, this is forced to 0.
* `actual_accel`: The acceleration command sent to the car. It's calculated by integrating `desired_accel` but limiting the change from the previous step (`state.accel_last`) based on calculated `jerk_upper` and `jerk_lower` limits.
* `jerk_limited_integrator(desired_accel, last_accel, jerk_upper, jerk_lower)`: This function calculates the maximum allowable acceleration change per control step (`DT_CTRL * 2`) based on the upper and lower jerk limits and applies it using `rate_limit`.
2. **Predictive Jerk Limits:** The upper and lower jerk limits (`jerk_upper`, `jerk_lower`) are not fixed but calculated dynamically based on several factors:
* **Actual Acceleration (`aego`):** The car's current acceleration (`CS.out.aEgo`, potentially blended with `CS.aBasis` at low speeds) is smoothed using a `FirstOrderFilter` (`self.aego`).
* **Acceleration Error:** The difference between the *commanded* acceleration (`self.accel_cmd`) and the *actual* smoothed acceleration (`self.aego.x`).
* **Lookahead Jerk:** We estimate the jerk required to reach the target acceleration (`accel_cmd`) within a future time window. This window (`future_t_upper`, `future_t_lower`) varies with speed, defined in `config.py` (`lookahead_jerk_bp`, `lookahead_jerk_upper_v`, `lookahead_jerk_lower_v`).
* `j_ego_upper = accel_error / future_t_upper`
* `j_ego_lower = accel_error / future_t_lower`
* **ISO 15622 Speed Factors:** Maximum allowed jerk decreases with speed according to ISO 15622. We interpolate values based on current velocity (`CS.out.vEgo`) to get `upper_speed_factor` and `lower_speed_factor`.
* **Minimum Jerk (`MIN_JERK`):** A baseline minimum jerk (currently 0.5 m/s³) is enforced to ensure responsiveness.
* **Combining Factors:**
* `desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)`
* `desired_jerk_lower = min(max(-j_ego_lower, MIN_JERK), lower_speed_factor)` (Note: `j_ego_lower` is negated for the lower limit calculation).
3. **Dynamic Braking Tune:** For use of the tune without the `LONG_TUNING_PREDICTIVE` flag, a specific negative jerk profile is applied during braking (`gen1_accel_error < 0`).
* `gen1_accel_error = a_ego_blended - self.state.accel_last`
* A dynamic lower jerk limit (`gen1_lower_jerk`) is interpolated based on this error using `DYNAMIC_LOWER_JERK_BP` and a dynamically scaled `DYNAMIC_LOWER_JERK_V` (scaled by `car_config.jerk_limits`).
* `gen1_desired_jerk_lower = min(gen1_lower_jerk, lower_speed_factor)`
4. **Applying Jerk Limits:**
* The `jerk_upper` limit is always smoothed using `ramp_update` to prevent abrupt changes.
* The `jerk_lower` limit is applied differently based on the `LONG_TUNING_PREDICTIVE` flag:
* **If Flag Set (predictive tune):** `jerk_lower` is set directly to `desired_jerk_lower`.
* **If Flag Not Set (dynamic tune):** `jerk_lower` is smoothed using `ramp_update` towards `gen1_desired_jerk_lower`.
**Configuration (`config.py`)**
* The `CarTuningConfig` dataclass holds tuning parameters like stopping/starting speeds, lookahead jerk profiles, actuator delay, and base jerk limits.
* `TUNING_CONFIGS` provides default configurations based on car type (CANFD, EV, HYBRID, DEFAULT).
* `CAR_SPECIFIC_CONFIGS` allows overriding defaults for specific car models (e.g., `KIA_NIRO_EV`, `HYUNDAI_IONIQ`).
* The `get_car_config` helper function selects the appropriate configuration based on the car fingerprint and flags.
* `get_longitudinal_tune` applies relevant config values (like stopping/starting speeds, delay) to the main `CarParams` (CP).
**Summary**
This tuning aims for smoother and more responsive longitudinal control by:
* Using a jerk-limited integrator for acceleration commands.
* Dynamically calculating jerk limits based on lookahead predictions, actual acceleration, and ISO 15622 speed constraints.
* Providing specific tuning profiles via a configuration system based on car type and model.
* Applying special braking logic for use of predictive or dynamic tune.
Whitespace-only changes.
@@ -0,0 +1,55 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from dataclasses import dataclass, field
from opendbc.car.hyundai.values import CAR
@dataclass
class CarTuningConfig:
v_ego_stopping: float = 0.25
v_ego_starting: float = 0.10
stopping_decel_rate: float = 0.40
lookahead_jerk_bp: list[float] = field(default_factory=lambda: [2., 5., 20.])
lookahead_jerk_upper_v: list[float] = field(default_factory=lambda: [0.3, 0.45, 0.6])
lookahead_jerk_lower_v: list[float] = field(default_factory=lambda: [0.3, 0.45, 0.6])
longitudinal_actuator_delay: float = 0.50
jerk_limits: float = 4.0
# Default configurations for different car types
TUNING_CONFIGS = {
"CANFD": CarTuningConfig(
v_ego_stopping=0.30,
),
"EV": CarTuningConfig(
stopping_decel_rate=0.45,
v_ego_stopping=0.35,
),
"HYBRID": CarTuningConfig(
v_ego_starting=0.15,
stopping_decel_rate=0.45,
v_ego_stopping=0.4,
),
"DEFAULT": CarTuningConfig(
v_ego_stopping=0.3,
)
}
# Car-specific configs
CAR_SPECIFIC_CONFIGS = {
CAR.KIA_NIRO_EV: CarTuningConfig(
v_ego_stopping=0.1,
stopping_decel_rate=0.3,
jerk_limits=3.3,
),
CAR.KIA_NIRO_PHEV_2022: CarTuningConfig(
stopping_decel_rate=0.8,
jerk_limits=5.0,
),
}
@@ -0,0 +1,266 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import numpy as np
from dataclasses import dataclass
from opendbc.car import structs, DT_CTRL
from opendbc.car.interfaces import CarStateBase
from opendbc.car.hyundai.values import CarControllerParams
from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import get_car_config, jerk_limited_integrator
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
LongCtrlState = structs.CarControl.Actuators.LongControlState
MIN_JERK = 0.5
@dataclass
class LongitudinalState:
desired_accel: float = 0.0
actual_accel: float = 0.0
accel_last: float = 0.0
jerk_upper: float = 0.0
jerk_lower: float = 0.0
comfort_band_upper: float = 0.0
comfort_band_lower: float = 0.0
stopping: bool = False
class LongitudinalController:
"""Longitudinal controller which gets injected into CarControllerParams."""
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
self.CP = CP
self.CP_SP = CP_SP
self.tuning = LongitudinalState()
self.car_config = get_car_config(CP)
self.long_control_state_last = LongCtrlState.off
self.stopping_count = 0
self.accel_cmd = 0.0
self.desired_accel = 0.0
self.actual_accel = 0.0
self.accel_last = 0.0
self.jerk_upper = 0.0
self.jerk_lower = 0.0
self.comfort_band_upper = 0.0
self.comfort_band_lower = 0.0
self.stopping = False
@property
def enabled(self) -> bool:
return bool(self.CP_SP.flags & (HyundaiFlagsSP.LONG_TUNING_DYNAMIC | HyundaiFlagsSP.LONG_TUNING_PREDICTIVE))
def get_stopping_state(self, actuators: structs.CarControl.Actuators) -> None:
stopping = actuators.longControlState == LongCtrlState.stopping
# If custom tuning is not enabled, use upstream stopping logic
if not self.enabled:
self.stopping = stopping
self.stopping_count = 0
return
# Reset stopping state when not in stopping mode
if not stopping:
self.stopping = False
self.stopping_count = 0
return
# When transitioning from off state to stopping
if self.long_control_state_last == LongCtrlState.off:
self.stopping = True
return
# Keep track of time in stopping state (in control cycles)
if self.stopping_count > 1 / (DT_CTRL * 5):
self.stopping = True
self.stopping_count += 1
def _calculate_speed_based_jerk_limits(self, velocity: float, long_control_state: LongCtrlState) -> tuple[float, float]:
"""Calculate jerk limits based on vehicle speed according to ISO 15622:2018.
Args:
velocity: Current vehicle speed (m/s)
long_control_state: Current longitudinal control state
Returns:
Tuple of (upper_limit, lower_limit) in m/s³
"""
if long_control_state == LongCtrlState.pid:
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0]))
else:
upper_limit = 0.5 # Default for non-PID states
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
else:
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [3.5, 3.5, 3.0]))
return upper_limit, lower_limit
def _calculate_lookahead_jerk(self, accel_error: float, velocity: float) -> tuple[float, float]:
"""Calculate lookahead jerk needed to reach target acceleration.
Args:
accel_error: Difference between target and current acceleration (m/s²)
velocity: Current vehicle speed (m/s)
Returns:
Tuple of (upper_jerk, lower_jerk) in m/s³
"""
# Time window to reach target acceleration, varies with speed
future_t_upper = float(np.interp(velocity, self.car_config.lookahead_jerk_bp, self.car_config.lookahead_jerk_upper_v))
future_t_lower = float(np.interp(velocity, self.car_config.lookahead_jerk_bp, self.car_config.lookahead_jerk_lower_v))
# Required jerk to reach target acceleration in lookahead window
j_ego_upper = accel_error / future_t_upper
j_ego_lower = accel_error / future_t_lower
jerk_limit = self.car_config.jerk_limits
j_ego_upper = np.clip(j_ego_upper, -jerk_limit, jerk_limit)
j_ego_lower = np.clip(j_ego_lower, -jerk_limit, jerk_limit)
return float(j_ego_upper), float(j_ego_lower)
def calculate_jerk(self, CC: structs.CarControl, CS: CarStateBase, long_control_state: LongCtrlState) -> None:
"""Calculate appropriate jerk limits for smooth acceleration/deceleration.
Args:
CC: Car control signals
CS: Car state
long_control_state: Current longitudinal control state
"""
# If custom tuning is disabled, use upstream fixed values
if not self.enabled:
jerk_limit = 3.0 if long_control_state == LongCtrlState.pid else 1.0
self.jerk_upper = jerk_limit
self.jerk_lower = 5.0
return
velocity = CS.out.vEgo
accel_error = self.accel_cmd - self.accel_last
# Calculate jerk limits based on speed
upper_speed_factor, lower_speed_factor = self._calculate_speed_based_jerk_limits(velocity, long_control_state)
# Apply jerk limits based on tuning approach
# Predictive tuning uses calculated desired jerk directly
# Minimal Dynamic tuning applies only a speed based approach
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
j_ego_upper, j_ego_lower = self._calculate_lookahead_jerk(accel_error, velocity)
desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)
desired_jerk_lower = min(max(-j_ego_lower, MIN_JERK), lower_speed_factor)
self.jerk_upper = desired_jerk_upper
self.jerk_lower = desired_jerk_lower if not self.CP.radarUnavailable else 5.0
else:
self.jerk_upper = upper_speed_factor
self.jerk_lower = lower_speed_factor
# Disable jerk when longitudinal control is inactive
if not CC.longActive:
self.jerk_upper = 0.0
self.jerk_lower = 0.0
def calculate_accel(self, CC: structs.CarControl) -> None:
"""Calculate commanded acceleration using jerk-limited approach.
Args:
CC: Car control signals
"""
# Skip custom processing if tuning is disabled or radar unavailable
if not self.enabled:
self.desired_accel = self.accel_cmd
self.actual_accel = self.accel_cmd
return
# Reset acceleration when control is inactive
if not CC.longActive:
self.desired_accel = 0.0
self.actual_accel = 0.0
self.accel_last = 0.0
return
# Force zero acceleration during stopping
if self.stopping:
self.desired_accel = 0.0
else:
self.desired_accel = float(np.clip(self.accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
# Apply jerk-limited integration to get smooth acceleration
if not self.CP.radarUnavailable:
self.actual_accel = jerk_limited_integrator(self.desired_accel, self.accel_last, self.jerk_upper, self.jerk_lower)
else:
self.actual_accel = self.desired_accel
self.accel_last = self.actual_accel
def calculate_comfort_band(self, CC: structs.CarControl, CS: CarStateBase) -> None:
if not self.enabled or not CC.longActive:
self.comfort_band_upper = 0.0
self.comfort_band_lower = 0.0
return
accel = CS.out.aEgo
accel_vals = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
decel_vals = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
self.comfort_band_upper = float(np.interp(accel, accel_vals, comfort_band_vals))
self.comfort_band_lower = float(np.interp(accel, decel_vals, comfort_band_vals[::-1]))
else:
self.comfort_band_upper = float(np.interp(accel, [0.0, 2.0], [0.0, 0.04]))
self.comfort_band_lower = float(np.interp(accel, [-3.5, -0.05], [0.04, 0.0]))
def get_tuning_state(self) -> None:
"""Update the tuning state object with current control values.
External components depend on this state for longitudinal control.
"""
self.tuning = LongitudinalState(
desired_accel=self.desired_accel,
actual_accel=self.actual_accel,
accel_last=self.accel_last,
jerk_upper=self.jerk_upper,
jerk_lower=self.jerk_lower,
comfort_band_upper=self.comfort_band_upper,
comfort_band_lower=self.comfort_band_lower,
stopping=self.stopping,
)
def update(self, CC: structs.CarControl, CS: CarStateBase) -> None:
"""Update longitudinal control calculations.
This is the main entry point called externally.
Args:
CC: Car control signals including actuators
CS: Car state information
"""
actuators = CC.actuators
long_control_state = actuators.longControlState
self.accel_cmd = CC.actuators.accel
self.get_stopping_state(actuators)
self.calculate_jerk(CC, CS, long_control_state)
self.calculate_accel(CC)
self.calculate_comfort_band(CC, CS)
self.get_tuning_state()
self.long_control_state_last = long_control_state
# TODO-SP Why were we calculating both jerks at once lol
@@ -0,0 +1,53 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from opendbc.car import structs, DT_CTRL, rate_limit
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.sunnypilot.car.hyundai.longitudinal.config import CarTuningConfig, TUNING_CONFIGS, CAR_SPECIFIC_CONFIGS
JERK_THRESHOLD = 0.1
JERK_STEP = 0.1
class LongitudinalTuningType:
OFF = 0
DYNAMIC = 1
PREDICTIVE = 2
def get_car_config(CP: structs.CarParams) -> CarTuningConfig:
# Get car type flags from specific configs or determine from car flags
car_config = CAR_SPECIFIC_CONFIGS.get(CP.carFingerprint)
# If car is not in specific configs, determine from flags
if car_config is None:
if CP.flags & HyundaiFlags.CANFD:
car_config = TUNING_CONFIGS["CANFD"]
elif CP.flags & HyundaiFlags.EV:
car_config = TUNING_CONFIGS["EV"]
elif CP.flags & HyundaiFlags.HYBRID:
car_config = TUNING_CONFIGS["HYBRID"]
else:
car_config = TUNING_CONFIGS["DEFAULT"]
return car_config
def get_longitudinal_tune(CP: structs.CarParams) -> None:
config = get_car_config(CP)
CP.vEgoStopping = config.v_ego_stopping
CP.vEgoStarting = config.v_ego_starting
CP.stoppingDecelRate = config.stopping_decel_rate
CP.startingState = False
CP.longitudinalActuatorDelay = config.longitudinal_actuator_delay
def jerk_limited_integrator(desired_accel, last_accel, jerk_upper, jerk_lower) -> float:
if desired_accel >= last_accel:
val = jerk_upper * DT_CTRL * 5
else:
val = jerk_lower * DT_CTRL * 5
return rate_limit(desired_accel, last_accel, -val, val)
@@ -0,0 +1,108 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from enum import StrEnum
from collections import namedtuple
from opendbc.car import Bus, DT_CTRL, structs
from opendbc.car.hyundai.values import CAR
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
from opendbc.sunnypilot.mads_base import MadsCarStateBase
from opendbc.can.parser import CANParser
ButtonType = structs.CarState.ButtonEvent.Type
MadsDataSP = namedtuple("MadsDataSP",
["enable_mads", "lat_active", "disengaging", "paused"])
class MadsCarController:
def __init__(self):
self.mads = MadsDataSP(False, False, False, False)
self.lat_disengage_blink = 0
self.lat_disengage_init = False
self.prev_lat_active = False
self.lkas_icon = 0
self.lfa_icon = 0
# display LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive
def mads_status_update(self, CC: structs.CarControl, CC_SP: structs.CarControlSP, frame: int) -> MadsDataSP:
enable_mads = CC_SP.mads.available
if CC.latActive:
self.lat_disengage_init = False
elif self.prev_lat_active:
self.lat_disengage_init = True
if not self.lat_disengage_init:
self.lat_disengage_blink = frame
paused = CC_SP.mads.enabled and not CC.latActive
disengaging = (frame - self.lat_disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
self.prev_lat_active = CC.latActive
return MadsDataSP(enable_mads, CC.latActive, disengaging, paused)
def create_lkas_icon(self, CP: structs.CarParams, enabled: bool) -> int:
if self.mads.enable_mads:
lkas_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1
else:
lkas_icon = 2 if enabled else 1
# Override common signals for KIA_OPTIMA_G4 and KIA_OPTIMA_G4_FL
if CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.HYUNDAI_KONA_NON_SCC):
lkas_icon = 3 if (self.mads.lat_active if self.mads.enable_mads else enabled) else 1
return lkas_icon
def create_lfa_icon(self, enabled: bool) -> int:
if self.mads.enable_mads:
lfa_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1 if self.mads.paused else 0
else:
lfa_icon = 2 if enabled else 0
return lfa_icon
def update(self, CP: structs.CarParams, CC: structs.CarControl, CC_SP: structs.CarControlSP, frame: int) -> None:
self.mads = self.mads_status_update(CC, CC_SP, frame)
self.lkas_icon = self.create_lkas_icon(CP, CC.enabled)
self.lfa_icon = self.create_lfa_icon(CC.enabled)
class MadsCarState(MadsCarStateBase):
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams):
super().__init__(CP, CP_SP)
self.main_cruise_enabled: bool = False
@staticmethod
def get_parser(CP, CP_SP, pt_messages) -> None:
pass
def get_main_cruise(self, ret: structs.CarState) -> bool:
if self.CP_SP.flags & HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE:
if any(be.type == ButtonType.mainCruise and be.pressed for be in ret.buttonEvents):
self.main_cruise_enabled = not self.main_cruise_enabled
else:
self.main_cruise_enabled = True
return self.main_cruise_enabled if ret.cruiseState.available else False
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
pass
def update_mads_canfd(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
if not self.CP.openpilotLongitudinalControl:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.available = cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
@@ -0,0 +1,82 @@
from opendbc.can.parser import CANParser
from opendbc.car import structs, Bus
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import DBC, HyundaiFlags
from opendbc.sunnypilot.car.hyundai.escc import EsccRadarInterfaceBase
class RadarInterfaceExt(EsccRadarInterfaceBase):
msg_src: str
trigger_msg: int
rcp: CANParser
pts: dict[int, structs.RadarData.RadarPoint]
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
EsccRadarInterfaceBase.__init__(self, CP, CP_SP)
self.CP = CP
self.CP_SP = CP_SP
self.track_id = 0
@property
def use_radar_interface_ext(self) -> bool:
return self.use_escc or self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC)
def get_msg_src(self) -> str | None:
if self.use_escc:
return "ESCC"
if self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC):
return "SCC_CONTROL" if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else "SCC11"
def get_radar_ext_can_parser(self) -> CANParser:
if self.ESCC.enabled:
lead_src, bus = "ESCC", 0
elif self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC):
lead_src = "SCC_CONTROL" if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else "SCC11"
bus = CanBus(self.CP).CAM if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else 2
else:
return None
messages = [(lead_src, 50)]
return CANParser(DBC[self.CP.carFingerprint][Bus.pt], messages, bus)
def get_trigger_msg(self, default_trigger_msg) -> int:
if self.ESCC.enabled:
return self.ESCC.trigger_msg
if self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC):
return 0x1A0 if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else 0x420
return default_trigger_msg
def initialize_radar_ext(self, default_trigger_msg) -> None:
if self.ESCC.enabled:
self.use_escc = True
self.rcp = self.get_radar_ext_can_parser()
self.trigger_msg = self.get_trigger_msg(default_trigger_msg)
def update_ext(self, ret: structs.RadarData) -> structs.RadarData:
if not self.rcp.can_valid:
ret.errors.canError = True
return ret
for ii in range(1):
msg_src = self.get_msg_src()
msg = self.rcp.vl[msg_src]
if ii not in self.pts:
self.pts[ii] = structs.RadarData.RadarPoint()
self.pts[ii].trackId = self.track_id
self.track_id += 1
valid = msg['ACC_ObjDist'] < 204.6 if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else msg['ACC_ObjStatus']
if valid:
self.pts[ii].dRel = msg['ACC_ObjDist']
self.pts[ii].yRel = float('nan') # FIXME-SP: Only some cars have lateral position from SCC
self.pts[ii].vRel = msg['ACC_ObjRelSpd']
else:
del self.pts[ii]
ret.points = list(self.pts.values())
return ret
Whitespace-only changes.
@@ -0,0 +1,64 @@
import unittest
from opendbc.sunnypilot.car.hyundai.escc import EnhancedSmartCruiseControl, ESCC_MSG
from opendbc.car.hyundai.carstate import CarState
from opendbc.car import structs
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
class TestEscc(unittest.TestCase):
def _make_car_params(self):
params = structs.CarParams()
params.carFingerprint = "HYUNDAI_SONATA"
return params
def _make_car_params_sp(self):
params = structs.CarParamsSP()
params.flags = HyundaiFlagsSP.ENHANCED_SCC
return params
def _make_escc(self):
return EnhancedSmartCruiseControl(self._make_car_params(), self._make_car_params_sp())
def test_escc_msg_id(self):
escc = self._make_escc()
self.assertEqual(escc.trigger_msg, ESCC_MSG)
def test_enabled_flag(self):
car_params = self._make_car_params()
for value in range(256):
with self.subTest(flags=value):
car_params_sp = structs.CarParamsSP()
car_params_sp.flags = value
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
self.assertEqual(escc.enabled, bool(value & HyundaiFlagsSP.ENHANCED_SCC))
def test_update_car_state(self):
car_params = self._make_car_params()
car_params_sp = self._make_car_params_sp()
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
car_state = CarState(car_params, car_params_sp)
car_state.escc_cmd_act = 1
car_state.escc_aeb_warning = 1
car_state.escc_aeb_dec_cmd_act = 1
car_state.escc_aeb_dec_cmd = 1
escc.update_car_state(car_state)
self.assertEqual(escc.car_state, car_state)
def test_update_scc12(self):
car_params = self._make_car_params()
car_params_sp = self._make_car_params_sp()
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
car_state = CarState(car_params, car_params_sp)
car_state.escc_cmd_act = 1
car_state.escc_aeb_warning = 1
car_state.escc_aeb_dec_cmd_act = 1
car_state.escc_aeb_dec_cmd = 1
escc.update_car_state(car_state)
scc12_message = {}
escc.update_scc12(scc12_message)
self.assertEqual(scc12_message["AEB_CmdAct"], 1)
self.assertEqual(scc12_message["CF_VSM_Warn"], 1)
self.assertEqual(scc12_message["CF_VSM_DecCmdAct"], 1)
self.assertEqual(scc12_message["CR_VSM_DecCmd"], 1)
self.assertEqual(scc12_message["AEB_Status"], 2)
@@ -0,0 +1,78 @@
import unittest
from enum import IntFlag
from opendbc.sunnypilot.car.hyundai.lead_data_ext import LeadDataCarController, CanLeadData, CanFdLeadData
from opendbc.car import structs
from opendbc.car.hyundai.values import HyundaiFlags
def make_carparams(flags: IntFlag = HyundaiFlags.LEGACY):
cp = structs.CarParams()
cp.carFingerprint = "HYUNDAI_SONATA"
cp.flags = flags.value
return cp
def make_carcontrolsp(leadDistance=10.0, leadRelSpeed=0.0, leadVisible=True):
c = structs.CarControlSP()
c.leadOne.dRel = leadDistance
c.leadOne.vRel = leadRelSpeed
c.leadOne.status = leadVisible
return c
class TestLeadDataCarController(unittest.TestCase):
def test_update_object_gap(self):
ctrl = LeadDataCarController(make_carparams())
# Initial value should be 0
self.assertEqual(ctrl.object_gap, 0)
# Set to 15 (should become 2 after hysteresis)
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
ctrl._update_object_gap(15)
self.assertEqual(ctrl.object_gap, 2)
# Set to 22 (should become 3 after hysteresis)
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
ctrl._update_object_gap(22)
self.assertEqual(ctrl.object_gap, 3)
# Set to 0 (should become 0 after hysteresis)
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
ctrl._update_object_gap(0)
self.assertEqual(ctrl.object_gap, 0)
def test_update_lead_visible_hysteresis(self):
ctrl = LeadDataCarController(make_carparams())
ctrl._update_lead_visible_hysteresis(True)
self.assertIsInstance(ctrl.lead_visible, bool)
ctrl._update_lead_visible_hysteresis(False)
self.assertIsInstance(ctrl.lead_visible, bool)
def test_update(self):
ctrl = LeadDataCarController(make_carparams())
sp = make_carcontrolsp(leadDistance=25, leadRelSpeed=-0.5, leadVisible=True)
ctrl.update(sp)
self.assertEqual(ctrl.lead_distance, 25)
self.assertEqual(ctrl.lead_rel_speed, -0.5)
self.assertIsInstance(ctrl.lead_visible, bool)
def test_lead_data_can(self):
ctrl = LeadDataCarController(make_carparams())
ctrl.object_gap = 1
ctrl.lead_distance = 10
ctrl.lead_rel_speed = -0.3
ctrl.lead_visible = True
ld = ctrl.lead_data
self.assertIsInstance(ld, CanLeadData)
self.assertEqual(ld.object_rel_gap, 2)
def test_lead_data_canfd(self):
ctrl = LeadDataCarController(make_carparams(HyundaiFlags.CANFD))
ctrl.object_gap = 1
ctrl.lead_distance = 10
ctrl.lead_rel_speed = 1.0
ctrl.lead_visible = True
ld = ctrl.lead_data
self.assertIsInstance(ld, CanFdLeadData)
self.assertEqual(ld.object_rel_gap, 1)
@@ -0,0 +1,121 @@
import unittest
from opendbc.testing import parameterized
from opendbc.car import CanData
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR, HyundaiFlags
from opendbc.sunnypilot.car.hyundai.escc import ESCC_MSG
ESCC_CARS = [
(CAR.HYUNDAI_ELANTRA_2021, ESCC_MSG),
]
CAMERA_SCC_CARS = [
(CAR.HYUNDAI_KONA_EV_2022, 0, 0x420, "SCC11"),
(CAR.HYUNDAI_IONIQ_5, HyundaiFlags.CANFD_CAMERA_SCC.value, 0x1A0, "SCC_CONTROL"),
]
STANDARD_RADAR_CARS = [
(CAR.HYUNDAI_ELANTRA_2021, 0),
(CAR.HYUNDAI_SANTA_FE, 0),
]
class TestRadarInterfaceExt(unittest.TestCase):
@staticmethod
def _setup_platform(car_name, additional_flags=0, escc_msg=None):
"""Set up the platform with specific parameters"""
CarInterface = interfaces[car_name]
CP = CarInterface.get_non_essential_params(car_name)
CP.flags |= additional_flags
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
CI = CarInterface(CP, CP_SP)
RD = CI.RadarInterface(CP, CP_SP)
if escc_msg is not None and hasattr(RD, 'use_escc'):
try:
RD.use_escc = True
except AttributeError:
object.__setattr__(RD, 'use_escc', True)
return RD, CP, CP_SP
@parameterized("car_name, escc_msg", ESCC_CARS)
def test_escc_radar_interface(self, car_name, escc_msg):
"""Test radar interface for ESCC-enabled cars"""
RD, CP, CP_SP = self._setup_platform(car_name, escc_msg=escc_msg)
# Assert that ESCC features are present
if hasattr(RD, 'use_escc'):
self.assertTrue(RD.use_escc, "ESCC car should have use_escc=True")
if hasattr(RD, 'use_radar_interface_ext'):
self.assertTrue(RD.use_radar_interface_ext, "ESCC car should use radar interface ext")
# Run radar interface once
RD.update([])
# Test radar fault
if not CP.radarUnavailable and RD.rcp is not None:
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
rr = RD.update(cans)
self.assertTrue(rr is None or len(rr.errors) > 0)
@parameterized("car_name, flags, expected_trigger, msg_src", CAMERA_SCC_CARS)
def test_camera_scc_radar_interface(self, car_name, flags, expected_trigger, msg_src):
"""Test radar interface for Camera SCC cars"""
RD, CP, CP_SP = self._setup_platform(car_name, additional_flags=flags)
# Assert Camera SCC flag is set appropriately
if flags & HyundaiFlags.CAMERA_SCC:
self.assertTrue(CP.flags & HyundaiFlags.CAMERA_SCC, "Car should have CAMERA_SCC flag")
if flags & HyundaiFlags.CANFD_CAMERA_SCC:
self.assertTrue(CP.flags & HyundaiFlags.CANFD_CAMERA_SCC, "Car should have CANFD_CAMERA_SCC flag")
# Check if using radar interface ext
if hasattr(RD, 'use_radar_interface_ext'):
self.assertTrue(RD.use_radar_interface_ext, "Camera SCC car should use radar interface ext")
# Verify trigger message
if hasattr(RD, 'trigger_msg'):
self.assertEqual(RD.trigger_msg, expected_trigger, f"Expected trigger_msg {expected_trigger}, got {RD.trigger_msg}")
# Run radar interface once
RD.update([])
# Test radar fault
if not CP.radarUnavailable and RD.rcp is not None:
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
rr = RD.update(cans)
self.assertTrue(rr is None or len(rr.errors) > 0)
@parameterized("car_name, flags", STANDARD_RADAR_CARS)
def test_standard_radar_interface(self, car_name, flags):
"""Test radar interface for standard radar cars"""
RD, CP, CP_SP = self._setup_platform(car_name, additional_flags=flags)
# Standard cars should not use radar interface ext
if hasattr(RD, 'use_radar_interface_ext'):
self.assertFalse(RD.use_radar_interface_ext, "Standard car should not use radar interface ext")
# Run radar interface once
RD.update([])
# For standard radar, test the _update method directly if available
if not CP.radarUnavailable and RD.rcp is not None and \
hasattr(RD, '_update') and hasattr(RD, 'trigger_msg'):
# Setup for _update test if needed
if hasattr(RD, 'updated_messages'):
RD.updated_messages = {RD.trigger_msg}
RD._update(RD.updated_messages)
# Test radar fault
if not CP.radarUnavailable and RD.rcp is not None:
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
rr = RD.update(cans)
self.assertTrue(rr is None or len(rr.errors) > 0)
@@ -0,0 +1,173 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import unittest
import numpy as np
from dataclasses import dataclass, field
from opendbc.car import structs
from opendbc.sunnypilot.car.hyundai.longitudinal.controller import LongitudinalController
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
LongCtrlState = structs.CarControl.Actuators.LongControlState
@dataclass
class CP:
carFingerprint: str = "KIA_NIRO_EV"
flags: int = 0
radarUnavailable: bool = False
@dataclass
class Actuators:
accel: float = 0.0
longControlState = LongCtrlState
@dataclass
class CC:
actuators: Actuators = field(default_factory=lambda: Actuators())
longActive: bool = True
@dataclass
class Out:
vEgo: float = 0.0
aEgo: float = 0.0
@dataclass
class CS:
out: Out = field(default_factory=lambda: Out())
aBasis: float = 0.0
class TestLongitudinalTuningController(unittest.TestCase):
def setUp(self):
self.CP = CP(flags=0)
self.CP_SP = CP(flags=0)
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_PREDICTIVE
self.CS = CS()
self.CC = CC()
self.controller = LongitudinalController(self.CP, self.CP_SP)
def test_enabled_and_disabled(self):
self.assertTrue(self.controller.enabled)
self.CP_SP.flags = 0
self.assertFalse(self.controller.enabled)
def test_stopping_state(self):
self.CC.actuators.longControlState = LongCtrlState.stopping
self.controller.get_stopping_state(self.CC.actuators)
self.assertTrue(self.controller.stopping)
self.assertEqual(self.controller.stopping_count, 0)
self.CC.actuators.longControlState = LongCtrlState.pid
self.controller.get_stopping_state(self.CC.actuators)
self.assertFalse(self.controller.stopping)
def test_calc_speed_based_jerk(self):
self.assertEqual((0.5, 5.0), self.controller._calculate_speed_based_jerk_limits(0.0, LongCtrlState.stopping))
velocities = [0.0, 2.0, 5.0, 7.0, 10.0, 15.0, 20.0, 25.0, 30.0]
for velocity in velocities:
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0]))
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
expected = (upper_limit, lower_limit)
actual = self.controller._calculate_speed_based_jerk_limits(velocity, LongCtrlState.pid)
self.assertEqual(expected, actual)
def test_calc_lookahead_jerk(self):
upper, lower = self.controller._calculate_lookahead_jerk(-0.5, 4.9)
self.assertAlmostEqual(upper, -1.1, delta=0.1)
self.assertAlmostEqual(lower, -1.1, delta=0.1)
upper, lower = self.controller._calculate_lookahead_jerk(0.5, 5.0)
self.assertAlmostEqual(upper, 1.1, delta=0.1)
self.assertAlmostEqual(lower, 1.1, delta=0.1)
def test_calc_jerk(self):
self.CP_SP.flags = 0
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
self.assertEqual(self.controller.jerk_upper, 3.0)
self.assertEqual(self.controller.jerk_lower, 5.0)
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.off)
self.assertEqual(self.controller.jerk_upper, 1.0)
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_PREDICTIVE
self.controller.__init__(self.CP, self.CP_SP)
self.CS.out.vEgo = 10.0
self.controller.accel_cmd = -3.5
self.controller.accel_last = -1.0
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
self.assertEqual(self.controller.jerk_upper, 0.5)
self.assertAlmostEqual(self.controller.jerk_lower, 3.3, delta=0.01)
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
self.controller.__init__(self.CP, self.CP_SP)
self.CS.out.vEgo = 10.0
self.CS.aBasis = -3.3
self.CS.out.aEgo = -3.5
self.controller.accel_cmd = -3.5
self.controller.accel_last = -1.0
for _ in range(50):
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
self.assertAlmostEqual(self.controller.jerk_upper, 2.67, delta=0.01)
self.assertAlmostEqual(self.controller.jerk_lower, 3.33, delta=0.01)
def test_calc_accel(self):
self.CP_SP.flags = 0
self.controller.accel_cmd = 1.5
self.controller.calculate_accel(self.CC)
self.assertEqual(self.controller.desired_accel, self.controller.accel_cmd)
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
self.CC.longActive = False
self.controller.calculate_accel(self.CC)
self.assertEqual(self.controller.desired_accel, 0.0)
self.CC.longActive = True
self.controller.stopping = True
self.controller.calculate_accel(self.CC)
self.assertEqual(self.controller.desired_accel, 0.0)
def test_calc_comfort_band(self):
stock_decels_list = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
stock_accels_list = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
stock_comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
decels_list = [-3.5, -3.1, -2.245, -1.853, -1.234, -0.64352, -0.06432, -0.00005]
accels_list = [0.0, 0.23345, 0.456, 0.5677, 0.6788, 0.834, 1.0, 1.3456, 1.8]
for decel in decels_list:
self.CS.out.aEgo = decel
self.controller.calculate_comfort_band(self.CC, self.CS)
actual = self.controller.comfort_band_lower
expected = float(np.interp(decel, stock_decels_list, [0.1, 0.08, 0.06, 0.04, 0.02, 0.0]))
self.assertEqual(actual, expected)
self.assertEqual(self.controller.comfort_band_upper, 0.0)
for accel in accels_list:
self.CS.out.aEgo = accel
self.controller.calculate_comfort_band(self.CC, self.CS)
actual = self.controller.comfort_band_upper
expected = float(np.interp(accel, stock_accels_list, stock_comfort_band_vals))
self.assertEqual(actual, expected)
self.assertEqual(self.controller.comfort_band_lower, 0.0)
def test_update(self):
self.CC.actuators.accel = 2.0
self.CC.actuators.longControlState = LongCtrlState.pid
self.CS.aBasis = 1.75
self.CS.out.aEgo = 2.0
self.CS.out.vEgo = 5.0
self.controller.update(self.CC, self.CS)
self.assertEqual(self.controller.jerk_lower, 0.5)
self.assertEqual(self.controller.jerk_upper, 3.0)
self.assertEqual(self.controller.comfort_band_lower, 0.0)
self.assertEqual(self.controller.comfort_band_upper, 0.10)
self.assertEqual(self.controller.desired_accel, 2.0)
@@ -0,0 +1,33 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from enum import IntFlag
class HyundaiSafetyFlagsSP:
DEFAULT = 0
ESCC = 1
LONG_MAIN_CRUISE_TOGGLEABLE = 2
HAS_LDA_BUTTON = 4
NON_SCC = 8
class HyundaiFlagsSP(IntFlag):
"""
Flags for Hyundai specific quirks within sunnypilot.
"""
ENHANCED_SCC = 1
HAS_LFA_BUTTON = 2 # Deprecated in favor of HyundaiFlags.HAS_LDA_BUTTON
LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 2 ** 2
ENABLE_RADAR_TRACKS_DEPRECATED = 2 ** 3
LONG_TUNING_DYNAMIC = 2 ** 4
LONG_TUNING_PREDICTIVE = 2 ** 5
NON_SCC = 2 ** 6
NON_SCC_RADAR_FCA = 2 ** 7 # most with FCA come from the camera
NON_SCC_NO_FCA = 2 ** 8 # not all have FCA
SPEED_LIMIT_AVAILABLE = 2 ** 9 # platforms with speed limit data available
HAS_LKAS12 = 2 ** 10