stash so far

commaai · Sep 4, 2024 · 5aa2c84 · 5aa2c84
1 parent b5f6d0c
commit 5aa2c84
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 12 deletions.
diff --git a/opendbc_repo b/opendbc_repo
diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py
@@ -22,6 +22,7 @@
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.car_specific import CarSpecificEvents, MockCarState
 from openpilot.selfdrive.car.helpers import convert_carControl, convert_to_capnp
+from openpilot.selfdrive.controls.radard import Track
 from openpilot.selfdrive.controls.lib.events import Events, ET
 
 REPLAY = "REPLAY" in os.environ
@@ -70,7 +71,7 @@ class Car:
   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
 
@@ -144,6 +145,7 @@ def __init__(self, CI=None, RI=None) -> None:
     self.car_events = CarSpecificEvents(self.CP)
     self.mock_carstate = MockCarState()
     self.v_cruise_helper = VCruiseHelper(self.CP)
+    self.tracks: dict[int, Track] = {}
 
     self.is_metric = self.params.get_bool("IsMetric")
     self.experimental_mode = self.params.get_bool("ExperimentalMode")
@@ -161,6 +163,9 @@ def state_update(self) -> car.CarState:
     if self.CP.carName == 'mock':
       CS = self.mock_carstate.update(CS)
 
+    # Update radar tracks from CAN
+    rr: structs.RadarData | None = self.RI.update(can_capnp_to_list(can_strs))
+
     self.sm.update(0)
 
     can_rcv_valid = len(can_strs) > 0

diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import importlib
 import math
 from collections import deque
 from typing import Any
@@ -286,19 +285,14 @@ 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'])
-
-  RI = RadarInterface(CP)
+  sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], frequency=int(1./DT_CTRL))
+  pm = messaging.PubMaster(['radarState'])
 
   rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
-  RD = RadarD(CP.radarTimeStep, RI.delay)
+  delay = 0  # TODO: add delay to params
+  RD = RadarD(CP.radarTimeStep, delay)
 
   while 1:
     can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)