Hyundai radar parser (#22241)

* add radar parser for 0x5XX range

* fix speed

* enable based of fingerprint

* fix speed scale

* use proper signal names

* add to release files

* hyundai: script to enable radar points

* typo

* add comment

* fix tests

* cleanup

* add note about persistent

* Update selfdrive/car/hyundai/radar_interface.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* fix bugs in hyundai enable radar points script

* accidentally removed comment

* add some other cars

* add cosine to dRel

* fw versions do not match openpilot

* bump opendbc

* include state 4

Co-authored-by: Greg Hogan <gregjhogan@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

opendbc

@@ -1 +1 @@
-Subproject commit d8ad386e402bf9022d6cb4cb1b9244b5cec28db5
+Subproject commit 9d1e6f328d186fa8ee945f7193f30ea7729ce80a

release/files_common

@@ -572,6 +572,7 @@ opendbc/honda_insight_ex_2019_can_generated.dbc
 opendbc/acura_ilx_2016_nidec.dbc
 
 opendbc/hyundai_kia_generic.dbc
+opendbc/hyundai_kia_mando_front_radar.dbc
 
 opendbc/mazda_2017.dbc
 

selfdrive/car/hyundai/interface.py

@@ -4,6 +4,7 @@ from panda import Panda
 from common.params import Params
 from selfdrive.config import Conversions as CV
 from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
+from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.car.disable_ecu import disable_ecu
@@ -22,7 +23,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.carName = "hyundai"
     ret.safetyModel = car.CarParams.SafetyModel.hyundai
-    ret.radarOffCan = True
+    ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1]
 
     ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar") and candidate in [CAR.SONATA, CAR.PALISADE]
     ret.safetyParam = 0

selfdrive/car/hyundai/radar_interface.py

@@ -1,36 +1,47 @@
 #!/usr/bin/env python3
+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
 
+RADAR_START_ADDR = 0x500
+RADAR_MSG_COUNT = 32
+
 
 def get_radar_can_parser(CP):
-  signals = [
+  if DBC[CP.carFingerprint]['radar'] is None:
-    # sig_name, sig_address, default
+    return None
-    ("ACC_ObjStatus", "SCC11", 0),
+
-    ("ACC_ObjLatPos", "SCC11", 0),
+  signals = []
-    ("ACC_ObjDist", "SCC11", 0),
+  checks = []
-    ("ACC_ObjRelSpd", "SCC11", 0),
+
-  ]
+  for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
-  checks = [
+    msg = f"R_{hex(addr)}"
-    # address, frequency
+    signals += [
-    ("SCC11", 50),
+      ("STATE", msg, 0),
-  ]
+      ("AZIMUTH", msg, 0),
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+      ("LONG_DIST", msg, 0),
+      ("REL_ACCEL", msg, 0),
+      ("REL_SPEED", msg, 0),
+    ]
+    checks += [(msg, 50)]
+  return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1)
 
 
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
-    self.rcp = get_radar_can_parser(CP)
     self.updated_messages = set()
-    self.trigger_msg = 0x420
+    self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
     self.track_id = 0
+
     self.radar_off_can = CP.radarOffCan
+    self.rcp = get_radar_can_parser(CP)
 
   def update(self, can_strings):
-    if self.radar_off_can:
+    if self.radar_off_can or (self.rcp is None):
       return super().update(None)
 
     vls = self.rcp.update_strings(can_strings)
@@ -46,25 +57,35 @@ class RadarInterface(RadarInterfaceBase):
 
   def _update(self, updated_messages):
     ret = car.RadarData.new_message()
-    cpt = self.rcp.vl
+    if self.rcp is None:
+      return ret
+
     errors = []
+
     if not self.rcp.can_valid:
       errors.append("canError")
     ret.errors = errors
 
-    valid = cpt["SCC11"]['ACC_ObjStatus']
+    for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
-    if valid:
+      msg = self.rcp.vl[f"R_{hex(addr)}"]
-      for ii in range(2):
+
-        if ii not in self.pts:
+      if addr not in self.pts:
-          self.pts[ii] = car.RadarData.RadarPoint.new_message()
+        self.pts[addr] = car.RadarData.RadarPoint.new_message()
-          self.pts[ii].trackId = self.track_id
+        self.pts[addr].trackId = self.track_id
-          self.track_id += 1
+        self.track_id += 1
-        self.pts[ii].dRel = cpt["SCC11"]['ACC_ObjDist']  # from front of car
+
-        self.pts[ii].yRel = -cpt["SCC11"]['ACC_ObjLatPos']  # in car frame's y axis, left is negative
+      valid = msg['STATE'] in [3, 4]
-        self.pts[ii].vRel = cpt["SCC11"]['ACC_ObjRelSpd']
+      if valid:
-        self.pts[ii].aRel = float('nan')
+        azimuth = math.radians(msg['AZIMUTH'])
-        self.pts[ii].yvRel = float('nan')
+        self.pts[addr].measured = True
-        self.pts[ii].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

selfdrive/car/hyundai/values.py

@@ -737,6 +737,8 @@ FEATURES = {
 HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV])  # these cars use a different gas signal
 EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
 
+# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
+# If no points are outputted by default it might be possible to turn it on using  selfdrive/debug/hyundai_enable_radar_points.py
 DBC = {
   CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
   CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
@@ -748,24 +750,24 @@ DBC = {
   CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
   CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None),
   CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None),
-  CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None),
+  CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
   CAR.IONIQ: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None),
-  CAR.KIA_NIRO_HEV: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_NIRO_HEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
   CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
-  CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
   CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
   CAR.KONA: dbc_dict('hyundai_kia_generic', None),
   CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None),
   CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
   CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None),
-  CAR.SONATA: dbc_dict('hyundai_kia_generic', None),
+  CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
-  CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None),
+  CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
-  CAR.PALISADE: dbc_dict('hyundai_kia_generic', None),
+  CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
   CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None),
   CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', None),

selfdrive/debug/hyundai_enable_radar_points.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python3
+"""Some Hyundai radars can be reconfigured to output (debug) radar points on bus 1.
+Reconfiguration is done over UDS by reading/writing to 0x0142 using the Read/Write Data By Identifier
+endpoints (0x22 & 0x2E). This script checks your radar firmware version against a list of known 
+firmware versions. If you want to try on a new radar make sure to note the default config value
+in case it's different from the other radars and you need to revert the changes.
+
+After changing the config the car should not show any faults when openpilot is not running. 
+These config changes are persistent accross car reboots. You need to run this script again
+to go back to the default values.
+
+USE AT YOUR OWN RISK! Safety features, like AEB and FCW, might be affected by these changes."""
+
+import sys
+import argparse
+from subprocess import check_output, CalledProcessError
+
+from panda.python import Panda
+from panda.python.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
+
+# If your radar supports changing data identifier 0x0142 as well make a PR to
+# this file to add your firmware version. Make sure to post a drive as proof!
+# NOTE: these firmware versions do not match what openpilot uses
+#       because this script uses a different diagnostic session type
+SUPPORTED_FW_VERSIONS = {
+  # 2020 SONATA
+  b"DN8_ SCC FHCUP      1.00 1.00 99110-L0000\x19\x08)\x15T    ": {
+    "default_config": b"\x00\x00\x00\x01\x00\x00",
+    "tracks_enabled": b"\x00\x00\x00\x01\x00\x01",
+  },
+  # TODO: verify palisade fw version for diagnostic session type 7
+  # # 2020 PALISADE
+  # b"LX2_ SCC FHCUP      1.00 1.04 99110-S8100         ": {
+  #   "default_config": b"\x00\x00\x00\x01\x00\x00",
+  #   "tracks_enabled": b"\x00\x00\x00\x01\x00\x01",
+  # },
+}
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(description='configure radar to output points (or reset to default)')
+  parser.add_argument('--default', action="store_true", default=False, help='reset to default configuration (default: false)')
+  parser.add_argument('--debug', action="store_true", default=False, help='enable debug output (default: false)')
+  parser.add_argument('--bus', type=int, default=0, help='can bus to use (default: 0)')
+  args = parser.parse_args()
+
+  try:
+    check_output(["pidof", "boardd"])
+    print("boardd is running, please kill openpilot before running this script! (aborted)")
+    sys.exit(1)
+  except CalledProcessError as e:
+    if e.returncode != 1: # 1 == no process found (boardd not running)
+      raise e
+
+  confirm = input("put your vehicle in accessory mode now and type OK to continue: ").upper().strip()
+  if confirm != "OK":
+    print("\nyou didn't type 'OK! (aborted)")
+    sys.exit(0)
+
+  panda = Panda() # type: ignore
+  panda.set_safety_mode(Panda.SAFETY_ELM327)
+  uds_client = UdsClient(panda, 0x7D0, bus=args.bus, debug=args.debug)
+
+  print("\n[START DIAGNOSTIC SESSION]")
+  session_type : SESSION_TYPE = 0x07 # type: ignore
+  uds_client.diagnostic_session_control(session_type)
+
+  print("[HARDWARE/SOFTWARE VERSION]")
+  fw_version_data_id : DATA_IDENTIFIER_TYPE = 0xf100 # type: ignore
+  fw_version = uds_client.read_data_by_identifier(fw_version_data_id)
+  print(fw_version)
+  if fw_version not in SUPPORTED_FW_VERSIONS.keys():
+    print("radar not supported! (aborted)")
+    sys.exit(1)
+
+  print("[GET CONFIGURATION]")
+  config_data_id : DATA_IDENTIFIER_TYPE = 0x0142 # type: ignore
+  current_config = uds_client.read_data_by_identifier(config_data_id)
+  new_config = SUPPORTED_FW_VERSIONS[fw_version]["default_config" if args.default else "tracks_enabled"]
+  print(f"current config: 0x{current_config.hex()}")
+  if current_config != new_config:
+    print("[CHANGE CONFIGURATION]")
+    print(f"new config:     0x{new_config.hex()}")
+    uds_client.write_data_by_identifier(config_data_id, new_config)
+    if not args.default and current_config != SUPPORTED_FW_VERSIONS[fw_version]["default_config"]:
+      print("\ncurrent config does not match expected default! (aborted)")
+      sys.exit(1)
+
+    print("[DONE]")
+    print("\nrestart your vehicle and ensure there are no faults")
+    if not args.default:
+      print("you can run this script again with --default to go back to the original (factory) settings")
+  else:
+    print("[DONE]")
+    print("\ncurrent config is already the desired configuration")
+    sys.exit(0)

tools/replay/unlog_ci_segment.py

@@ -13,6 +13,7 @@ from collections import defaultdict
 import cereal.messaging as messaging
 from tools.lib.framereader import FrameReader
 from tools.lib.logreader import LogReader
+from selfdrive.test.openpilotci import get_url
 
 IGNORE = ['initData', 'sentinel']
 
@@ -21,11 +22,11 @@ def input_ready():
   return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], [])
 
 
-def replay(route, loop):
+def replay(route, segment, loop):
   route = route.replace('|', '/')
 
-  lr = LogReader(f"cd:/{route}/rlog.bz2")
+  lr = LogReader(get_url(route, segment))
-  fr = FrameReader(f"cd:/{route}/fcamera.hevc", readahead=True)
+  fr = FrameReader(get_url(route, segment, "fcamera"), readahead=True)
 
   # Build mapping from frameId to segmentId from roadEncodeIdx, type == fullHEVC
   msgs = [m for m in lr if m.which() not in IGNORE]
@@ -93,6 +94,7 @@ def replay(route, loop):
 if __name__ == "__main__":
   parser = argparse.ArgumentParser()
   parser.add_argument("--loop", action='store_true')
+  parser.add_argument("route")
   parser.add_argument("segment")
   args = parser.parse_args()
 
@@ -100,7 +102,7 @@ if __name__ == "__main__":
   tty.setcbreak(sys.stdin)
 
   try:
-    replay(args.segment, args.loop)
+    replay(args.route, args.segment, args.loop)
     termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
   except Exception:
     termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)