From 9a83eb772c333d7969ccbcf2fd0b4bbdfeef6f36 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 15 Oct 2022 18:57:20 -0400 Subject: [PATCH] Forward modified FCA11 and FCA12 messages --- selfdrive/car/hyundai/carcontroller.py | 4 +-- selfdrive/car/hyundai/carstate.py | 23 +++++++++++++- selfdrive/car/hyundai/hyundaican.py | 43 ++++++++++++++++---------- 3 files changed, 51 insertions(+), 19 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 773cf9817e..50f531474c 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -162,7 +162,7 @@ class CarController: if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: # 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), + can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), CS.fca11, self.CP.carFingerprint, hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override)) # 20 Hz LFA MFA message @@ -174,7 +174,7 @@ 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, CS.fca12, self.CP.carFingerprint)) # 2 Hz front radar options if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 5da1dd72c8..22f4c89111 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -143,6 +143,8 @@ class CarState(CarStateBase): # save the entire LKAS11 and CLU11 self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) self.clu11 = copy.copy(cp.vl["CLU11"]) + self.fca11 = copy.copy(cp_cruise.vl["FCA11"]) + self.fca12 = copy.copy(cp_cruise.vl["FCA12"]) self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED self.prev_cruise_buttons = self.cruise_buttons[-1] @@ -390,11 +392,30 @@ class CarState(CarStateBase): if CP.carFingerprint in FEATURES["use_fca"]: signals += [ + ("CF_VSM_Prefill", "FCA11"), + ("CF_VSM_HBACmd", "FCA11"), + ("CF_VSM_BeltCmd", "FCA11"), + ("CR_VSM_DecCmd", "FCA11"), + ("FCA_Status", "FCA11"), + ("FCA_StopReq", "FCA11"), + ("FCA_DrvSetStatus", "FCA11"), + ("FCA_Failinfo", "FCA11"), + ("CR_FCA_Alive", "FCA11"), + ("FCA_RelativeVelocity", "FCA11"), + ("FCA_TimetoCollision", "FCA11"), + ("CR_FCA_ChkSum", "FCA11"), + ("PAINT1_Status", "FCA11"), ("FCA_CmdAct", "FCA11"), ("CF_VSM_Warn", "FCA11"), ("CF_VSM_DecCmdAct", "FCA11"), + + ("FCA_USM", "FCA12"), + ("FCA_DrvSetState", "FCA12"), + ] + checks += [ + ("FCA11", 50), + ("FCA12", 50), ] - checks.append(("FCA11", 50)) else: signals += [ ("AEB_CmdAct", "SCC12"), diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index dcb8430976..494899cda2 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -95,7 +95,7 @@ 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, fca11, car_fingerprint, lead_visible, set_speed, stopping, long_override): commands = [] scc11_values = { @@ -133,21 +133,27 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s } commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) - # note that some vehicles most likely have an alternate checksum/counter definition - # 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 - } - fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] - fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7]) + if car_fingerprint in CAMERA_SCC_CAR: + fca11_values = fca11 + fca11_values["PAINT1_Status"] = 1 + fca11_values["FCA_DrvSetStatus"] = 1 + fca11_values["FCA_Status"] = 1 # AEB disabled, until a route with AEB or FCW trigger is verified + else: + # note that some vehicles most likely have an alternate checksum/counter definition + # 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 + } + fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] + fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7]) commands.append(packer.make_can_msg("FCA11", 0, fca11_values)) return commands -def create_acc_opt(packer): +def create_acc_opt(packer, fca12, car_fingerprint): commands = [] scc13_values = { @@ -157,10 +163,15 @@ 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 - } + if car_fingerprint in CAMERA_SCC_CAR: + fca12_values = 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