GM: disengage on radar fault (commaai#396)

* GM: copy cadillac's radar header DBC

* Remove unused interface imports

* Rename num targets message to radar header

* Catch various radar faults

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

selfdrive/car/gm/radar_interface.py

@@ -11,26 +11,30 @@
 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):
 
   dbc_f = DBC[car_fingerprint]['radar']
   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 @@ def update(self):
       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]