From 32d50a650429d1194f5296d6e08677c8fe928a8b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 17 Feb 2023 18:31:10 -0500 Subject: [PATCH 1/2] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 54e270ec08..b65212a33e 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 54e270ec083d4059cd733500f6d0aebdb76a70ca +Subproject commit b65212a33e9ec4c66436e929e2d9f0b929568c4b From 2830c020f69d0e7f8270655775cd0ff306605444 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Fri, 17 Feb 2023 18:33:14 -0500 Subject: [PATCH 2/2] Hyundai: Enhanced SCC (ESCC) Radar Interceptor support (#114) * escc: wip * finish up * use new intflag * bump panda --- selfdrive/car/hyundai/carcontroller.py | 11 ++- selfdrive/car/hyundai/carstate.py | 36 ++++++++ selfdrive/car/hyundai/hyundaican.py | 25 ++++-- selfdrive/car/hyundai/interface.py | 8 +- selfdrive/car/hyundai/radar_interface.py | 105 +++++++++++++++-------- selfdrive/car/hyundai/values.py | 1 + 6 files changed, 139 insertions(+), 47 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 1e6f78af20..1c60c85182 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -76,12 +76,14 @@ class CarController: sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint, hud_control) + 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: @@ -176,7 +178,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, accel, jerk, int(self.frame / 2), - hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override)) + hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, + CS, escc)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: @@ -184,10 +187,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() diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 685261a988..255ccd8e8a 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -45,6 +45,11 @@ class CarState(CarStateBase): self.params = CarControllerParams(CP) + 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: return self.update_canfd(cp, cp_cam) @@ -142,6 +147,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 @@ -368,6 +387,23 @@ class CarState(CarStateBase): signals.append(("CF_Lvr_Gear", "LVR12")) checks.append(("LVR12", 100)) + 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 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 858f3d0876..889aed4d32 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -96,7 +96,8 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0): } 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): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, + CS, escc): commands = [] scc11_values = { @@ -118,6 +119,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 @@ -138,9 +144,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]) @@ -148,7 +159,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 = { @@ -159,8 +170,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)) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index d2cc5b4ec0..e8985fde51 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -52,6 +52,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. @@ -278,6 +281,9 @@ class CarInterface(CarInterfaceBase): if candidate in CAMERA_SCC_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + 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: @@ -299,7 +305,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 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 4ecca542b5..55c07502e3 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -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 diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index d0a4e4dd1a..c990c8c0f8 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -62,6 +62,7 @@ class HyundaiFlags(IntFlag): CANFD_ALT_GEARS_2 = 64 SEND_LFA = 128 USE_FCA = 256 + SP_ENHANCED_SCC = 512 class CAR: