diff --git a/opendbc/gm_global_a_object.dbc b/opendbc/gm_global_a_object.dbc index b4aa31a3e1..8847bab39d 100644 --- a/opendbc/gm_global_a_object.dbc +++ b/opendbc/gm_global_a_object.dbc @@ -67,8 +67,37 @@ BO_ 776 ASCMAccSpeedStatus: 7 NEO SG_ VehicleSpeed : 15|12@0+ (1,0) [0|0] "" B233B_LRR SG_ AlwaysOne : 3|1@0+ (1,0) [0|0] "" B233B_LRR -BO_ 1120 LRRNumObjects: 8 B233B_LRR - SG_ LRRNumObjects : 20|5@0+ (1,0) [0|0] "" NEO +BO_ 1120 F_LRR_Obj_Header: 8 LRR_FO + SG_ FLRRRollingCount : 7|2@0+ (1,0) [0|3] "" EOCM_F_FO + SG_ FLRRModeCmdFdbk : 23|3@0+ (1,0) [0|7] "" EOCM_F_FO + SG_ FLRRNumValidTargets : 20|5@0+ (1,0) [0|31] "" EOCM_F_FO + SG_ FLRRTimeStampV : 31|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRTimeStamp : 2|11@0+ (1,0) [0|2047] "ms" EOCM_F_FO + SG_ FLRRRoadTypeInfo : 5|3@0+ (1,0) [0|7] "" EOCM_F_FO + SG_ FLRRBurstChecksum : 55|16@0+ (1,0) [0|65535] "" EOCM_F_FO + SG_ FLRRDiagSpare : 30|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRVltgOutRngLo : 44|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRVltgOutRngHi : 43|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRSvcAlgnInPrcs : 38|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRSnsrBlckd : 45|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRSnstvFltPrsntInt : 24|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRPlntAlgnInProc : 37|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRMsalgnYawRt : 47|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRMsalgnYawLt : 46|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRLonVelPlsblityFlt : 35|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRYawRtPlsblityFlt : 34|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRMsalgnPtchUp : 32|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRMsalgnPtchDn : 33|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRInitDiagCmplt : 40|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRHWFltPrsntInt : 25|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRExtIntrfrnc : 36|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRCANSgnlSpvFld : 29|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRCANRxErr : 28|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRTunlDtctd : 27|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRAmbTmpOutRngLw : 42|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRAmbTmpOutRngHi : 41|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRAntTngFltPrsnt : 26|1@0+ (1,0) [0|1] "" EOCM_F_FO + SG_ FLRRAlgnFltPrsnt : 39|1@0+ (1,0) [0|1] "" EOCM_F_FO BO_ 1134 LRRObject14: 8 B233B_LRR SG_ TrkRange : 5|11@0+ (0.125,0) [0|255.875] "m" NEO diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index bc35b3459b..a14cb1603f 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -11,13 +11,13 @@ from common.realtime import sec_since_boot from selfdrive.services import service_list import selfdrive.messaging as messaging -NUM_TARGETS_MSG = 1120 -SLOT_1_MSG = NUM_TARGETS_MSG + 1 +RADAR_HEADER_MSG = 1120 +SLOT_1_MSG = RADAR_HEADER_MSG + 1 NUM_SLOTS = 20 # Actually it's 0x47f, but can parser only reports # messages that are present in DBC -LAST_RADAR_MSG = NUM_TARGETS_MSG + NUM_SLOTS +LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS def create_radard_can_parser(canbus, car_fingerprint): @@ -25,12 +25,16 @@ def create_radard_can_parser(canbus, car_fingerprint): if car_fingerprint == CAR.VOLT: # C1A-ARS3-A by Continental radar_targets = range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS) - signals = zip(['LRRNumObjects'] + + signals = zip(['FLRRNumValidTargets', + 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', + 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', + 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, - [NUM_TARGETS_MSG] + radar_targets * 6, - [0] + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + + [RADAR_HEADER_MSG] * 7 + radar_targets * 6, + [0] * 7 + + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0] * NUM_SLOTS) @@ -44,8 +48,6 @@ class RadarInterface(object): def __init__(self, CP): # radar self.pts = {} - self.track_id = 0 - self.num_targets = 0 self.delay = 0.0 # Delay of radar @@ -70,22 +72,27 @@ class RadarInterface(object): if LAST_RADAR_MSG in updated_messages: break + header = self.rcp.vl[RADAR_HEADER_MSG] + fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \ + header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \ + header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt'] errors = [] if not self.rcp.can_valid: - errors.append("notValid") + errors.append("commIssue") + if fault: + errors.append("fault") ret.errors = errors currentTargets = set() - if self.rcp.vl[NUM_TARGETS_MSG]['LRRNumObjects'] != self.num_targets: - self.num_targets = self.rcp.vl[NUM_TARGETS_MSG]['LRRNumObjects'] + num_targets = header['FLRRNumValidTargets'] # Not all radar messages describe targets, - # no need to monitor all of the sself.rcp.msgs_upd + # no need to monitor all of the self.rcp.msgs_upd for ii in updated_messages: - if ii == NUM_TARGETS_MSG: + if ii == RADAR_HEADER_MSG: continue - if self.num_targets == 0: + if num_targets == 0: break cpt = self.rcp.vl[ii]