From ba353f7d8af487563a505b90d3ee1448754f46ac Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 28 Oct 2024 19:55:43 -0500 Subject: [PATCH] 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 --- opendbc/car/ford/radar_interface.py | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py index 2178f6a3f6..c2972475b3 100644 --- a/opendbc/car/ford/radar_interface.py +++ b/opendbc/car/ford/radar_interface.py @@ -130,13 +130,6 @@ def _update_delphi_mrr(self): 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 @@ def _update_delphi_mrr(self): dRel = cos(azimuth) * dist # m from front of car yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive - # 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: + 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 self.pts[i].trackId = self.track_id self.track_id += 1 @@ -163,6 +163,7 @@ def _update_delphi_mrr(self): self.pts[i].measured = True else: - del self.pts[i] + if i in self.pts: + del self.pts[i] return errors