diff --git a/cereal/car.capnp b/cereal/car.capnp index 91b02834d36da5..1b319cba5aedbf 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -511,6 +511,7 @@ struct CarParams { carFw @44 :List(CarFw); radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard + radarDelay @74 :Float32; fingerprintSource @49: FingerprintSource; networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network diff --git a/cereal/log.capnp b/cereal/log.capnp index 079b62e20928cc..b715d14220a86a 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -611,7 +611,6 @@ struct RadarState @0x9a185389d6fdd05f { leadOne @3 :LeadData; leadTwo @4 :LeadData; - cumLagMs @5 :Float32; struct LeadData { dRel @0 :Float32; @@ -641,6 +640,7 @@ struct RadarState @0x9a185389d6fdd05f { calCycleDEPRECATED @8 :Int32; calPercDEPRECATED @9 :Int8; canMonoTimesDEPRECATED @10 :List(UInt64); + cumLagMsDEPRECATED @5 :Float32; } struct LiveCalibrationData { @@ -671,7 +671,7 @@ struct LiveCalibrationData { } } -struct LiveTracks { +struct LiveTracksDEPRECATED { trackId @0 :Int32; dRel @1 :Float32; yRel @2 :Float32; @@ -2335,7 +2335,7 @@ struct Event { pandaStates @81 :List(PandaState); peripheralState @80 :PeripheralState; radarState @13 :RadarState; - liveTracks @16 :List(LiveTracks); + liveTracks @131 :Car.RadarData; sendcan @17 :List(CanData); liveCalibration @19 :LiveCalibrationData; carState @22 :Car.CarState; @@ -2465,5 +2465,6 @@ struct Event { navModelDEPRECATED @104 :NavModelData; uiPlanDEPRECATED @106 :UiPlan; liveLocationKalmanDEPRECATED @72 :LiveLocationKalman; + liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED); } } diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 57b41a5cf5b087..541a7c44ae2361 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -16,8 +16,8 @@ from opendbc.car import DT_CTRL, carlog, structs from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable from opendbc.car.fw_versions import ObdCallback -from opendbc.car.car_helpers import get_car -from opendbc.car.interfaces import CarInterfaceBase +from opendbc.car.car_helpers import get_car, get_radar_interface +from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import CarSpecificEvents, MockCarState @@ -63,13 +63,14 @@ def can_send(msgs: list[CanData]) -> None: class Car: CI: CarInterfaceBase + RI: RadarInterfaceBase CP: structs.CarParams CP_capnp: car.CarParams - def __init__(self, CI=None) -> None: + def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks']) self.can_rcv_cum_timeout_counter = 0 @@ -101,12 +102,14 @@ def __init__(self, CI=None) -> None: cached_params = structs.CarParams(carName=_cached_params.carName, carFw=_cached_params.carFw, carVin=_cached_params.carVin) self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params) + self.RI = get_radar_interface(self.CI.CP) self.CP = self.CI.CP # continue onto next fingerprinting step in pandad self.params.put_bool("FirmwareQueryDone", True) else: self.CI, self.CP = CI, CI.CP + self.RI = RI # set alternative experiences from parameters self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") @@ -149,7 +152,7 @@ def __init__(self, CI=None) -> None: # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) - def state_update(self) -> car.CarState: + def state_update(self) -> tuple[car.CarState, structs.RadarData | None]: """carState update loop, driven by can""" # Update carState from CAN @@ -159,6 +162,9 @@ def state_update(self) -> car.CarState: if self.CP.carName == 'mock': CS = self.mock_carstate.update(CS) + # Update radar tracks from CAN + RD: structs.RadarData | None = self.RI.update(can_capnp_to_list(can_strs)) + self.sm.update(0) can_rcv_valid = len(can_strs) > 0 @@ -175,9 +181,9 @@ def state_update(self) -> car.CarState: CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) - return CS + return CS, RD - def update_events(self, CS: car.CarState): + def update_events(self, CS: car.CarState, RD: structs.RadarData | None): self.events.clear() CS.events = self.car_events.update(self.CI.CS, self.CS_prev, self.CI.CC, self.CC_prev).to_msg() @@ -196,9 +202,12 @@ def update_events(self, CS: car.CarState): (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): self.events.add(EventName.pedalPressed) + if RD is not None and len(RD.errors): + self.events.add(EventName.radarFault) + CS.events = self.events.to_msg() - def state_publish(self, CS: car.CarState): + def state_publish(self, CS: car.CarState, RD: structs.RadarData | None): """carState and carParams publish loop""" # carParams - logged every 50 seconds (> 1 per segment) @@ -222,6 +231,12 @@ def state_publish(self, CS: car.CarState): cs_send.carState.cumLagMs = -self.rk.remaining * 1000. self.pm.send('carState', cs_send) + if RD is not None: + tracks_msg = messaging.new_message('liveTracks') + tracks_msg.valid = CS.canValid and len(RD.errors) == 0 + tracks_msg.liveTracks = convert_to_capnp(RD) + self.pm.send('liveTracks', tracks_msg) + def controls_update(self, CS: car.CarState, CC: car.CarControl): """control update loop, driven by carControl""" @@ -241,14 +256,14 @@ def controls_update(self, CS: car.CarState, CC: car.CarControl): self.CC_prev = CC def step(self): - CS = self.state_update() + CS, RD = self.state_update() - self.update_events(CS) + self.update_events(CS, RD) if not self.sm['carControl'].enabled and self.events.contains(ET.ENABLE): self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) - self.state_publish(CS) + self.state_publish(CS, RD) initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index f4d899346847e1..75e1c66d24254a 100644 --- a/selfdrive/car/helpers.py +++ b/selfdrive/car/helpers.py @@ -35,7 +35,7 @@ def asdictref(obj) -> dict[str, Any]: return _asdictref_inner(obj) -def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarControl.Actuators) -> capnp.lib.capnp._DynamicStructBuilder: +def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarControl.Actuators | structs.RadarData) -> capnp.lib.capnp._DynamicStructBuilder: struct_dict = asdictref(struct) if isinstance(struct, structs.CarParams): @@ -51,6 +51,8 @@ def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarC struct_capnp = car.CarState.new_message(**struct_dict) elif isinstance(struct, structs.CarControl.Actuators): struct_capnp = car.CarControl.Actuators.new_message(**struct_dict) + elif isinstance(struct, structs.RadarData): + struct_capnp = car.RadarData.new_message(**struct_dict) else: raise ValueError(f"Unsupported struct type: {type(struct)}") diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index e8df0a8310f054..0548f52764d590 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -448,7 +448,7 @@ def test_panda_safety_carstate(self): checks['cruiseState'] += CS.cruiseState.enabled != self.safety.get_cruise_engaged_prev() else: # Check for enable events on rising edge of controls allowed - card.update_events(CS) + card.update_events(CS, None) card.CS_prev = CS button_enable = (any(evt.enable for evt in CS.events) and not any(evt == EventName.pedalPressed for evt in card.events.names)) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c6ecdea4c2eaa0..4a608e3f1d8294 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -96,7 +96,7 @@ def __init__(self, CI=None): 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets + self.sensor_packets + self.gps_packets, - ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], + ignore_alive=ignore, ignore_avg_freq=ignore+['testJoystick'], ignore_valid=['testJoystick', ], frequency=int(1/DT_CTRL)) self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -303,7 +303,7 @@ def update_events(self, CS): self.events.add(EventName.cameraFrameRate) if not REPLAY and self.rk.lagging: self.events.add(EventName.controlsdLagging) - if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])): + if not self.sm.valid['radarState']: self.events.add(EventName.radarFault) if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f3aa099bfbf6ba..f577025fa42dcd 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -1,18 +1,15 @@ #!/usr/bin/env python3 -import importlib import math from collections import deque from typing import Any import capnp from cereal import messaging, log, car -from opendbc.car import structs from openpilot.common.numpy_fast import interp from openpilot.common.params import Params -from openpilot.common.realtime import DT_CTRL, Ratekeeper, Priority, config_realtime_process +from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process from openpilot.common.swaglog import cloudlog from openpilot.common.simple_kalman import KF1D -from openpilot.selfdrive.pandad import can_capnp_to_list # Default lead acceleration decay set to 50% at 1s @@ -194,14 +191,14 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn class RadarD: - def __init__(self, radar_ts: float, delay: int = 0): + def __init__(self, delay: float = 0.0): self.current_time = 0.0 self.tracks: dict[int, Track] = {} - self.kalman_params = KalmanParams(radar_ts) + self.kalman_params = KalmanParams(DT_MDL) self.v_ego = 0.0 - self.v_ego_hist = deque([0.0], maxlen=delay+1) + self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1) self.last_v_ego_frame = -1 self.radar_state: capnp._DynamicStructBuilder | None = None @@ -209,7 +206,7 @@ def __init__(self, radar_ts: float, delay: int = 0): self.ready = False - def update(self, sm: messaging.SubMaster, rr: structs.RadarData): + def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -255,27 +252,14 @@ def update(self, sm: messaging.SubMaster, rr: structs.RadarData): self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) - def publish(self, pm: messaging.PubMaster, lag_ms: float): + def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None radar_msg = messaging.new_message("radarState") radar_msg.valid = self.radar_state_valid radar_msg.radarState = self.radar_state - radar_msg.radarState.cumLagMs = lag_ms pm.send("radarState", radar_msg) - # publish tracks for UI debugging (keep last) - tracks_msg = messaging.new_message('liveTracks', len(self.tracks)) - tracks_msg.valid = self.radar_state_valid - for index, tid in enumerate(sorted(self.tracks.keys())): - tracks_msg.liveTracks[index] = { - "trackId": tid, - "dRel": float(self.tracks[tid].dRel), - "yRel": float(self.tracks[tid].yRel), - "vRel": float(self.tracks[tid].vRel), - } - pm.send('liveTracks', tracks_msg) - # fuses camera and radar data for best lead detection def main() -> None: @@ -286,31 +270,17 @@ def main() -> None: CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams) cloudlog.info("radard got CarParams") - # import the radar from the fingerprint - cloudlog.info("radard is importing %s", CP.carName) - RadarInterface = importlib.import_module(f'opendbc.car.{CP.carName}.radar_interface').RadarInterface - # *** setup messaging - can_sock = messaging.sub_sock('can') - sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL)) - pm = messaging.PubMaster(['radarState', 'liveTracks']) + sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2') + pm = messaging.PubMaster(['radarState']) - RI = RadarInterface(CP) - - rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) - RD = RadarD(CP.radarTimeStep, RI.delay) + RD = RadarD(CP.radarDelay) while 1: - can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) - rr: structs.RadarData | None = RI.update(can_capnp_to_list(can_strings)) - sm.update(0) - if rr is None: - continue - - RD.update(sm, rr) - RD.publish(pm, -rk.remaining*1000.0) + sm.update() - rk.monitor_time() + RD.update(sm, sm['liveTracks']) + RD.publish(pm) if __name__ == "__main__": diff --git a/selfdrive/controls/tests/test_leads.py b/selfdrive/controls/tests/test_leads.py index 4b98628bf6e847..89582d1e647f8b 100644 --- a/selfdrive/controls/tests/test_leads.py +++ b/selfdrive/controls/tests/test_leads.py @@ -11,19 +11,21 @@ def test_radar_fault(self): def single_iter_pkg(): # single iter package, with meaningless cans and empty carState/modelV2 msgs = [] - for _ in range(5): + for _ in range(500): can = messaging.new_message("can", 1) cs = messaging.new_message("carState") + cp = messaging.new_message("carParams") msgs.append(can.as_reader()) msgs.append(cs.as_reader()) + msgs.append(cp.as_reader()) model = messaging.new_message("modelV2") msgs.append(model.as_reader()) return msgs msgs = [m for _ in range(3) for m in single_iter_pkg()] - out = replay_process_with_name("radard", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2) - states = [m for m in out if m.which() == "radarState"] - failures = [not state.valid and len(state.radarState.radarErrors) for state in states] + out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2) + states = [m for m in out if m.which() == "liveTracks"] + failures = [not state.valid and len(state.liveTracks.errors) for state in states] assert len(states) == 0 or all(failures) diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 07979a06cc7812..723a9ff5b34159 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -1,6 +1,6 @@ from collections import defaultdict -from cereal import messaging +from cereal import messaging, car from opendbc.car.fingerprints import MIGRATION from opendbc.car.toyota.values import EPS_SCALE from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index @@ -17,6 +17,7 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals msgs = migrate_carOutput(msgs) msgs = migrate_controlsState(msgs) msgs = migrate_liveLocationKalman(msgs) + msgs = migrate_liveTracks(msgs) if manager_states: msgs = migrate_managerState(msgs) if panda_states: @@ -28,6 +29,35 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals return msgs +def migrate_liveTracks(lr): + all_msgs = [] + for msg in lr: + if msg.which() != "liveTracksDEPRECATED": + all_msgs.append(msg) + continue + + new_msg = messaging.new_message('liveTracks') + new_msg.valid = msg.valid + new_msg.logMonoTime = msg.logMonoTime + + pts = [] + for track in msg.liveTracksDEPRECATED: + pt = car.RadarData.RadarPoint() + pt.trackId = track.trackId + + pt.dRel = track.dRel + pt.yRel = track.yRel + pt.vRel = track.vRel + pt.aRel = track.aRel + pt.measured = True + pts.append(pt) + + new_msg.liveTracks.points = pts + all_msgs.append(new_msg.as_reader()) + + return all_msgs + + def migrate_liveLocationKalman(lr): # migration needed only for routes before livePose if any(msg.which() == 'livePose' for msg in lr): diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 3a933fefc9153f..78008ca90986bb 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -480,7 +480,7 @@ def controlsd_config_callback(params, cfg, lr): ProcessConfig( proc_name="card", pubs=["pandaStates", "carControl", "onroadEvents", "can"], - subs=["sendcan", "carState", "carParams", "carOutput"], + subs=["sendcan", "carState", "carParams", "carOutput", "liveTracks"], ignore=["logMonoTime", "carState.cumLagMs"], init_callback=card_fingerprint_callback, should_recv_callback=card_rcv_callback, @@ -490,12 +490,11 @@ def controlsd_config_callback(params, cfg, lr): ), ProcessConfig( proc_name="radard", - pubs=["can", "carState", "modelV2"], - subs=["radarState", "liveTracks"], - ignore=["logMonoTime", "radarState.cumLagMs"], + pubs=["liveTracks", "carState", "modelV2"], + subs=["radarState"], + ignore=["logMonoTime"], init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("can"), - main_pub="can", + should_recv_callback=FrequencyBasedRcvCallback("modelV2"), ), ProcessConfig( proc_name="plannerd", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 90d6c85cb94604..1e3ef33c7e7bc5 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fb0827a00b21b97733f8385cf7ce87a21a526f4c \ No newline at end of file +2b842a259e15f4b8679f16a14de405b5df06ae1f \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 9dd781e89da86c..b3e66444a7ac12 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -36,7 +36,7 @@ PROCS = { # Baseline CPU usage by process "selfdrive.controls.controlsd": 32.0, - "selfdrive.car.card": 26.0, + "selfdrive.car.card": 31.0, "./loggerd": 14.0, "./encoderd": 17.0, "./camerad": 14.5, @@ -44,7 +44,7 @@ "./ui": 18.0, "selfdrive.locationd.paramsd": 9.0, "./sensord": 7.0, - "selfdrive.controls.radard": 7.0, + "selfdrive.controls.radard": 2.0, "selfdrive.modeld.modeld": 13.0, "selfdrive.modeld.dmonitoringmodeld": 8.0, "system.hardware.hardwared": 3.87,