121 lines
3.2 KiB
Python
121 lines
3.2 KiB
Python
import math
|
|
|
|
from iqdbc.can import CANParser
|
|
from iqdbc.can.dbc import DBC as DBCLoader
|
|
from iqdbc.car import Bus, structs
|
|
from iqdbc.car.interfaces import RadarInterfaceBase
|
|
from iqdbc.car.common.conversions import Conversions as CV
|
|
from iqdbc.car.volkswagen.values import DBC, VolkswagenFlags
|
|
|
|
RADAR_ADDR = 0x24F
|
|
NO_OBJECT = 0
|
|
LANE_TYPES = ("Same_Lane", "Left_Lane", "Right_Lane")
|
|
SIGNAL_SETS = tuple(
|
|
(
|
|
f"{prefix}_ObjectID",
|
|
f"{prefix}_Long_Distance",
|
|
f"{prefix}_Lat_Distance",
|
|
f"{prefix}_Rel_Velo",
|
|
)
|
|
for lane in LANE_TYPES
|
|
for idx in (1, 2)
|
|
for prefix in (f"{lane}_0{idx}",)
|
|
)
|
|
|
|
|
|
def get_radar_can_parser(CP):
|
|
if CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
dbc_name = DBC[CP.carFingerprint][Bus.radar]
|
|
dbc = DBCLoader(dbc_name)
|
|
if "Strukturen_01" in dbc.name_to_msg:
|
|
messages = [("Strukturen_01", 25)]
|
|
elif "MEB_Distance_01" in dbc.name_to_msg:
|
|
messages = [("MEB_Distance_01", 25)]
|
|
else:
|
|
return None
|
|
else:
|
|
return None
|
|
|
|
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 2)
|
|
|
|
|
|
class RadarInterface(RadarInterfaceBase):
|
|
def __init__(self, CP, CP_IQ):
|
|
super().__init__(CP, CP_IQ)
|
|
|
|
self.updated_messages: set[int] = set()
|
|
self.trigger_msg: int = RADAR_ADDR
|
|
self._track_id_counter: int = 0
|
|
|
|
self.radar_off_can: bool = CP.radarUnavailable
|
|
self.rcp: CANParser | None = get_radar_can_parser(CP)
|
|
|
|
self._pts = self.pts
|
|
|
|
def update(self, can_strings):
|
|
"""Entry-point called by the vehicle loop every CAN tick."""
|
|
if self.radar_off_can or self.rcp is None:
|
|
return super().update(None)
|
|
|
|
vls = self.rcp.update(can_strings)
|
|
self.updated_messages.update(vls)
|
|
|
|
if self.trigger_msg not in self.updated_messages:
|
|
return None
|
|
|
|
radar_data = self._process_radar_frame()
|
|
self.updated_messages.clear()
|
|
return radar_data
|
|
|
|
def _process_radar_frame(self):
|
|
ret = structs.RadarData()
|
|
|
|
if self.rcp is None:
|
|
return ret
|
|
|
|
if not self.rcp.can_valid:
|
|
ret.errors.canError = True
|
|
return ret
|
|
|
|
msg = self.rcp.vl["Strukturen_01"] if "Strukturen_01" in self.rcp.vl else self.rcp.vl["MEB_Distance_01"]
|
|
get = msg.__getitem__
|
|
|
|
active_objects: dict[int, tuple[float, float, float]] = {}
|
|
for obj_id_sig, long_sig, lat_sig, vel_sig in SIGNAL_SETS:
|
|
obj_id = get(obj_id_sig)
|
|
if obj_id == NO_OBJECT:
|
|
continue
|
|
|
|
if obj_id in active_objects:
|
|
ret.errors.canError = True
|
|
return ret
|
|
|
|
active_objects[obj_id] = (
|
|
get(long_sig), # dRel
|
|
get(lat_sig), # yRel
|
|
get(vel_sig), # vRel
|
|
)
|
|
|
|
for obj_id, (d_rel, y_rel, v_rel) in active_objects.items():
|
|
if obj_id not in self._pts:
|
|
pt = structs.RadarData.RadarPoint()
|
|
pt.trackId = self._track_id_counter
|
|
self._track_id_counter += 1
|
|
self._pts[obj_id] = pt
|
|
else:
|
|
pt = self._pts[obj_id]
|
|
|
|
pt.measured = True
|
|
pt.dRel = d_rel
|
|
pt.yRel = y_rel
|
|
pt.vRel = v_rel
|
|
pt.aRel = math.nan
|
|
pt.yvRel = math.nan
|
|
|
|
inactive_ids = self._pts.keys() - active_objects.keys()
|
|
for obj_id in inactive_ids:
|
|
self._pts.pop(obj_id, None)
|
|
|
|
ret.points = list(self._pts.values())
|
|
return ret
|