fix

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

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