From df87f451903b609aa577377af1bd5de12f34a754 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 19 Feb 2024 17:36:55 +0000 Subject: [PATCH] ESCC: Forward FCA11 and FCA12 for all stock FCW/AEB commands --- CHANGELOGS.md | 2 ++ selfdrive/car/hyundai/carstate.py | 18 ++++++----- selfdrive/car/hyundai/hyundaican.py | 48 +++++++++++++---------------- 3 files changed, 35 insertions(+), 33 deletions(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 281e708cc7..ddae7b8066 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -28,6 +28,8 @@ sunnypilot - 0.9.6.0 (2024-xx-xx) * UPDATED: Custom Offsets * Continued support for Legacy Driving Models (e.g., ND, BDv2, BDv1, FV, NS) * Deprecate support for newer Driving Models (e.g., CHv2, CH, LAv2, LAv1) +* UPDATED: Hyundai/Kia/Genesis - ESCC Radar Interceptor + * Message parsing improvements with the latest firmware update: https://github.com/sunnypilot/panda/tree/test-escc-smdps * UI Updates * NEW❗: Visuals: Display Feature Status toggle * Display the statuses of certain features on the driving screen diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 262c31789a..3782367592 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -169,19 +169,20 @@ class CarState(CarStateBase): ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking ret.stockAeb = aeb_warning and aeb_braking elif self.CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC: - aeb_src = "ESCC" + aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA else "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_warning_sig = "CF_VSM_Warn" if self.CP.flags & HyundaiFlags.USE_FCA.value else "CF_VSM_Warn_SCC12" + aeb_braking_sig = "CF_VSM_DecCmdAct" 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 not self.CP.flags & HyundaiFlags.USE_FCA: + 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 @@ -374,6 +375,9 @@ class CarState(CarStateBase): if CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC.value: messages.append(("ESCC", 50)) + if CP.flags & HyundaiFlags.USE_FCA.value and not any([msg[0] == "FCA11" for msg in messages]): + messages.append(("FCA11", 50)) + if CP.spFlags & HyundaiFlagsSP.SP_NAV_MSG: messages.append(("Navi_HU", 5)) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 91229478d5..2a624d6099 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -152,18 +152,18 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_distance, "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, } # show AEB disabled indicator on dash with SCC12 if not sending FCA messages. # these signals also prevent a TCS fault on non-FCA cars with alpha longitudinal if not use_fca: scc12_values["CF_VSM_ConfMode"] = 1 - scc12_values["AEB_Status"] = 1 # AEB disabled + scc12_values["AEB_Status"] = 2 if escc else 1 # AEB disabled + if escc: + scc12_values["AEB_CmdAct"] = CS.escc_cmd_act + scc12_values["CF_VSM_Warn"] = CS.escc_aeb_warning + scc12_values["CF_VSM_DecCmdAct"] = CS.escc_aeb_dec_cmd_act + scc12_values["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 @@ -181,7 +181,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_distance, commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) # Only send FCA11 on cars where it exists on the bus - if use_fca: + if use_fca and not escc: if car_fingerprint in CAMERA_SCC_CAR: fca11_values = CS.fca11 fca11_values["PAINT1_Status"] = 1 @@ -192,14 +192,9 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_distance, # https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602 fca11_values = { "CR_FCA_Alive": idx % 0xF, - "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, + "PAINT1_Status": 1, + "FCA_DrvSetStatus": 1, + "FCA_Status": 1, # AEB disabled } fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7]) @@ -217,17 +212,18 @@ def create_acc_opt(packer, escc, CS, car_fingerprint): } commands.append(packer.make_can_msg("SCC13", 0, scc13_values)) - # TODO: this needs to be detected and conditionally sent on unsupported long cars - if car_fingerprint in CAMERA_SCC_CAR: - fca12_values = CS.fca12 - fca12_values["FCA_DrvSetState"] = 2 - fca12_values["FCA_USM"] = 1 # AEB disabled, until a route with AEB or FCW trigger is verified - else: - fca12_values = { - "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)) + if not escc: + # TODO: this needs to be detected and conditionally sent on unsupported long cars + if car_fingerprint in CAMERA_SCC_CAR: + fca12_values = CS.fca12 + fca12_values["FCA_DrvSetState"] = 2 + fca12_values["FCA_USM"] = 1 # AEB disabled, until a route with AEB or FCW trigger is verified + else: + fca12_values = { + "FCA_DrvSetState": 2, + "FCA_USM": 1, # AEB disabled + } + commands.append(packer.make_can_msg("FCA12", 0, fca12_values)) return commands