Restore Camry cluster radar parser

opendbc_repo/opendbc/car/toyota/cluster_radar.py

@@ -0,0 +1,59 @@
+"""Dom's six-cluster 10 Hz radar receiver for the 2018–20 Camry."""
+from opendbc.can import CANParser
+from opendbc.car import Bus
+from opendbc.car.interfaces import RadarInterfaceBase
+from opendbc.car.structs import RadarData
+from opendbc.car.toyota.values import DBC
+
+CLUSTERS = tuple(range(0x680, 0x686))
+EGO_SPEED_SCALE = 0.922
+
+
+class ClusterRadarInterface(RadarInterfaceBase):
+  def __init__(self, cp):
+    super().__init__(cp)
+    self.rcp = CANParser(DBC[cp.carFingerprint][Bus.radar], [(addr, 10) for addr in CLUSTERS], 1)
+    self.pt_cp = CANParser(DBC[cp.carFingerprint][Bus.pt], [('WHEEL_SPEEDS', 80)], 0)
+    self.updated_messages = set()
+    self.track_id = 0
+
+  def update(self, can_packets):
+    self.updated_messages.update(self.rcp.update(can_packets))
+    self.pt_cp.update(can_packets)
+    if CLUSTERS[-1] not in self.updated_messages:
+      return None
+    ret = RadarData()
+    if not self.pt_cp.can_valid:
+      self.updated_messages.clear()
+      ret.errors.canError = True
+      return ret
+    if not self.rcp.can_valid:
+      ret.errors.canError = True
+    wheel = self.pt_cp.vl['WHEEL_SPEEDS']
+    ego = sum(wheel['WHEEL_SPEED_' + corner] for corner in ('FL', 'FR', 'RL', 'RR')) / 4. / 3.6 * self.CP.wheelSpeedFactor
+    updated_ids = set()
+    for address in sorted(self.updated_messages):
+      if address not in CLUSTERS:
+        continue
+      cluster = self.rcp.vl[address]
+      track = int(cluster['ID'])
+      if track == 0x3F or cluster['LONG_DIST'] <= 0:
+        continue
+      updated_ids.add(track)
+      if track not in self.pts:
+        self.pts[track] = RadarData.RadarPoint()
+        self.pts[track].trackId = self.track_id
+        self.track_id += 1
+      point = self.pts[track]
+      point.dRel = float(cluster['LONG_DIST'])
+      point.yRel = -float(cluster['LAT_DIST'])
+      point.vRel = float(cluster['SPEED']) - ego * EGO_SPEED_SCALE
+      point.deprecated.aRel = float('nan')
+      point.deprecated.yvRel = float(cluster['LAT_SPEED'])
+      point.deprecated.measured = True
+    for track in list(self.pts):
+      if track not in updated_ids:
+        del self.pts[track]
+    self.updated_messages.clear()
+    ret.points = list(self.pts.values())
+    return ret

opendbc_repo/opendbc/car/toyota/interface.py

@@ -125,6 +125,9 @@ class CarInterface(CarInterfaceBase):
       ret.flags |= ToyotaFlags.HAS_BSM.value
 
     ret.radarUnavailable = Bus.radar not in DBC[candidate]
+    if candidate == CAR.TOYOTA_CAMRY:
+      ret.radarUnavailable = False
+      ret.deprecated.radarTimeStep = 0.1
 
     from opendbc.car.toyota.radar_filter import hardware_supported as radar_filter_supported
     radar_filter = radar_filter_supported(candidate, fingerprint)

opendbc_repo/opendbc/car/toyota/radar_interface.py

@@ -2,7 +2,7 @@
 from opendbc.can import CANParser
 from opendbc.car import Bus
 from opendbc.car.structs import RadarData
-from opendbc.car.toyota.values import DBC, ToyotaFlags, ARS510_RADAR_CAR
+from opendbc.car.toyota.values import CAR, DBC, ToyotaFlags, ARS510_RADAR_CAR
 from opendbc.car.toyota.ars510_acc_target import RadarInterface as Ars510AccRadarInterface
 from opendbc.car.toyota.ars510_radar import RadarInterface as Ars510FusedRadarInterface
 from opendbc.car.interfaces import RadarInterfaceBase
@@ -28,6 +28,12 @@ def _create_radar_can_parser(CP):
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
+    self.cluster = None
+    if CP.carFingerprint == CAR.TOYOTA_CAMRY and not CP.radarUnavailable:
+      from opendbc.car.toyota.cluster_radar import ClusterRadarInterface
+      self.cluster = ClusterRadarInterface(CP)
+      self.rcp = None
+      return
     self.ars510 = None
     if CP.carFingerprint in ARS510_RADAR_CAR and CP.flags & ToyotaFlags.ARS510_RADAR and not CP.radarUnavailable:
       parser = Ars510FusedRadarInterface if CP.flags & ToyotaFlags.ARS510_FUSED else Ars510AccRadarInterface
@@ -48,6 +54,8 @@ class RadarInterface(RadarInterfaceBase):
     self.updated_messages = set()
 
   def update(self, can_strings):
+    if self.cluster is not None:
+      return self.cluster.update(can_strings)
     if self.ars510 is not None:
       return self.ars510.update(can_strings)
     if self.rcp is None:

opendbc_repo/opendbc/car/toyota/values.py

@@ -182,7 +182,7 @@ class CAR(Platforms):
       ToyotaCarDocs("Toyota Camry Hybrid 2018-20", video="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
     ],
     CarSpecs(mass=3400. * CV.LB_TO_KG, wheelbase=2.82448, steerRatio=13.7, tireStiffnessFactor=0.7933),
-    {Bus.pt: 'toyota_nodsu_pt_generated'},
+    {Bus.pt: 'toyota_nodsu_pt_generated', Bus.radar: 'toyota_radar_dsu_tssp'},
     flags=ToyotaFlags.NO_DSU,
   )
   TOYOTA_CAMRY_TSS2 = ToyotaTSS2PlatformConfig( # TSS 2.5

openpilot/starpilot/car/toyota/tests/test_camry_cluster_callers.py

@@ -0,0 +1,37 @@
+"""Camry cluster radar under actual filter caller/native qualification."""
+from unittest.mock import patch
+import pytest
+from opendbc.can import CANPacker
+from opendbc.car.toyota.values import CAR
+from openpilot.starpilot.car.toyota.tests import test_rav4_angle_filter_callers as helpers
+
+source_matched_native=helpers.source_matched_native
+daemon_gc_scope=helpers.daemon_gc_scope
+
+
+@pytest.mark.parametrize('alpha,disable',[(False,False),(True,False),(True,True)])
+@pytest.mark.parametrize('aol',[False,True])
+def test_camry_cluster_actual_filter_callers(alpha,disable,aol,tmp_path):
+  physical=helpers.helpers.physical
+  publish=helpers.card_module.Car.radar_publish
+  packer=CANPacker('toyota_radar_dsu_tssp')
+  frames_count=0
+  observed=[]
+  def frames(*args,**kwargs):
+    nonlocal frames_count
+    frames_count+=1
+    output=physical(*args,**kwargs)
+    if frames_count%10==0:
+      for address in range(0x680,0x686):
+        output.append(packer.make_can_msg(address,1,{'ID':4 if address==0x680 else 63,
+          'LONG_DIST':30. if address==0x680 else 0.,'LAT_DIST':1.5,'SPEED':10.,'LAT_SPEED':.2}))
+    return output
+  def capture(host,rd):
+    publish(host,rd)
+    if rd is not None and not rd.errors.canError:
+      assert len(rd.points)==1
+      observed.append(rd.points[0].dRel)
+  with patch.object(helpers.helpers,'physical',side_effect=frames),patch.object(helpers.card_module.Car,'radar_publish',capture):
+    helpers.run_case(CAR.TOYOTA_CAMRY,alpha,disable,0,tmp_path,aol=aol)
+  assert len(observed)>=50
+  assert all(distance==pytest.approx(30.,abs=.03) for distance in observed)

openpilot/starpilot/car/toyota/tests/test_camry_cluster_radar.py

@@ -0,0 +1,93 @@
+"""Real CAN parser/Card publisher/RadarD path for Dom Camry clusters."""
+import pytest
+from unittest.mock import patch
+from opendbc.can import CANPacker
+from opendbc.car import gen_empty_fingerprint
+from opendbc.car.toyota.interface import CarInterface
+from opendbc.car.toyota.radar_interface import RadarInterface
+from opendbc.car.toyota.values import CAR
+from openpilot.cereal import messaging
+from openpilot.selfdrive.car.card import Car
+from openpilot.selfdrive.controls.radard import RadarD
+from openpilot.common.prefix import OpenpilotPrefix
+
+
+def setup():
+  cp=CarInterface.get_params(CAR.TOYOTA_CAMRY,gen_empty_fingerprint(),[],False,False,False)
+  assert not cp.radarUnavailable
+  return cp,RadarInterface(cp),CANPacker('toyota_radar_dsu_tssp'),CANPacker('toyota_nodsu_pt_generated')
+
+
+def batch(radar,pt,tick,*,target=True,wheels=True,bus=1,omit=None):
+  frames=[]
+  if wheels:
+    frames.append(pt.make_can_msg('WHEEL_SPEEDS',0,{f'WHEEL_SPEED_{c}':36. for c in ('FL','FR','RL','RR')}))
+  if tick%10==0:
+    for address in range(0x680,0x686):
+      if address==omit:
+        continue
+      fields={'ID':3 if target and address==0x680 else 63,'LONG_DIST':30. if target and address==0x680 else 0.,
+              'LAT_DIST':1.5,'SPEED':10.,'LAT_SPEED':.2}
+      frames.append(radar.make_can_msg(address,bus,fields))
+  return [(1_000_000_000+tick*10_000_000,frames)]
+
+
+def test_cluster_factory_can_card_and_radard(tmp_path):
+  with OpenpilotPrefix():
+    _check_card_and_radard()
+
+
+def _check_card_and_radard():
+  cp,ri,rp,pt=setup()
+  ci=CarInterface(cp)
+  host=Car(CI=ci,RI=ri)
+  published={}
+  capture=patch.object(host.pm,'send',side_effect=lambda n,m:published.update({n:m.as_reader()}))
+  capture.start()
+  try:
+    for tick in range(1,51):
+      result=host.radar_update(batch(rp,pt,tick))
+      host.radar_publish(result)
+    rd=published['radarTracks'].radarTracks
+    assert len(rd.points)==1 and not rd.errors.canError
+    point=rd.points[0]
+    assert point.dRel==pytest.approx(30.,abs=.03)
+    assert point.yRel==pytest.approx(-1.5,abs=.02)
+    assert point.vRel==pytest.approx(10.-10.*cp.wheelSpeedFactor*.922,abs=.08)
+    sm=messaging.SubMaster(['modelV2','carState'])
+    model=messaging.new_message('modelV2',valid=True)
+    model.modelV2.velocity.x=[10.]
+    car=messaging.new_message('carState',valid=True)
+    car.carState.vEgo=10.
+    sm.update_msgs(1.,[model,car])
+    fused=RadarD(radar_available=True)
+    fused.update(sm,rd)
+    assert len(fused.tracks)==1
+    assert next(iter(fused.tracks.values())).dRel==pytest.approx(point.dRel)
+    for tick in range(51,61):
+      host.radar_publish(host.radar_update(batch(rp,pt,tick,target=False)))
+    assert len(published['radarTracks'].radarTracks.points)==0
+  finally:
+    capture.stop()
+    host.vehicle_startup.close()
+
+
+@pytest.mark.parametrize('fault',['wheel_expiry','cluster_expiry','wrong_bus'])
+def test_cluster_source_faults(fault):
+  cp,ri,rp,pt=setup()
+  for tick in range(1,51):
+    rd=ri.update(batch(rp,pt,tick)) or locals().get('rd')
+  assert rd is not None and not rd.errors.canError
+  for tick in range(51,251):
+    result=ri.update(batch(rp,pt,tick,wheels=fault!='wheel_expiry',bus=2 if fault=='wrong_bus' else 1,
+                           omit=0x680 if fault=='cluster_expiry' else None))
+    if result is not None:
+      rd=result
+  if fault=='wrong_bus':
+    # No trigger can publish a new frame; the previous event remains old.
+    assert result is None
+  else:
+    assert rd.errors.canError
+  for tick in range(251,301):
+    rd=ri.update(batch(rp,pt,tick)) or rd
+  assert not rd.errors.canError

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "ebc12f616a72e4a946b56c2f83176afb0581750a0b8add23f9074badfa7fce43", "schema_version": 1}
+{"protected_inventory_sha256": "908f150c49255e1a0b0eb3318d185ce0202711e928db4583eba0ea4b02aa1391", "schema_version": 1}