mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-08 08:35:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into dev-priv/master

Browse Source 
# Conflicts:
#	panda
#	selfdrive/car/hyundai/carcontroller.py
#	selfdrive/car/hyundai/carstate.py
#	selfdrive/car/hyundai/hyundaican.py
#	selfdrive/car/hyundai/interface.py
This commit is contained in:
Jason Wen
2023-02-17 18:39:06 -05:00
parent 36461f1885 2830c020f6
commit 2fc2e108be
7 changed files with 139 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda
+1 -1
Submodule panda updated: be1d9dd5bb...fe1ffdc07b
selfdrive/car/hyundai/carcontroller.py
+7 -4
View File
@@ -94,12 +94,14 @@ class CarController:
self.lat_active_last = CC.latActive
escc = self.CP.flags & HyundaiFlags.SP_ENHANCED_SCC.value
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
if self.frame % 100 == 0 and not ((self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or escc) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
@@ -197,7 +199,8 @@ class CarController:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, CS.mainEnabled))
hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, CS.mainEnabled,
CS, escc))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
@@ -205,10 +208,10 @@ class CarController:
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
can_sends.extend(hyundaican.create_acc_opt(self.packer, escc))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl and not escc:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
new_actuators = actuators.copy()
selfdrive/car/hyundai/carstate.py
+35
View File
@@ -48,6 +48,10 @@ class CarState(CarStateBase):
self.lfa_enabled = False
self.prev_lfa_enabled = False
self.mainEnabled = False
self.escc_aeb_warning = 0
self.escc_aeb_dec_cmd_act = 0
self.escc_cmd_act = 0
self.escc_aeb_dec_cmd = 0
def update(self, cp, cp_cam):
if self.CP.carFingerprint in CANFD_CAR:
@@ -146,6 +150,20 @@ class CarState(CarStateBase):
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = aeb_warning and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
elif self.CP.flags & HyundaiFlags.SP_ENHANCED_SCC:
aeb_src = "ESCC"
aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct"
aeb_warning_sig = "CF_VSM_Warn_FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "CF_VSM_Warn_SCC12"
aeb_braking_sig = "CF_VSM_DecCmdAct_FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "CF_VSM_DecCmdAct_SCC12"
aeb_braking_cmd = "CR_VSM_DecCmd_FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "CR_VSM_DecCmd_SCC12"
aeb_warning = cp.vl[aeb_src][aeb_warning_sig] != 0
aeb_braking = cp.vl[aeb_src][aeb_braking_sig] != 0 or cp.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = aeb_warning and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
self.escc_aeb_warning = cp.vl[aeb_src][aeb_warning_sig]
self.escc_aeb_dec_cmd_act = cp.vl[aeb_src][aeb_braking_sig]
self.escc_cmd_act = cp.vl[aeb_src][aeb_sig]
self.escc_aeb_dec_cmd = cp.vl[aeb_src][aeb_braking_cmd]
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
@@ -390,6 +408,23 @@ class CarState(CarStateBase):
signals.append(("LFA_Pressed", "BCM_PO_11"))
checks.append(("BCM_PO_11", 50))
if CP.flags & HyundaiFlags.SP_ENHANCED_SCC.value:
if CP.flags & HyundaiFlags.USE_FCA.value:
signals += [
("FCA_CmdAct", "ESCC"),
("CF_VSM_Warn_FCA11", "ESCC"),
("CF_VSM_DecCmdAct_FCA11", "ESCC"),
("CR_VSM_DecCmd_FCA11", "ESCC"),
]
else:
signals += [
("AEB_CmdAct", "ESCC"),
("CF_VSM_Warn_SCC12", "ESCC"),
("CF_VSM_DecCmdAct_SCC12", "ESCC"),
("CR_VSM_DecCmd_SCC12", "ESCC"),
]
checks.append(("ESCC", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@staticmethod
selfdrive/car/hyundai/hyundaican.py
+18 -7
View File
@@ -98,7 +98,8 @@ def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, main_enabled):
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, main_enabled,
CS, escc):
commands = []
scc11_values = {
@@ -120,6 +121,11 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
"aReqRaw": accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"CR_VSM_Alive": idx % 0xF,
"AEB_CmdAct": CS.escc_cmd_act,
"CF_VSM_Warn": CS.escc_aeb_warning,
"CF_VSM_DecCmdAct": CS.escc_aeb_dec_cmd_act,
"CR_VSM_DecCmd": CS.escc_aeb_dec_cmd,
}
scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2]
scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
@@ -140,9 +146,14 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
"CR_FCA_Alive": idx % 0xF,
"PAINT1_Status": 1,
"FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled
"PAINT1_Status": 0 if escc else 1,
"FCA_DrvSetStatus": 0 if escc else 1,
"FCA_Status": 0 if escc else 1, # AEB disabled
"FCA_CmdAct": CS.escc_cmd_act,
"CF_VSM_Warn": CS.escc_aeb_warning,
"CF_VSM_DecCmdAct": CS.escc_aeb_dec_cmd_act,
"CR_VSM_DecCmd": CS.escc_aeb_dec_cmd,
}
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
@@ -150,7 +161,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
return commands
def create_acc_opt(packer):
def create_acc_opt(packer, escc):
commands = []
scc13_values = {
@@ -161,8 +172,8 @@ def create_acc_opt(packer):
commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
fca12_values = {
"FCA_DrvSetState": 2,
"FCA_USM": 1, # AEB disabled
"FCA_DrvSetState": 0 if escc else 2,
"FCA_USM": 0 if escc else 1, # AEB disabled
}
commands.append(packer.make_can_msg("FCA12", 0, fca12_values))
selfdrive/car/hyundai/interface.py
+7 -1
View File
@@ -53,6 +53,9 @@ class CarInterface(CarInterfaceBase):
if 0x38d in fingerprint[0] or 0x38d in fingerprint[2]:
ret.flags |= HyundaiFlags.USE_FCA.value
if 0x2AB in fingerprint[0]:
ret.flags |= HyundaiFlags.SP_ENHANCED_SCC.value
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.
@@ -283,6 +286,9 @@ class CarInterface(CarInterfaceBase):
ret.flags |= HyundaiFlags.SP_CAN_LFA_BTN.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN
if ret.flags & HyundaiFlags.SP_ENHANCED_SCC:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_ESCC
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
if candidate in HYBRID_CAR:
@@ -304,7 +310,7 @@ class CarInterface(CarInterfaceBase):
@staticmethod
def init(CP, logcan, sendcan):
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value):
if CP.openpilotLongitudinalControl and not ((CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or (CP.flags & HyundaiFlags.SP_ENHANCED_SCC)):
addr, bus = 0x7d0, 0
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, 5
selfdrive/car/hyundai/radar_interface.py
+70 -35
View File
@@ -4,37 +4,52 @@ import math
from cereal import car
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase
from selfdrive.car.hyundai.values import DBC
from selfdrive.car.hyundai.values import DBC, HyundaiFlags
RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
def get_radar_can_parser(CP):
if DBC[CP.carFingerprint]['radar'] is None:
return None
signals = []
checks = []
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = f"RADAR_TRACK_{addr:x}"
signals += [
("STATE", msg),
("AZIMUTH", msg),
("LONG_DIST", msg),
("REL_ACCEL", msg),
("REL_SPEED", msg),
if (CP.flags & HyundaiFlags.SP_ENHANCED_SCC) and DBC[CP.carFingerprint]['radar'] is None:
msg = "ESCC"
signals = [
("ObjValid", msg),
("ACC_ObjStatus", msg),
("ACC_ObjLatPos", msg),
("ACC_ObjDist", msg),
("ACC_ObjRelSpd", msg),
]
checks += [(msg, 50)]
return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1)
checks = [
(msg, 50),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
else:
if DBC[CP.carFingerprint]['radar'] is None:
return None
signals = []
checks = []
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = f"RADAR_TRACK_{addr:x}"
signals += [
("STATE", msg),
("AZIMUTH", msg),
("LONG_DIST", msg),
("REL_ACCEL", msg),
("REL_SPEED", msg),
]
checks += [(msg, 50)]
return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.enhanced_scc = (CP.flags & HyundaiFlags.SP_ENHANCED_SCC) and DBC[CP.carFingerprint]['radar'] is None
self.updated_messages = set()
self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.trigger_msg = 0x2AB if self.enhanced_scc else RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
@@ -66,26 +81,46 @@ class RadarInterface(RadarInterfaceBase):
errors.append("canError")
ret.errors = errors
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]
if self.enhanced_scc:
msg = self.rcp.vl["ESCC"]
valid = msg['ACC_ObjStatus']
for ii in range(1):
if valid:
if ii not in self.pts:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].measured = True
self.pts[ii].dRel = msg['ACC_ObjDist']
self.pts[ii].yRel = -msg['ACC_ObjLatPos']
self.pts[ii].vRel = msg['ACC_ObjRelSpd']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
if addr not in self.pts:
self.pts[addr] = car.RadarData.RadarPoint.new_message()
self.pts[addr].trackId = self.track_id
self.track_id += 1
else:
if ii in self.pts:
del self.pts[ii]
else:
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]
valid = msg['STATE'] in (3, 4)
if valid:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[addr].measured = True
self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[addr].vRel = msg['REL_SPEED']
self.pts[addr].aRel = msg['REL_ACCEL']
self.pts[addr].yvRel = float('nan')
if addr not in self.pts:
self.pts[addr] = car.RadarData.RadarPoint.new_message()
self.pts[addr].trackId = self.track_id
self.track_id += 1
else:
del self.pts[addr]
valid = msg['STATE'] in (3, 4)
if valid:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[addr].measured = True
self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[addr].vRel = msg['REL_SPEED']
self.pts[addr].aRel = msg['REL_ACCEL']
self.pts[addr].yvRel = float('nan')
else:
del self.pts[addr]
ret.points = list(self.pts.values())
return ret
selfdrive/car/hyundai/values.py
+1
View File
@@ -62,6 +62,7 @@ class HyundaiFlags(IntFlag):
CANFD_ALT_GEARS_2 = 64
SEND_LFA = 128
USE_FCA = 256
SP_ENHANCED_SCC = 512
SP_CAN_LFA_BTN = 128