From b7bb78723b5b10cc859b44ba3c2517824bfc5ea2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 4 Dec 2023 03:14:39 +0000 Subject: [PATCH] Hyundai CAN Longitudinal: Enable radar tracks for certain Sante Fe platforms --- CHANGELOGS.md | 5 +++ selfdrive/car/hyundai/enable_radar_tracks.py | 47 ++++++++++++++++++++ selfdrive/car/hyundai/interface.py | 11 +++++ selfdrive/car/hyundai/radar_interface.py | 4 +- selfdrive/car/hyundai/values.py | 7 +-- 5 files changed, 70 insertions(+), 4 deletions(-) create mode 100644 selfdrive/car/hyundai/enable_radar_tracks.py diff --git a/CHANGELOGS.md b/CHANGELOGS.md index f5a6ec32cb..8b115ef7aa 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -1,5 +1,10 @@ sunnypilot - 0.9.5.2 (2023-xx-xx) ======================== +* NEW❗: Hyundai CAN longitudinal: + * NEW❗: Enable radar tracks for certain Santa Fe platforms + * Internal Combustion Engine (ICE) 2021-23 + * Hybrid 2022-23 + * Plug-in Hybrid 2022-23 * NEW❗: Lane Change: When manually braking with steering engaged, turning on the turn signal will default to Nudge mode sunnypilot - 0.9.5.1 (2023-11-17) diff --git a/selfdrive/car/hyundai/enable_radar_tracks.py b/selfdrive/car/hyundai/enable_radar_tracks.py new file mode 100644 index 0000000000..3f7dccd69e --- /dev/null +++ b/selfdrive/car/hyundai/enable_radar_tracks.py @@ -0,0 +1,47 @@ +from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery +from system.swaglog import cloudlog + +EXT_DIAG_REQUEST = b'\x10\x07' +EXT_DIAG_RESPONSE = b'\x50\x07' + +WRITE_DATA_REQUEST = b'\x2e' +WRITE_DATA_RESPONSE = b'\x68' + +RADAR_TRACKS_CONFIG = b"\x00\x00\x00\x01\x00\x01" + + +def enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42', timeout=0.1, retry=10, debug=False): + cloudlog.warning("radar_tracks: enabling ...") + + for i in range(retry): + try: + query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug) + + for _, _ in query.get_data(timeout).items(): + cloudlog.warning("radar_tracks: reconfigure radar to output radar points ...") + + query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], + [WRITE_DATA_REQUEST + config_data_id + RADAR_TRACKS_CONFIG], + [WRITE_DATA_RESPONSE], debug=debug) + query.get_data(0) + + cloudlog.warning("radar_tracks: successfully enabled") + return True + + except Exception as e: + cloudlog.exception(f"radar_tracks exception: {e}") + + cloudlog.error(f"radar_tracks retry ({i + 1}) ...") + cloudlog.error(f"radar_tracks: failed") + return False + + +if __name__ == "__main__": + import time + import cereal.messaging as messaging + sendcan = messaging.pub_sock('sendcan') + logcan = messaging.sub_sock('can') + time.sleep(1) + + enabled = enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42', timeout=0.1, debug=False) + print(f"enabled: {enabled}") diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5eb0bbbe8f..fb605844ac 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,6 +2,7 @@ from cereal import car from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params +from openpilot.selfdrive.car.hyundai.enable_radar_tracks import enable_radar_tracks from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \ CANFD_UNSUPPORTED_LONGITUDINAL_CAR, NON_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \ @@ -84,6 +85,10 @@ class CarInterface(CarInterfaceBase): # Values from optimizer ret.steerRatio = 16.55 # 13.8 is spec end-to-end ret.tireStiffnessFactor = 0.82 + if candidate in (CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): + if any(fw.ecu == "fwdRadar" and fw.fwVersion is not None for fw in car_fw): + ret.radarUnavailable = False + ret.spFlags |= HyundaiFlagsSP.SP_RADAR_TRACKS.value elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): ret.mass = 1513. ret.wheelbase = 2.84 @@ -371,6 +376,12 @@ class CarInterface(CarInterfaceBase): if CP.flags & HyundaiFlags.ENABLE_BLINKERS: disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01') + # for enabling radar tracks on startup + # some CAN platforms are able to enable radar tracks config at the radar ECU, + # but the config is reset after ignition cycle + if CP.openpilotLongitudinalControl and (CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS): + enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42') + def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) self.CS = self.sp_update_params(self.CS) diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 5d7782c524..76f2c370d6 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -39,6 +39,8 @@ class RadarInterface(RadarInterfaceBase): self.radar_off_can = CP.radarUnavailable self.rcp = get_radar_can_parser(CP) + self.sp_radar_tracks = CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS + def update(self, can_strings): if self.radar_off_can or (self.rcp is None): return super().update(None) @@ -46,7 +48,7 @@ class RadarInterface(RadarInterfaceBase): vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) - if self.trigger_msg not in self.updated_messages: + if self.trigger_msg not in self.updated_messages and not self.sp_radar_tracks: return None rr = self._update(self.updated_messages) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 62c584435d..388cf8fca7 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -73,6 +73,7 @@ class HyundaiFlagsSP(IntFlag): SP_NAV_MSG = 4 SP_CAMERA_SCC_LEAD = 8 SP_LKAS12 = 16 + SP_RADAR_TRACKS = 32 class CAR(StrEnum): @@ -2310,9 +2311,9 @@ DBC = { CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None), CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None), + CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), + CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), + CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format CAR.TUCSON: dbc_dict('hyundai_kia_generic', None),