commaai
/
opendbc
mirror of https://github.com/maweiyuweiwei/opendbc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ford: fix unnecessary radar point creation and deletion (#1426) 

* don't create and delete capnp RadarPoint objects, and don't double increment track id

* clean up
This commit is contained in:
Shane Smiskol 2024-10-28 19:55:43 -05:00 committed by GitHub
parent 1c0b1da8f0
commit ba353f7d8a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
opendbc/car/ford/radar_interface.py
View File

@ -130,13 +130,6 @@ class RadarInterface(RadarInterfaceBase):
if scanIndex != headerScanIndex:
continue
if i not in self.pts:
self.pts[i] = structs.RadarData.RadarPoint()
self.pts[i].trackId = self.track_id
self.pts[i].aRel = float('nan')
self.pts[i].yvRel = float('nan')
self.track_id += 1
valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
# Long range measurement mode is more sensitive and can detect the road surface
@ -150,9 +143,16 @@ class RadarInterface(RadarInterfaceBase):
dRel = cos(azimuth) * dist # m from front of car
yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive
if i not in self.pts:
self.pts[i] = structs.RadarData.RadarPoint()
self.pts[i].trackId = self.track_id
self.pts[i].aRel = float('nan')
self.pts[i].yvRel = float('nan')
self.track_id += 1
elif abs(self.pts[i].vRel - distRate) > 2 or abs(self.pts[i].dRel - dRel) > 5:
# delphi doesn't notify of track switches, so do it manually
# TODO: refactor this to radard if more radars behave this way
if abs(self.pts[i].vRel - distRate) > 2 or abs(self.pts[i].dRel - dRel) > 5:
self.pts[i].trackId = self.track_id
self.track_id += 1
@ -163,6 +163,7 @@ class RadarInterface(RadarInterfaceBase):
self.pts[i].measured = True
else:
if i in self.pts:
del self.pts[i]
return errors