Hyundai: Longitudinal support for CAN-based Camera SCC cars (commaai#139

)

* Hyundai: Longitudinal support for CAN-based Camera SCC cars

* gate FCA12 send to CAMERA_SCC_CARS for now

* unused

* sync with upstream PR

* Update CHANGELOGS.md

* Update CHANGELOGS.md

* separate from upstream

* bump panda

CHANGELOGS.md

@@ -11,6 +11,7 @@ sunnypilot - 0.9.2.3 (2023-06-xx)
 * UPDATED: Green Traffic Light Chime no longer displays an orange border when executed
 * FIXED: Ram HD (2500/3500): Fixed/Constant steering ratio
   * Live parameters have trouble with self-tuning the steering ratio on this platform with upstream openpilot 0.9.2
+* Hyundai: Longitudinal support for CAN-based Camera SCC cars thanks to Zack1010OP's Patreon sponsor!
 
 sunnypilot - 0.9.2.2 (2023-06-13)
 ========================

selfdrive/car/hyundai/carcontroller.py

@@ -8,7 +8,7 @@
 from selfdrive.car import apply_driver_steer_torque_limits
 from selfdrive.car.hyundai import hyundaicanfd, hyundaican
 from selfdrive.car.hyundai.hyundaicanfd import CanBus
-from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
+from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR
 from selfdrive.controls.lib.drive_helpers import HYUNDAI_V_CRUISE_MIN
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -167,7 +167,8 @@ def update(self, CC, CS, now_nanos):
     # *** 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) or escc) and self.CP.openpilotLongitudinalControl:
+    if self.frame % 100 == 0 and not ((self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or escc) and \
+      self.CP.carFingerprint not in CAMERA_SCC_CAR 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:
@@ -288,15 +289,15 @@ def update(self, CC, CS, now_nanos):
         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,
-                                                        CS, escc))
+                                                        CS, escc, self.CP.carFingerprint))
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
         can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, CC.latActive, lateral_paused, blinking_icon))
 
       # 5 Hz ACC options
       if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
-        can_sends.extend(hyundaican.create_acc_opt(self.packer, escc))
+        can_sends.extend(hyundaican.create_acc_opt(self.packer, escc, CS, self.CP.carFingerprint))
 
       # 2 Hz front radar options
       if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl and not escc:

selfdrive/car/hyundai/carstate.py

@@ -178,6 +178,10 @@ def update(self, cp, cp_cam):
     # save the entire LKAS11 and CLU11
     self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
     self.clu11 = copy.copy(cp.vl["CLU11"])
+    # only forward FCA messages for FCW/AEB when using openpilot longitudinal on Camera SCC cars
+    if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint in CAMERA_SCC_CAR:
+      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.prev_cruise_buttons = self.cruise_buttons[-1]
     self.prev_main_buttons = self.main_buttons[-1]
@@ -517,6 +521,34 @@ def get_cam_can_parser(CP):
           ("CF_VSM_DecCmdAct", "SCC12"),
         ]
 
+    if CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR:
+      if CP.flags & HyundaiFlags.USE_FCA.value:
+        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),
+        ]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
 
   @staticmethod

selfdrive/car/hyundai/hyundaican.py

@@ -100,7 +100,7 @@ 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,
-                        CS, escc):
+                        CS, escc, car_fingerprint):
   commands = []
 
   scc11_values = {
@@ -145,24 +145,30 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
 
   # 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": 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,
-  }
+  if car_fingerprint in CAMERA_SCC_CAR:
+    fca11_values = CS.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:
+    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,
+    }
   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, escc):
+def create_acc_opt(packer, escc, CS, car_fingerprint):
   commands = []
 
   scc13_values = {
@@ -172,10 +178,15 @@ def create_acc_opt(packer, escc):
   }
   commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
 
-  fca12_values = {
-    "FCA_DrvSetState": 0 if escc else 2,
-    "FCA_USM": 0 if escc else 1, # AEB disabled
-  }
+  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))
 
   return commands

selfdrive/car/hyundai/interface.py

@@ -248,7 +248,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     else:
       ret.longitudinalTuning.kpV = [0.5]
       ret.longitudinalTuning.kiV = [0.0]
-      ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
+      ret.experimentalLongitudinalAvailable = candidate not in LEGACY_SAFETY_MODE_CAR
     ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
     ret.pcmCruise = not ret.openpilotLongitudinalControl
 
@@ -330,7 +330,8 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
 
   @staticmethod
   def init(CP, logcan, sendcan):
-    if CP.openpilotLongitudinalControl and not ((CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or (CP.flags & HyundaiFlags.SP_ENHANCED_SCC)):
+    if CP.openpilotLongitudinalControl and not ((CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) or (CP.flags & HyundaiFlags.SP_ENHANCED_SCC)) and \
+      CP.carFingerprint not in CAMERA_SCC_CAR:
       addr, bus = 0x7d0, 0
       if CP.flags & HyundaiFlags.CANFD_HDA2.value:
         addr, bus = 0x730, CanBus(CP).ECAN