From 986e75dea0dd568e3dc89c669453ce42383bc89c Mon Sep 17 00:00:00 2001 From: ryley Date: Tue, 26 Apr 2022 18:30:18 -0500 Subject: [PATCH] try to fix parser error --- opendbc/mazda_2017.dbc | 4 +- opendbc/mazda_radar.dbc | 56 +++++++-------------- selfdrive/car/mazda/mazdacan.py | 6 +-- selfdrive/car/mazda/radar_interface.py | 67 ++++++++++++-------------- 4 files changed, 54 insertions(+), 79 deletions(-) diff --git a/opendbc/mazda_2017.dbc b/opendbc/mazda_2017.dbc index 12255328c..b47052c2f 100644 --- a/opendbc/mazda_2017.dbc +++ b/opendbc/mazda_2017.dbc @@ -541,7 +541,7 @@ BO_ 865 RADAR_361: 8 XXX SG_ MSGS_2 : 39|28@0+ (1,0) [0|268435455] "" XXX SG_ CTR : 59|4@0+ (1,0) [0|15] "" XXX -BO_ 870 RADAR_366: 8 XXX +BO_ 866 RADAR_362: 8 XXX SG_ MSGS_1 : 7|32@0+ (1,0) [0|4294967295] "" XXX SG_ MSGS_2 : 39|28@0+ (1,0) [0|268435455] "" XXX SG_ CTR : 59|4@0+ (1,0) [0|15] "" XXX @@ -561,7 +561,7 @@ BO_ 869 RADAR_365: 8 XXX SG_ MSGS_2 : 39|28@0+ (1,0) [0|268435455] "" XXX SG_ CTR : 59|4@0+ (1,0) [0|15] "" XXX -BO_ 866 RADAR_362: 8 XXX +BO_ 870 RADAR_366: 8 XXX SG_ MSGS_1 : 7|32@0+ (1,0) [0|4294967295] "" XXX SG_ MSGS_2 : 39|28@0+ (1,0) [0|268435455] "" XXX SG_ CTR : 59|4@0+ (1,0) [0|15] "" XXX diff --git a/opendbc/mazda_radar.dbc b/opendbc/mazda_radar.dbc index 584c6f66c..9b25f3867 100644 --- a/opendbc/mazda_radar.dbc +++ b/opendbc/mazda_radar.dbc @@ -63,53 +63,33 @@ BO_ 539 CRZ_INFO: 8 XXX SG_ CHECKSUM : 56|8@1+ (1,0) [0|255] "" XXX BO_ 865 RADAR_TRACK_361: 8 XXX - SG_ DIST_OBJ : 7|12@0+ (1,0) [0|31] "" XXX - SG_ ANG_OBJ : 11|12@0- (1,0) [0|255] "" XXX - SG_ RELV_OBJ : 31|11@0- (1,0) [0|255] "" XXX - SG_ NEW_SIGNAL_1 : 36|9@0+ (1,0) [0|3] "" XXX - SG_ SPEED_INVERSE : 43|12@0- (-0.225,0) [0|255] "kph" XXX - SG_ NEW_SIGNAL_2 : 63|4@0+ (1,0) [0|15] "" XXX - SG_ CTR : 56|4@1+ (1,0) [0|15] "" XXX + SG_ DIST_OBJ : 7|12@0+ (1,0) [0|4095] "" XXX + SG_ ANG_OBJ : 11|12@0- (1,0) [-2047|2047] "" XXX + SG_ RELV_OBJ : 31|11@0- (1,0) [-1023|1023] "" XXX BO_ 866 RADAR_TRACK_362: 8 XXX - SG_ DIST_OBJ : 7|12@0+ (1,0) [0|255] "" XXX - SG_ RELV_OBJ : 31|11@0- (1,0) [0|63] "" XXX - SG_ NEW_SIGNAL_1 : 36|9@0+ (1,0) [0|3] "" XXX - SG_ STEER_ANGLE : 43|12@0+ (-1,2048) [0|7] "" XXX - SG_ NEW_SIGNAL_2 : 63|4@0+ (1,0) [0|15] "" XXX - SG_ CTR : 59|4@0+ (1,0) [0|15] "" XXX - SG_ ANG_OBJ : 11|12@0- (1,0) [0|7] "" XXX + SG_ DIST_OBJ : 7|12@0+ (1,0) [0|4095] "" XXX + SG_ ANG_OBJ : 11|12@0- (1,0) [-2047|2047] "" XXX + SG_ RELV_OBJ : 31|11@0- (1,0) [-1023|1023] "" XXX BO_ 867 RADAR_TRACK_363: 8 XXX - SG_ CTR : 59|4@0+ (1,0) [0|255] "" XXX - SG_ NEW_SIGNAL_1 : 36|13@0+ (1,0) [0|7] "" XXX - SG_ NEW_SIGNAL_2 : 55|12@0+ (1,0) [0|7] "" XXX - SG_ DIST_OBJ : 7|12@0+ (1,0) [0|131071] "" XXX - SG_ ANG_OBJ : 11|12@0- (1,0) [0|127] "" XXX - SG_ RELV_OBJ : 31|11@0- (1,0) [0|63] "" XXX + SG_ DIST_OBJ : 7|12@0+ (1,0) [0|4095] "" XXX + SG_ ANG_OBJ : 11|12@0- (1,0) [-2047|2047] "" XXX + SG_ RELV_OBJ : 31|11@0- (1,0) [-1023|1023] "" XXX BO_ 868 RADAR_TRACK_364: 8 XXX - SG_ DIST_OBJ : 7|12@0+ (1,0) [0|131071] "" XXX - SG_ ANG_OBJ : 11|12@0- (1,0) [0|127] "" XXX - SG_ RELV_OBJ : 31|11@0- (1,0) [0|63] "" XXX - SG_ NEW_SIGNAL_1 : 36|13@0+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_2 : 55|12@0+ (1,0) [0|7] "" XXX - SG_ CTR : 59|4@0+ (1,0) [0|255] "" XXX + SG_ DIST_OBJ : 7|12@0+ (1,0) [0|4095] "" XXX + SG_ ANG_OBJ : 11|12@0- (1,0) [-2047|2047] "" XXX + SG_ RELV_OBJ : 31|11@0- (1,0) [-1023|1023] "" XXX BO_ 869 RADAR_TRACK_365: 8 XXX - SG_ DIST_OBJ : 7|12@0+ (1,0) [0|131071] "" XXX - SG_ ANG_OBJ : 11|12@0- (1,0) [0|127] "" XXX - SG_ RELV_OBJ : 31|11@0- (1,0) [0|63] "" XXX - SG_ NEW_SIGNAL_1 : 36|13@0+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_2 : 55|12@0+ (1,0) [0|7] "" XXX - SG_ CTR : 59|4@0+ (1,0) [0|255] "" XXX + SG_ DIST_OBJ : 7|12@0+ (1,0) [0|4095] "" XXX + SG_ ANG_OBJ : 11|12@0- (1,0) [-2047|2047] "" XXX + SG_ RELV_OBJ : 31|11@0- (1,0) [-1023|1023] "" XXX BO_ 870 RADAR_TRACK_366: 8 XXX - SG_ DIST_OBJ : 7|12@0+ (1,0) [0|131071] "" XXX - SG_ ANG_OBJ : 11|12@0- (1,0) [0|127] "" XXX - SG_ RELV_OBJ : 31|11@0- (1,0) [0|63] "" XXX - SG_ NEW_SIGNAL_1 : 36|13@0+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_2 : 55|12@0+ (1,0) [0|7] "" XXX - SG_ CTR : 59|4@0+ (1,0) [0|255] "" XXX + SG_ DIST_OBJ : 7|12@0+ (1,0) [0|4095] "" XXX + SG_ ANG_OBJ : 11|12@0- (1,0) [-2047|2047] "" XXX + SG_ RELV_OBJ : 31|11@0- (1,0) [-1023|1023] "" XXX BO_ 1177 RADAR_499: 8 XXX \ No newline at end of file diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 0ec36a359..62244a2ba 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -192,9 +192,9 @@ def create_radar_command(packer, car_fingerprint, frame, c, CS): addr_name = f"RADAR_{addr}" msg = CS.cp_cam.vl[addr_name] values = { - "MSGS" : int(msg["MSGS"]), - "MSGS" : int(msg["MSGS"]), - "CTR" : int(msg["CTR"]) + "MSGS_1" : int(msg["MSGS_1"]), + "MSGS_2" : int(msg["MSGS_2"]), + "CTR" : int(msg["CTR"]) } ret.append(packer.make_can_msg(addr_name, 0, values)) diff --git a/selfdrive/car/mazda/radar_interface.py b/selfdrive/car/mazda/radar_interface.py index 774aa2c19..6ff138059 100755 --- a/selfdrive/car/mazda/radar_interface.py +++ b/selfdrive/car/mazda/radar_interface.py @@ -7,13 +7,13 @@ from selfdrive.car.interfaces import RadarInterfaceBase from selfdrive.car.mazda.values import DBC def get_radar_can_parser(CP): - #if DBC[CP.carFingerprint]['radar'] is None: - # return None + if DBC[CP.carFingerprint]['radar'] is None: + return None signals = [] checks = [] for addr in range(361,367): - msg = f"RADAR_{addr}" + msg = f"RADAR_TRACK_{addr}" signals += [ ("ANG_OBJ", msg), ("DIST_OBJ", msg), @@ -32,9 +32,9 @@ class RadarInterface(RadarInterfaceBase): self.rcp = get_radar_can_parser(CP) def update(self, can_strings): - #if self.radar_off_can or (self.rcp is None): - #print(self.radar_off_can) - #return super().update(None) + if self.radar_off_can or (self.rcp is None): + print(self.radar_off_can) + return super().update(None) vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) rr = self._update(self.updated_messages) @@ -42,35 +42,30 @@ class RadarInterface(RadarInterfaceBase): return rr def _update(self, updated_messages): - ret = car.RadarData.new_message() - if self.rcp is None: - return ret - - errors = [] - - if not self.rcp.can_valid: - errors.append("canError") - ret.errors = errors - - for addr in range(361,367): - msg = self.rcp.vl[f"RADAR_TRACK_{addr}"] - if addr not in self.pts: - self.pts[addr] = car.RadarData.RadarPoint.new_message() - self.pts[addr].trackId = self.track_id - self.track_id += 1 - - valid = (msg['DIST_OBJ'] != 4095) and (msg['ANG_OBJ'] != 2046) and (msg['RELV_OBJ'] != -16) - if valid: - azimuth = math.radians(msg['ANG_OBJ']/64) - self.pts[addr].measured = True - self.pts[addr].dRel = msg['DIST_OBJ']/16 - self.pts[addr].yRel = -math.sin(azimuth) * msg['DIST_OBJ']/16 - self.pts[addr].vRel = msg['RELV_OBJ']/64 - self.pts[addr].aRel = float('nan') - self.pts[addr].yvRel = float('nan') - else: - del self.pts[addr] - - ret.points = list(self.pts.values()) + ret = car.RadarData.new_message() + if self.rcp is None: return ret + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + for addr in range(361,367): + msg = self.rcp.vl[f"RADAR_TRACK_{addr}"] + if addr not in self.pts: + self.pts[addr] = car.RadarData.RadarPoint.new_message() + self.pts[addr].trackId = self.track_id + self.track_id += 1 + valid = (msg['DIST_OBJ'] != 4095) and (msg['ANG_OBJ'] != 2046) and (msg['RELV_OBJ'] != -16) + if valid: + azimuth = math.radians(msg['ANG_OBJ']/64) + self.pts[addr].measured = True + self.pts[addr].dRel = msg['DIST_OBJ']/16 + self.pts[addr].yRel = -math.sin(azimuth) * msg['DIST_OBJ']/16 + self.pts[addr].vRel = msg['RELV_OBJ']/64 + self.pts[addr].aRel = float('nan') + self.pts[addr].yvRel = float('nan') + else: + del self.pts[addr] + ret.points = list(self.pts.values()) + return ret