From fec4d0346a4a67bf7eb1460586a28f6b722b60c6 Mon Sep 17 00:00:00 2001 From: Discountchubbs Date: Thu, 28 Nov 2024 10:07:11 -0800 Subject: [PATCH] radar tracks to staging :) --- cereal/car.capnp | 2 + common/params.cc | 3 + selfdrive/car/__init__.py | 11 +++ selfdrive/car/card.py | 47 +++++++++-- selfdrive/car/docs.py | 0 selfdrive/car/fingerprinting.py | 37 +++++++++ selfdrive/car/hyundai/carcontroller.py | 42 +++++++++- selfdrive/car/hyundai/carstate.py | 1 + selfdrive/car/hyundai/enable_radar_tracks.py | 47 +++++++++++ selfdrive/car/hyundai/hyundaican.py | 5 +- selfdrive/car/hyundai/hyundaicanfd.py | 16 +++- selfdrive/car/hyundai/interface.py | 46 ++++++++++- selfdrive/car/hyundai/radar_interface.py | 85 ++++++++++++++------ selfdrive/car/hyundai/values.py | 5 ++ selfdrive/car/interfaces.py | 11 ++- selfdrive/car/param_manager.py | 24 ++++++ selfdrive/controls/lib/events.py | 4 + selfdrive/controls/radard.py | 21 ++--- system/manager/manager.py | 0 19 files changed, 355 insertions(+), 52 deletions(-) mode change 100755 => 100644 selfdrive/car/card.py mode change 100755 => 100644 selfdrive/car/docs.py create mode 100644 selfdrive/car/fingerprinting.py create mode 100644 selfdrive/car/hyundai/enable_radar_tracks.py create mode 100644 selfdrive/car/param_manager.py mode change 100755 => 100644 selfdrive/controls/lib/events.py mode change 100755 => 100644 system/manager/manager.py diff --git a/cereal/car.capnp b/cereal/car.capnp index 62559a402b7817..0b23cb456940ac 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -117,6 +117,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { paramsdTemporaryError @50; paramsdPermanentError @119; actuatorsApiUnavailable @120; + hyundaiRadarTracksAvailable @150; # Use ID 150 instead of 142 # FrogPilot Events accel30 @121; @@ -488,6 +489,7 @@ struct CarParams { enableDsu @5 :Bool; # driving support unit enableBsm @56 :Bool; # blind spot monitoring flags @64 :UInt32; # flags for car specific quirks + fpFlags @76 :UInt32; # flags for car specific quirks that are fingerprint specific experimentalLongitudinalAvailable @71 :Bool; minEnableSpeed @7 :Float32; diff --git a/common/params.cc b/common/params.cc index 6d64a1d120ccfa..85388189e00a61 100644 --- a/common/params.cc +++ b/common/params.cc @@ -136,6 +136,9 @@ std::unordered_map keys = { {"GsmRoaming", PERSISTENT}, {"HardwareSerial", PERSISTENT}, {"HasAcceptedTerms", PERSISTENT}, + {"HyundaiRadarTracksAvailable", PERSISTENT}, + {"HyundaiRadarTracksAvailableCache", PERSISTENT}, + {"HyundaiRadarTracksAvailablePersistent", PERSISTENT}, {"IMEI", PERSISTENT}, {"InstallDate", PERSISTENT}, {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 307b5df0c3468e..836826bbed1edc 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -11,6 +11,7 @@ from openpilot.common.utils import Freezable from openpilot.selfdrive.car.docs_definitions import CarDocs +DT_CTRL = 0.01 # car state and control loop timestep (s) # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. @@ -272,6 +273,8 @@ class PlatformConfig(Freezable): flags: int = 0 + fpFlags: int = 0 + platform_str: str | None = None def __hash__(self) -> int: @@ -315,3 +318,11 @@ def create_dbc_map(cls) -> dict[str, DbcDict]: @classmethod def with_flags(cls, flags: IntFlag) -> set['Platforms']: return {p for p in cls if p.config.flags & flags} + + @classmethod + def with_fp_flags(cls, fpFlags: IntFlag) -> set['Platforms']: + return {p for p in cls if p.config.fpFlags & fpFlags} + + @classmethod + def without_fp_flags(cls, fpFlags: IntFlag) -> set['Platforms']: + return {p for p in cls if not (p.config.fpFlags & fpFlags)} diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py old mode 100755 new mode 100644 index 7e055f53c84dc4..836bf3dfb71f4f --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import os import time +import threading +from types import SimpleNamespace import cereal.messaging as messaging @@ -15,6 +17,7 @@ from openpilot.selfdrive.pandad import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_one_can from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.car.param_manager import ParamManager from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, update_frogpilot_toggles @@ -40,6 +43,7 @@ def __init__(self, CI=None): self.last_actuators_output = car.CarControl.Actuators.new_message() + self.frogpilot_toggles = get_frogpilot_toggles() self.params = Params() if CI is None: @@ -108,6 +112,10 @@ def __init__(self, CI=None): self.params.put_nonblocking("CarParamsCache", cp_bytes) self.params.put_nonblocking("CarParamsPersistent", cp_bytes) + self.param_manager: ParamManager = ParamManager() + self.param_manager.update(self.params) + self._params_list: SimpleNamespace = self.param_manager.get_params() + update_frogpilot_toggles() def state_update(self) -> car.CarState: @@ -115,7 +123,11 @@ def state_update(self) -> car.CarState: # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - CS, FPCS = self.CI.update(self.CC_prev, can_strs, self.frogpilot_toggles) + + if self.sm['frogpilotPlan'].togglesUpdated: + self.frogpilot_toggles = get_frogpilot_toggles() + + CS, FPCS = self.CI.update(self.CC_prev, can_strs, self._params_list, self.frogpilot_toggles) self.sm.update(0) @@ -206,14 +218,33 @@ def step(self): self.initialized_prev = initialized self.CS_prev = CS.as_reader() - def card_thread(self): - while True: - self.step() - self.rk.monitor_time() + def fp_params_thread(self, event: threading.Event) -> None: + while not event.is_set(): + self.param_manager.update(self.params) + self._params_list = self.param_manager.get_params() + time.sleep(0.1) - # Update FrogPilot parameters - if self.sm['frogpilotPlan'].togglesUpdated: - self.frogpilot_toggles = get_frogpilot_toggles() + def frogpilot_params_thread(self, event: threading.Event) -> None: + while not event.is_set(): + if self.sm['frogpilotPlan'].togglesUpdated: + self.frogpilot_toggles = get_frogpilot_toggles() + time.sleep(0.1) + + def card_thread(self): + event = threading.Event() + fp_thread = threading.Thread(target=self.fp_params_thread, args=(event,)) + frog_thread = threading.Thread(target=self.frogpilot_params_thread, args=(event,)) + + try: + fp_thread.start() + frog_thread.start() + while True: + self.step() + self.rk.monitor_time() + finally: + event.set() + fp_thread.join() + frog_thread.join() def main(): config_realtime_process(4, Priority.CTRL_HIGH) diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py old mode 100755 new mode 100644 diff --git a/selfdrive/car/fingerprinting.py b/selfdrive/car/fingerprinting.py new file mode 100644 index 00000000000000..3551067204695e --- /dev/null +++ b/selfdrive/car/fingerprinting.py @@ -0,0 +1,37 @@ +from collections.abc import Callable + +import cereal.messaging as messaging +from openpilot.selfdrive.car import gen_empty_fingerprint + +FRAME_FINGERPRINT = 25 # 0.25s + + +def get_one_can(logcan): + while True: + can = messaging.recv_one_retry(logcan) + if len(can.can) > 0: + return can + + +def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: + finger = gen_empty_fingerprint() + frame = 0 + done = False + + while not done: + a = next_can() + + for can in a.can: + # The fingerprint dict is generated for all buses, this way the car interface + # can use it to detect a (valid) multipanda setup and initialize accordingly + if can.src < 128: + if can.src not in finger: + finger[can.src] = {} + finger[can.src][can.address] = len(can.dat) + + # bail if we've been waiting for more than 2s + done = frame > 100 + + frame += 1 + + return finger diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index ff037c116e5c20..e309965ae61ea0 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,6 +1,8 @@ from cereal import car +import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance @@ -59,6 +61,15 @@ def __init__(self, dbc_name, CP, VM): self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 + sub_services = ['longitudinalPlan'] + if CP.openpilotLongitudinalControl: + sub_services.append('radarState') + + if sub_services: + self.sm = messaging.SubMaster(sub_services) + + self.param_s = Params() + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl @@ -108,6 +119,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN]) + # CAN-FD platforms if self.CP.carFingerprint in CANFD_CAR: hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2 @@ -133,8 +145,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): if hda2: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, - set_speed_in_units, hud_control)) + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units, hud_control)) self.accel_last = accel else: # button presses @@ -148,13 +159,14 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): if not self.CP.openpilotLongitudinalControl: can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True)) + 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 use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), hud_control, set_speed_in_units, stopping, - CC.cruiseControl.override, use_fca, CS.out.cruiseState.available)) + CC.cruiseControl.override, use_fca, CS, self.CP, CS.out.cruiseState.available)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: @@ -162,7 +174,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # 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, self.CP)) # 2 Hz front radar options if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl: @@ -211,3 +223,25 @@ def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11 self.last_button_frame = self.frame return can_sends + + def get_target_speed(self, v_cruise_kph_prev): + v_cruise_kph = v_cruise_kph_prev + if self.slc_state > 1: + v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH + if not self.slc_active_stock: + v_cruise_kph = v_cruise_kph_prev + return v_cruise_kph + + # jerk calculations thanks to apilot! + def cal_jerk(self, accel, actuators): + self.accel_raw = accel + if actuators.longControlState == LongCtrlState.off: + accel_diff = 0.0 + elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0: + accel_diff = 0.0 + else: + accel_diff = self.accel_raw - self.accel_last_jerk + + accel_diff /= DT_CTRL + self.jerk = self.jerk * 0.9 + accel_diff * 0.1 + return self.jerk diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 842074c83ad2f0..372d7445c34854 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -136,6 +136,7 @@ def update(self, cp, cp_cam, frogpilot_toggles): ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1 + ret.brakeLightsDEPRECATED = bool(cp.vl["TCS13"]["BrakeLight"]) ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): diff --git a/selfdrive/car/hyundai/enable_radar_tracks.py b/selfdrive/car/hyundai/enable_radar_tracks.py new file mode 100644 index 00000000000000..3a64d50cb94b36 --- /dev/null +++ b/selfdrive/car/hyundai/enable_radar_tracks.py @@ -0,0 +1,47 @@ +from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery +from openpilot.common.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/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 81ff568a6ea6fd..0ad36553233353 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -126,7 +126,7 @@ def create_lfahda_mfc(packer, enabled, lat_active, hda_set_speed=0): } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, cruise_available): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CS, CP, cruise_available): commands = [] scc11_values = { @@ -187,7 +187,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se return commands -def create_acc_opt(packer): +def create_acc_opt(packer, CS, CP): commands = [] scc13_values = { @@ -211,3 +211,4 @@ def create_frt_radar_opt(packer): "CF_FCA_Equip_Front_Radar": 1, } return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values) + diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index c26c42c02d45f9..1f8a88ed0aaf5b 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active): return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): +def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -171,6 +171,18 @@ def create_spas_messages(packer, CAN, frame, left_blink, right_blink): return ret +def create_fca_warning_light(packer, CAN, frame): + ret = [] + if frame % 2 == 0: + values = { + 'AEB_SETTING': 0x1, # show AEB disabled icon + 'SET_ME_2': 0x2, + 'SET_ME_FF': 0xff, + 'SET_ME_FC': 0xfc, + 'SET_ME_9': 0x9, + } + ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values)) + return ret def create_adrv_messages(packer, CAN, frame): # messages needed to car happy after disabling @@ -182,6 +194,8 @@ def create_adrv_messages(packer, CAN, frame): } ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values)) + ret.extend(create_fca_warning_light(packer, CAN, frame)) + if frame % 2 == 0: values = { 'AEB_SETTING': 0x1, # show AEB disabled icon diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 7ae1679bfd0e34..84587107342115 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,7 +1,11 @@ +import cereal.messaging as messaging from cereal import car, custom from panda import Panda +from openpilot.common.params import Params +from openpilot.selfdrive.car.fingerprinting import can_fingerprint, get_one_can +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, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \ +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsFP, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \ CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \ UNSUPPORTED_LONGITUDINAL_CAR, Buttons from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR @@ -108,9 +112,16 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.stoppingControl = True ret.startingState = True ret.vEgoStarting = 0.1 - ret.startAccel = 1.0 + ret.startAccel = 1.5 ret.longitudinalActuatorDelay = 0.5 + if ret.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): + ret.startingState = False + ret.stopAccel = -2.0 + else: + ret.startingState = True + ret.stopAccel = -1.0 + # *** feature detection *** if candidate in CANFD_CAR: ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN] @@ -123,6 +134,11 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp if 0x544 in fingerprint[0]: ret.flags |= HyundaiFlags.NAV_MSG.value + if ret.flags & HyundaiFlags.MANDO_RADAR and ret.radarUnavailable: + ret.fpFlags |= HyundaiFlagsFP.FP_RADAR_TRACKS.value + if Params().get_bool("HyundaiRadarTracksAvailable"): + ret.radarUnavailable = False + # *** panda safety config *** if candidate in CANFD_CAR: cfgs = [get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd), ] @@ -173,8 +189,9 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp @staticmethod def init(CP, logcan, sendcan): - if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value): - addr, bus = 0x7d0, 0 + if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and \ + CP.carFingerprint not in CAMERA_SCC_CAR: + addr, bus = 0x7d0, CanBus(CP).ECAN if CP.carFingerprint in CANFD_CAR else 0 if CP.flags & HyundaiFlags.CANFD_HDA2.value: addr, bus = 0x730, CanBus(CP).ECAN disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') @@ -183,6 +200,24 @@ def init(CP, logcan, sendcan): 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.fpFlags & HyundaiFlagsFP.FP_RADAR_TRACKS: + enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42') + + params = Params() + rt_avail = params.get_bool("HyundaiRadarTracksAvailable") + rt_avail_persist = params.get_bool("HyundaiRadarTracksAvailablePersistent") + params.put_bool_nonblocking("HyundaiRadarTracksAvailableCache", rt_avail) + if not rt_avail_persist: + messaging.drain_sock_raw(logcan) + fingerprint = can_fingerprint(lambda: get_one_can(logcan)) + radar_unavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[CP.carFingerprint]["radar"] is None + params.put_bool_nonblocking("HyundaiRadarTracksAvailable", not radar_unavailable) + params.put_bool_nonblocking("HyundaiRadarTracksAvailablePersistent", True) + + def _update(self, c, frogpilot_toggles): ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) @@ -207,6 +242,9 @@ def _update(self, c, frogpilot_toggles): if self.low_speed_alert: events.add(car.CarEvent.EventName.belowSteerSpeed) + if self.CS.params_list.hyundai_radar_tracks_available and not self.CS.params_list.hyundai_radar_tracks_available_cache: + events.add(car.CarEvent.EventName.hyundaiRadarTracksAvailable) + ret.events = events.to_msg() return ret, fp_ret diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 52600509860f5a..7b834ad984aaae 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -3,7 +3,8 @@ from cereal import car from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import RadarInterfaceBase -from openpilot.selfdrive.car.hyundai.values import DBC +from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus +from openpilot.selfdrive.car.hyundai.values import DBC, HyundaiFlags, HyundaiFlagsFP, CANFD_CAR RADAR_START_ADDR = 0x500 RADAR_MSG_COUNT = 32 @@ -12,7 +13,15 @@ def get_radar_can_parser(CP): if DBC[CP.carFingerprint]['radar'] is None: - return None + if CP.carFingerprint in CANFD_CAR: + if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC: + lead_src, bus = "SCC_CONTROL", CanBus(CP).CAM + else: + return None + else: + return None + messages = [(lead_src, 50)] + return CANParser(DBC[CP.carFingerprint]['pt'], messages, bus) messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)] return CANParser(DBC[CP.carFingerprint]['radar'], messages, 1) @@ -21,13 +30,17 @@ def get_radar_can_parser(CP): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) + self.CP = CP + self.camera_scc = CP.flags & HyundaiFlags.CANFD_CAMERA_SCC self.updated_messages = set() - self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1 + self.trigger_msg = 0x1A0 if self.camera_scc else (RADAR_START_ADDR + RADAR_MSG_COUNT - 1) self.track_id = 0 self.radar_off_can = CP.radarUnavailable self.rcp = get_radar_can_parser(CP) + self.fp_radar_tracks = CP.fpFlags & HyundaiFlagsFP.FP_RADAR_TRACKS + def update(self, can_strings): if self.radar_off_can or (self.rcp is None): return super().update(None) @@ -41,6 +54,11 @@ def update(self, can_strings): rr = self._update(self.updated_messages) self.updated_messages.clear() + radar_error = [] + if rr is not None: + radar_error = rr.errors + if list(radar_error) and self.fp_radar_tracks: + return super().update(None) return rr def _update(self, updated_messages): @@ -54,26 +72,47 @@ def _update(self, updated_messages): errors.append("canError") ret.errors = errors - for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): - msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"] - - 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] + if self.camera_scc: + msg_src = "SCC_CONTROL" + msg = self.rcp.vl[msg_src] + valid = msg['ACC_ObjDist'] < 204.6 + for ii in range(1): + if valid: + 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].measured = True + self.pts[ii].dRel = msg['ACC_ObjDist'] + self.pts[ii].yRel = float('nan') + self.pts[ii].vRel = msg['ACC_ObjRelSpd'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + + else: + if ii in self.pts: + del self.pts[ii] + else: + for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): + msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"] + + 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 df4079449344b0..147cbf16c7a700 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -100,6 +100,11 @@ class HyundaiFlags(IntFlag): LKAS12 = 2 ** 25 NAV_MSG = 2 ** 26 + +class HyundaiFlagsFP(IntFlag): + FP_RADAR_TRACKS = 2 ** 8 + + class Footnote(Enum): CANFD = CarFootnote( "Requires a CAN FD panda kit if not using " + diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 8ab60d786ecbed..082e97c60b23b1 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -5,6 +5,7 @@ from abc import abstractmethod, ABC from difflib import SequenceMatcher from enum import StrEnum +from types import SimpleNamespace from typing import Any, NamedTuple from collections.abc import Callable from functools import cache @@ -14,8 +15,10 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG +from openpilot.selfdrive.car.param_manager import ParamManager from openpilot.selfdrive.car.values import PLATFORMS from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS, V_CRUISE_MAX, get_friction from openpilot.selfdrive.controls.lib.events import Events @@ -223,6 +226,7 @@ def __init__(self, CP, CarController, CarState): dbc_name = "" if self.cp is None else self.cp.dbc_name self.CC: CarControllerBase = CarController(dbc_name, CP, self.VM) + self.param_s = Params() # FrogPilot variables self.frogpilot_toggles = get_frogpilot_toggles() @@ -286,6 +290,7 @@ def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_ ret.minSteerSpeed = platform.config.specs.minSteerSpeed ret.tireStiffnessFactor = platform.config.specs.tireStiffnessFactor ret.flags |= int(platform.config.flags) + ret.fpFlags |= int(platform.config.fpFlags) ret = cls._get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs) @@ -387,12 +392,14 @@ def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_ def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: list[bytes], frogpilot_toggles) -> car.CarState: + def update(self, c: car.CarControl, can_strings: list[bytes], params_list: SimpleNamespace, frogpilot_toggles) -> car.CarState: # parse can for cp in self.can_parsers: if cp is not None: cp.update_strings(can_strings) + self.CS.params_list = params_list + # get CarState ret, fp_ret = self._update(c, frogpilot_toggles) @@ -563,6 +570,8 @@ def __init__(self, CP): self.cluster_speed_hyst_gap = 0.0 self.cluster_min_speed = 0.0 # min speed before dropping to 0 + self.params_list: SimpleNamespace = ParamManager().get_params() + Q = [[0.0, 0.0], [0.0, 100.0]] R = 0.3 A = [[1.0, DT_CTRL], [0.0, 1.0]] diff --git a/selfdrive/car/param_manager.py b/selfdrive/car/param_manager.py new file mode 100644 index 00000000000000..744fbf451ae7ad --- /dev/null +++ b/selfdrive/car/param_manager.py @@ -0,0 +1,24 @@ +from types import SimpleNamespace + +from openpilot.common.params import Params + + +class ParamManager: + def __init__(self): + self._params_list: SimpleNamespace = self._create_namespace({ + "hyundai_radar_tracks_available": False, + "hyundai_radar_tracks_available_cache": False, + }) + + @staticmethod + def _create_namespace(data: dict) -> SimpleNamespace: + return SimpleNamespace(**data) + + def get_params(self) -> SimpleNamespace: + return self._params_list + + def update(self, params: Params) -> None: + self._params_list = self._create_namespace({ + "hyundai_radar_tracks_available": params.get_bool("HyundaiRadarTracksAvailable"), + "hyundai_radar_tracks_available_cache": params.get_bool("HyundaiRadarTracksAvailableCache"), + }) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py old mode 100755 new mode 100644 index 1b7523d7c17f1c..c296211fb73e18 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -901,6 +901,10 @@ def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM ET.NO_ENTRY: high_cpu_usage_alert, }, + EventName.hyundaiRadarTracksAvailable: { + ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize"), + }, + EventName.accFaulted: { ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"), ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index bdb986428abfa8..0b939c01d6d4e2 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -13,7 +13,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.simple_kalman import KF1D - +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles # Default lead acceleration decay set to 50% at 1s @@ -93,10 +93,11 @@ def reset_a_lead(self, aLeadK: float, aLeadTau: float): self.aLeadK = aLeadK self.aLeadTau = aLeadTau - def get_RadarState(self, model_prob: float = 0.0): + def get_RadarState(self, CP: car.CarParams = None, lead_msg_y: float = 0.0, model_prob: float = 0.0): + y_rel_vision = False if CP is None or CP.carName != "hyundai" else CP.flags & HyundaiFlags.CANFD_CAMERA_SCC return { "dRel": float(self.dRel), - "yRel": float(self.yRel), + "yRel": float(-lead_msg_y) if y_rel_vision else float(self.yRel), "vRel": float(self.vRel), "vLead": float(self.vLead), "vLeadK": float(self.vLeadK), @@ -182,7 +183,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, frogpilot_toggles: SimpleNamespace, frogpilotCarState: capnp._DynamicStructReader, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, CP: car.CarParams, frogpilot_toggles: SimpleNamespace, frogpilotCarState: capnp._DynamicStructReader, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > frogpilot_toggles.lead_detection_probability: track = match_vision_to_track(v_ego, lead_msg, tracks) @@ -191,7 +192,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = {'status': False} if track is not None: - lead_dict = track.get_RadarState(lead_msg.prob) + lead_dict = track.get_RadarState(CP, lead_msg.y[0], lead_msg.prob) elif (track is None) and ready and (lead_msg.prob > frogpilot_toggles.lead_detection_probability): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) @@ -222,7 +223,7 @@ def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStruct class RadarD: - def __init__(self, frogpilot_toggles, radar_ts: float, delay: int = 0): + def __init__(self, frogpilot_toggles, radar_ts: float, CP: car.CarParams, delay: int = 0): # type: ignore self.points: dict[int, tuple[float, float, float]] = {} self.current_time = 0.0 @@ -240,6 +241,8 @@ def __init__(self, frogpilot_toggles, radar_ts: float, delay: int = 0): self.ready = False + self.CP = CP + # FrogPilot variables self.frogpilot_toggles = frogpilot_toggles @@ -296,8 +299,8 @@ def update(self, sm: messaging.SubMaster, rr): model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.frogpilot_toggles, sm['frogpilotCarState'], low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles, sm['frogpilotCarState'], low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.frogpilot_toggles, sm['frogpilotCarState'], low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.frogpilot_toggles, sm['frogpilotCarState'], low_speed_override=False) if self.frogpilot_toggles.adjacent_lead_tracking and self.ready: self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True) @@ -380,7 +383,7 @@ def main(): # FrogPilot variables frogpilot_toggles = get_frogpilot_toggles() - RD = RadarD(frogpilot_toggles, CP.radarTimeStep, RI.delay) + RD = RadarD(frogpilot_toggles, CP.radarTimeStep, CP, RI.delay) if not frogpilot_toggles.radarless_model: sm = messaging.SubMaster(['modelV2', 'carState', 'frogpilotCarState', 'frogpilotPlan'], frequency=int(1./DT_CTRL)) diff --git a/system/manager/manager.py b/system/manager/manager.py old mode 100755 new mode 100644