You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
107 lines
3.7 KiB
107 lines
3.7 KiB
#!/usr/bin/env python
|
|
import os
|
|
from selfdrive.can.parser import CANParser
|
|
from cereal import car
|
|
from common.realtime import sec_since_boot
|
|
import zmq
|
|
from selfdrive.services import service_list
|
|
import selfdrive.messaging as messaging
|
|
|
|
|
|
RADAR_MSGS_C = range(0x2c2, 0x2d4+2, 2) # c_ messages 706,...,724
|
|
RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2) # d_ messages
|
|
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D)
|
|
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
|
|
|
|
def _create_radard_can_parser():
|
|
dbc_f = 'chrysler_pacifica_2017_hybrid_private_fusion.dbc'
|
|
msg_n = len(RADAR_MSGS_C)
|
|
# list of [(signal name, message name or number, initial values), (...)]
|
|
# [('RADAR_STATE', 1024, 0),
|
|
# ('LONG_DIST', 1072, 255),
|
|
# ('LONG_DIST', 1073, 255),
|
|
# ('LONG_DIST', 1074, 255),
|
|
# ('LONG_DIST', 1075, 255),
|
|
|
|
# The factor and offset are applied by the dbc parsing library, so the
|
|
# default values should be after the factor/offset are applied.
|
|
signals = list(zip(['LONG_DIST'] * msg_n +
|
|
['LAT_DIST'] * msg_n +
|
|
['REL_SPEED'] * msg_n,
|
|
RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST
|
|
RADAR_MSGS_D, # REL_SPEED
|
|
[0] * msg_n + # LONG_DIST
|
|
[-1000] * msg_n + # LAT_DIST
|
|
[-146.278] * msg_n)) # REL_SPEED set to 0, factor/offset to this
|
|
# TODO what are the checks actually used for?
|
|
# honda only checks the last message,
|
|
# toyota checks all the messages. Which do we want?
|
|
checks = list(zip(RADAR_MSGS_C +
|
|
RADAR_MSGS_D,
|
|
[20]*msg_n + # 20Hz (0.05s)
|
|
[20]*msg_n)) # 20Hz (0.05s)
|
|
|
|
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
|
|
|
|
def _address_to_track(address):
|
|
if address in RADAR_MSGS_C:
|
|
return (address - RADAR_MSGS_C[0]) // 2
|
|
if address in RADAR_MSGS_D:
|
|
return (address - RADAR_MSGS_D[0]) // 2
|
|
raise ValueError("radar received unexpected address %d" % address)
|
|
|
|
class RadarInterface(object):
|
|
def __init__(self, CP):
|
|
self.pts = {}
|
|
self.delay = 0.0 # Delay of radar #TUNE
|
|
self.rcp = _create_radard_can_parser()
|
|
context = zmq.Context()
|
|
self.logcan = messaging.sub_sock(context, service_list['can'].port)
|
|
|
|
def update(self):
|
|
canMonoTimes = []
|
|
|
|
updated_messages = set() # set of message IDs (sig_addresses) we've seen
|
|
|
|
while 1:
|
|
tm = int(sec_since_boot() * 1e9)
|
|
updated_messages.update(self.rcp.update(tm, True))
|
|
if LAST_MSG in updated_messages:
|
|
break
|
|
|
|
ret = car.RadarState.new_message()
|
|
errors = []
|
|
if not self.rcp.can_valid:
|
|
errors.append("commIssue")
|
|
ret.errors = errors
|
|
ret.canMonoTimes = canMonoTimes
|
|
|
|
for ii in updated_messages: # ii should be the message ID as a number
|
|
cpt = self.rcp.vl[ii]
|
|
trackId = _address_to_track(ii)
|
|
|
|
if trackId not in self.pts:
|
|
self.pts[trackId] = car.RadarState.RadarPoint.new_message()
|
|
self.pts[trackId].trackId = trackId
|
|
self.pts[trackId].aRel = float('nan')
|
|
self.pts[trackId].yvRel = float('nan')
|
|
self.pts[trackId].measured = True
|
|
|
|
if 'LONG_DIST' in cpt: # c_* message
|
|
self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car
|
|
# our lat_dist is positive to the right in car's frame.
|
|
# TODO what does yRel want?
|
|
self.pts[trackId].yRel = cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
|
else: # d_* message
|
|
self.pts[trackId].vRel = cpt['REL_SPEED']
|
|
|
|
# We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid.
|
|
ret.points = [x for x in self.pts.values() if x.dRel != 0]
|
|
return ret
|
|
|
|
if __name__ == "__main__":
|
|
RI = RadarInterface(None)
|
|
while 1:
|
|
ret = RI.update()
|
|
print(chr(27) + "[2J") # clear screen
|
|
print(ret)
|
|
|