diff --git a/panda b/panda index be1d9dd5bb..fe1ffdc07b 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit be1d9dd5bb497e7d1199ce0f47135edda6d70312 +Subproject commit fe1ffdc07be6afdbf47d3d6c5dd2210ecd752766 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 022090c4af..efa8d47681 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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() diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index d2de747ea0..d5546a8c2b 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -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 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 5203672ceb..7045cbf105 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -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)) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6042aa49d2..bd8bea79c0 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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 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 ef72e592d2..a277b84444 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 SP_CAN_LFA_BTN = 128