diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 5cec3bf855..06638db6a5 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -85,20 +85,24 @@ class ControlsExt(ModelStateBase): _lead.radar = src.radar _lead.radarTrackId = src.radarTrackId + @staticmethod + def get_radar_track_data(CC_SP: custom.CarControlSP, live_tracks, valid: bool) -> None: + CC_SP.radarTracksActive = valid and len(live_tracks.trackSources) > 0 + source_tracks = live_tracks.points if CC_SP.radarTracksActive else () + radar_tracks = CC_SP.init('radarTracks', len(source_tracks)) + for dst, src in zip(radar_tracks, source_tracks, strict=True): + dst.trackId = src.trackId + dst.dRel = src.dRel + dst.yRel = src.yRel + dst.vRel = src.vRel + def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP: CC_SP = custom.CarControlSP.new_message() self.get_lead_data(CC_SP.leadOne, sm['radarState'].leadOne) self.get_lead_data(CC_SP.leadTwo, sm['radarState'].leadTwo) - live_tracks = sm['liveTracks'] - CC_SP.radarTracksActive = sm.valid['liveTracks'] and len(live_tracks.trackSources) > 0 - radar_tracks = CC_SP.init('radarTracks', len(live_tracks.points) if CC_SP.radarTracksActive else 0) - for dst, src in zip(radar_tracks, live_tracks.points, strict=True): - dst.trackId = src.trackId - dst.dRel = src.dRel - dst.yRel = src.yRel - dst.vRel = src.vRel + self.get_radar_track_data(CC_SP, sm['liveTracks'], sm.valid['liveTracks']) # MADS state mads_src = sm['selfdriveStateSP'].mads diff --git a/sunnypilot/selfdrive/controls/test_controlsd_ext.py b/sunnypilot/selfdrive/controls/test_controlsd_ext.py new file mode 100644 index 0000000000..f1b27a9bcf --- /dev/null +++ b/sunnypilot/selfdrive/controls/test_controlsd_ext.py @@ -0,0 +1,34 @@ +from cereal import car, custom + +from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt + + +def make_live_tracks(with_source: bool): + live_tracks = car.RadarData.new_message() + point = live_tracks.init("points", 1)[0] + point.trackId = 7 + point.dRel = 12 + point.yRel = -3 + point.vRel = 1 + if with_source: + live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 1}] + return live_tracks + + +def test_inactive_radar_tracks_ignores_stale_points(): + cc_sp = custom.CarControlSP.new_message() + + ControlsExt.get_radar_track_data(cc_sp, make_live_tracks(with_source=False), valid=True) + + assert not cc_sp.radarTracksActive + assert len(cc_sp.radarTracks) == 0 + + +def test_active_radar_tracks_are_copied(): + cc_sp = custom.CarControlSP.new_message() + + ControlsExt.get_radar_track_data(cc_sp, make_live_tracks(with_source=True), valid=True) + + assert cc_sp.radarTracksActive + assert len(cc_sp.radarTracks) == 1 + assert cc_sp.radarTracks[0].trackId == 7