mirror of
https://github.com/infiniteCable2/opendbc.git
synced 2026-06-08 10:54:51 +08:00
c4f5a4f9d2
# Conflicts: # docs/CARS.md # opendbc/car/car_helpers.py # opendbc/car/chrysler/carcontroller.py # opendbc/car/chrysler/values.py # opendbc/car/honda/carcontroller.py # opendbc/car/honda/hondacan.py # opendbc/car/hyundai/hyundaicanfd.py # opendbc/car/rivian/interface.py
254 lines
6.9 KiB
Python
254 lines
6.9 KiB
Python
import numpy as np
|
|
from opendbc.car import CanBusBase
|
|
from opendbc.car.crc import CRC16_XMODEM
|
|
from opendbc.car.hyundai.values import HyundaiFlags
|
|
from opendbc.sunnypilot.car.hyundai.lead_data_ext import CanFdLeadData
|
|
|
|
|
|
class CanBus(CanBusBase):
|
|
def __init__(self, CP, fingerprint=None, lka_steering=None) -> None:
|
|
super().__init__(CP, fingerprint)
|
|
|
|
if lka_steering is None:
|
|
lka_steering = CP.flags & HyundaiFlags.CANFD_LKA_STEERING.value if CP is not None else False
|
|
|
|
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. LKA steering cars
|
|
# have a different harness than the LFA steering variants in order to split
|
|
# a different bus, since the steering is done by different ECUs.
|
|
self._a, self._e = 1, 0
|
|
if lka_steering:
|
|
self._a, self._e = 0, 1
|
|
|
|
self._a += self.offset
|
|
self._e += self.offset
|
|
self._cam = 2 + self.offset
|
|
|
|
@property
|
|
def ECAN(self):
|
|
return self._e
|
|
|
|
@property
|
|
def ACAN(self):
|
|
return self._a
|
|
|
|
@property
|
|
def CAM(self):
|
|
return self._cam
|
|
|
|
|
|
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, lkas_icon):
|
|
values = {
|
|
"LKA_OptUsmSta": 2,
|
|
"LKA_SysIndReq": lkas_icon,
|
|
"StrTqReqVal": apply_torque,
|
|
"LKA_SysWrn": 0,
|
|
"ActToiSta": 1 if lat_active else 0,
|
|
"LKA_UsmMod": 0, # hide LKAS settings
|
|
"LKA_RcgSta": 0,
|
|
"Damping_Gain": 100, # can potentially tuned for better perf [3, 200]
|
|
}
|
|
|
|
ret = []
|
|
if CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
|
|
lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS"
|
|
if CP.openpilotLongitudinalControl:
|
|
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
|
|
ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, values))
|
|
else:
|
|
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
|
|
|
|
return ret
|
|
|
|
|
|
def create_suppress_lfa(packer, CAN, lfa_block_msg, lka_steering_alt):
|
|
suppress_msg = "CAM_0x362" if lka_steering_alt else "CAM_0x2a4"
|
|
msg_bytes = 32 if lka_steering_alt else 24
|
|
|
|
values = {f"BYTE{i}": lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
|
|
values["COUNTER"] = lfa_block_msg["COUNTER"]
|
|
values["SET_ME_0"] = 0
|
|
values["SET_ME_0_2"] = 0
|
|
values["LEFT_LANE_LINE"] = 0
|
|
values["RIGHT_LANE_LINE"] = 0
|
|
return packer.make_can_msg(suppress_msg, CAN.ACAN, values)
|
|
|
|
|
|
def create_buttons(packer, CP, CAN, cnt, btn):
|
|
values = {
|
|
"COUNTER": cnt,
|
|
"SET_ME_1": 1,
|
|
"CRUISE_BUTTONS": btn,
|
|
}
|
|
|
|
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_LKA_STEERING else CAN.CAM
|
|
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
|
|
|
|
|
|
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
|
|
# CAN FD camera-based SCC requires additional signals to be preserved
|
|
# verbatim from the previous SCC_CONTROL frame to avoid checksum or
|
|
# state validation faults. Classic CAN SCC only validates a subset.
|
|
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
|
|
values = {s: cruise_info_copy[s] for s in [
|
|
"COUNTER",
|
|
"CHECKSUM",
|
|
"NEW_SIGNAL_1",
|
|
"MainMode_ACC",
|
|
"ACCMode",
|
|
"ZEROS_9",
|
|
"CRUISE_STANDSTILL",
|
|
"ZEROS_5",
|
|
"DISTANCE_SETTING",
|
|
"VSetDis",
|
|
]}
|
|
else:
|
|
values = {s: cruise_info_copy[s] for s in [
|
|
"COUNTER",
|
|
"CHECKSUM",
|
|
"ACCMode",
|
|
"VSetDis",
|
|
"CRUISE_STANDSTILL",
|
|
]}
|
|
values.update({
|
|
"ACCMode": 4,
|
|
"aReqRaw": 0.0,
|
|
"aReqValue": 0.0,
|
|
})
|
|
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
|
|
|
|
|
|
def create_lfahda_cluster(packer, CAN, enabled, lfa_icon):
|
|
values = {
|
|
"HDA_ICON": 1 if enabled else 0,
|
|
"LFA_ICON": lfa_icon,
|
|
}
|
|
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
|
|
|
|
|
|
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control,
|
|
lead_data: CanFdLeadData, main_cruise_enabled, tuning):
|
|
jerk = 5
|
|
jn = jerk / 50
|
|
if not enabled or gas_override:
|
|
a_val, a_raw = 0, 0
|
|
else:
|
|
a_raw = accel # noqa: F841
|
|
a_val = np.clip(accel, accel_last - jn, accel_last + jn) # noqa: F841
|
|
|
|
values = {
|
|
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
|
|
"MainMode_ACC": 1 if main_cruise_enabled else 0,
|
|
"StopReq": 1 if tuning.stopping else 0,
|
|
"aReqValue": tuning.actual_accel,
|
|
"aReqRaw": tuning.actual_accel,
|
|
"VSetDis": set_speed,
|
|
"JerkLowerLimit": tuning.jerk_lower,
|
|
"JerkUpperLimit": tuning.jerk_upper,
|
|
|
|
"ACC_ObjDist": int(lead_data.lead_distance),
|
|
"ACC_ObjRelSpd": lead_data.lead_rel_speed,
|
|
"ObjValid": int(not lead_data.lead_visible),
|
|
"SCC_ObjSta": 0 if not (enabled and lead_data.lead_visible) else (1 if gas_override else 2),
|
|
"SET_ME_2": 0x4,
|
|
"SET_ME_3": 0x3,
|
|
"SET_ME_TMP_64": 0x64,
|
|
"DISTANCE_SETTING": hud_control.leadDistanceBars,
|
|
}
|
|
|
|
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
|
|
|
|
|
|
def create_spas_messages(packer, CAN, left_blink, right_blink):
|
|
ret = []
|
|
|
|
values = {
|
|
}
|
|
ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values))
|
|
|
|
blink = 0
|
|
if left_blink:
|
|
blink = 3
|
|
elif right_blink:
|
|
blink = 4
|
|
values = {
|
|
"BLINKER_CONTROL": blink,
|
|
}
|
|
ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values))
|
|
|
|
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
|
|
|
|
ret = []
|
|
|
|
values = {
|
|
}
|
|
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
|
|
|
|
ret.extend(create_fca_warning_light(packer, CAN, frame))
|
|
|
|
if frame % 5 == 0:
|
|
values = {
|
|
'SET_ME_1C': 0x1c,
|
|
'SET_ME_FF': 0xff,
|
|
'SET_ME_TMP_F': 0xf,
|
|
'SET_ME_TMP_F_2': 0xf,
|
|
}
|
|
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
|
|
|
|
values = {
|
|
'SET_ME_E1': 0xe1,
|
|
'SET_ME_3A': 0x3a,
|
|
}
|
|
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values))
|
|
|
|
if frame % 20 == 0:
|
|
values = {
|
|
'SET_ME_15': 0x15,
|
|
}
|
|
ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values))
|
|
|
|
if frame % 100 == 0:
|
|
values = {
|
|
'SET_ME_22': 0x22,
|
|
'SET_ME_41': 0x41,
|
|
}
|
|
ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values))
|
|
|
|
return ret
|
|
|
|
|
|
def hkg_can_fd_checksum(address: int, sig, d: bytearray) -> int:
|
|
crc = 0
|
|
for i in range(2, len(d)):
|
|
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ d[i]]) & 0xFFFF
|
|
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ ((address >> 0) & 0xFF)]) & 0xFFFF
|
|
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ ((address >> 8) & 0xFF)]) & 0xFFFF
|
|
if len(d) == 8:
|
|
crc ^= 0x5F29
|
|
elif len(d) == 16:
|
|
crc ^= 0x041D
|
|
elif len(d) == 24:
|
|
crc ^= 0x819D
|
|
elif len(d) == 32:
|
|
crc ^= 0x9F5B
|
|
return crc
|