diff --git a/opendbc_repo/opendbc/car/toyota/cluster_radar.py b/opendbc_repo/opendbc/car/toyota/cluster_radar.py new file mode 100644 index 0000000000..10ea035af6 --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 966197d11f..47471d7aa5 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/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) diff --git a/opendbc_repo/opendbc/car/toyota/radar_interface.py b/opendbc_repo/opendbc/car/toyota/radar_interface.py index 233e1ce6ef..84d1c4f4cc 100755 --- a/opendbc_repo/opendbc/car/toyota/radar_interface.py +++ b/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: diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 1901257651..d43049be31 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/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 diff --git a/openpilot/starpilot/car/toyota/tests/test_camry_cluster_callers.py b/openpilot/starpilot/car/toyota/tests/test_camry_cluster_callers.py new file mode 100644 index 0000000000..118e02f052 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/toyota/tests/test_camry_cluster_radar.py b/openpilot/starpilot/car/toyota/tests/test_camry_cluster_radar.py new file mode 100644 index 0000000000..f06942e1af --- /dev/null +++ b/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 diff --git a/prebuilt.json b/prebuilt.json index 9e274712a6..dcd14cb5e7 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "ebc12f616a72e4a946b56c2f83176afb0581750a0b8add23f9074badfa7fce43", "schema_version": 1} +{"protected_inventory_sha256": "908f150c49255e1a0b0eb3318d185ce0202711e928db4583eba0ea4b02aa1391", "schema_version": 1}