diff --git a/opendbc b/opendbc index d8ad386e40..9d1e6f328d 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit d8ad386e402bf9022d6cb4cb1b9244b5cec28db5 +Subproject commit 9d1e6f328d186fa8ee945f7193f30ea7729ce80a diff --git a/release/files_common b/release/files_common index b0fff9a75c..72e31e5584 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 0f1166890e..f2ecbbe2cb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/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 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index fdbe837af6..4d2fef752e 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/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 = [ - # sig_name, sig_address, default - ("ACC_ObjStatus", "SCC11", 0), - ("ACC_ObjLatPos", "SCC11", 0), - ("ACC_ObjDist", "SCC11", 0), - ("ACC_ObjRelSpd", "SCC11", 0), - ] - checks = [ - # address, frequency - ("SCC11", 50), - ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + 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"R_{hex(addr)}" + signals += [ + ("STATE", msg, 0), + ("AZIMUTH", msg, 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'] - if valid: - for ii in range(2): - 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].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 - self.pts[ii].vRel = cpt["SCC11"]['ACC_ObjRelSpd'] - self.pts[ii].aRel = float('nan') - self.pts[ii].yvRel = float('nan') - self.pts[ii].measured = True + for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): + msg = self.rcp.vl[f"R_{hex(addr)}"] + + 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 + + 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 459722655e..1c97d91006 100644 --- a/selfdrive/car/hyundai/values.py +++ b/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_LF: dbc_dict('hyundai_kia_generic', None), - CAR.PALISADE: 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), # Has 0x5XX messages, but different format + 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), diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py new file mode 100755 index 0000000000..a5ef155cd6 --- /dev/null +++ b/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) diff --git a/tools/replay/unlog_segment.py b/tools/replay/unlog_ci_segment.py similarity index 90% rename from tools/replay/unlog_segment.py rename to tools/replay/unlog_ci_segment.py index 3f057c1913..5bceef875e 100755 --- a/tools/replay/unlog_segment.py +++ b/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") - fr = FrameReader(f"cd:/{route}/fcamera.hevc", readahead=True) + lr = LogReader(get_url(route, segment)) + 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)