mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 18:12:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

GM: disengage on radar fault (#396)

Browse Source 
* GM: copy cadillac's radar header DBC

* Remove unused interface imports

* Rename num targets message to radar header

* Catch various radar faults

old-commit-hash: 3d05cca678
This commit is contained in:
Vasily Tarasov
2018-10-23 18:59:34 -07:00
committed by rbiasini
parent bd651baa56
commit aa512e987a
2 changed files with 52 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc/gm_global_a_object.dbc
+31 -2
View File
@@ -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
+21 -14
View File
@@ -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]