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.
152 lines
3.3 KiB
152 lines
3.3 KiB
#!/usr/bin/env python3
|
|
import zmq
|
|
import time
|
|
import random
|
|
from collections import defaultdict, OrderedDict
|
|
|
|
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
|
from selfdrive.car.toyota.toyotacan import make_can_msg
|
|
import cereal.messaging as messaging
|
|
from cereal.services import service_list
|
|
|
|
|
|
fields = range(0, 256)
|
|
fields = [105, 225]
|
|
field_results = defaultdict(lambda: "\x00\x00")
|
|
cur_field = 97
|
|
|
|
def send(sendcan, addr, m):
|
|
packet = make_can_msg(addr, m, 0, False)
|
|
packets = can_list_to_can_capnp([packet], msgtype='sendcan')
|
|
sendcan.send(packets.to_bytes())
|
|
|
|
|
|
def recv(can, addr):
|
|
received = False
|
|
r = []
|
|
|
|
while not received:
|
|
c = messaging.recv_one(can)
|
|
for msg in c.can:
|
|
if msg.address == addr:
|
|
r.append(msg)
|
|
received = True
|
|
return r
|
|
|
|
|
|
def recv_timeout(can, addr):
|
|
received = False
|
|
r = []
|
|
t = time.time()
|
|
|
|
while not received:
|
|
c = messaging.recv_one_or_none(can)
|
|
|
|
if c is not None:
|
|
for msg in c.can:
|
|
if msg.address == addr:
|
|
r.append(msg)
|
|
received = True
|
|
|
|
if time.time() - t > 0.05:
|
|
received = True
|
|
|
|
return r
|
|
|
|
|
|
def print_hex(d):
|
|
s = map(ord, d)
|
|
s = "".join(["{:02X}".format(b) for b in s])
|
|
print(s)
|
|
|
|
|
|
TYPES = {
|
|
0: 'single',
|
|
1: 'first',
|
|
2: 'consecutive',
|
|
3: 'flow'
|
|
}
|
|
|
|
FIRST = "\x42\x02\xA8\x01\x00\x00\x00\x00"
|
|
CONTINUE = "\x42\x30\x01\x00\x00\x00\x00\x00"
|
|
|
|
TEST_ON = "\x42\x02\x10\x60\x00\x00\x00\x00"
|
|
TEST_OFF = "\x42\x02\x10\x5F\x00\x00\x00\x00"
|
|
|
|
POLL = "\x42\x02\x21\x69\x00\x00\x00\x00"
|
|
|
|
prev_rcv_t = ""
|
|
recv_data = []
|
|
l = 0
|
|
index = 0
|
|
|
|
|
|
can = messaging.sub_sock('can')
|
|
sendcan = messaging.pub_sock('sendcan')
|
|
|
|
time.sleep(0.5)
|
|
|
|
send(sendcan, 1872, FIRST)
|
|
results = []
|
|
|
|
test_mode = False
|
|
|
|
while True:
|
|
# Send flow control if necessary
|
|
if prev_rcv_t == 'first' or prev_rcv_t == 'consecutive':
|
|
send(sendcan, 1872, CONTINUE)
|
|
|
|
received = recv_timeout(can, 1880)
|
|
|
|
if len(received) == 0:
|
|
print(chr(27) + "[2J")
|
|
print(time.time())
|
|
if results[0] != "\x7F\x21\x31":
|
|
field_results[cur_field] = results[0]
|
|
else:
|
|
fields.remove(cur_field)
|
|
for k in fields:
|
|
if field_results[k] == "\x00\x00":
|
|
continue
|
|
print(k, end=' ')
|
|
print_hex(field_results[k])
|
|
results = []
|
|
|
|
if not test_mode:
|
|
send(sendcan, 1872, TEST_ON)
|
|
test_mode = True
|
|
else:
|
|
cur_field = random.choice(fields)
|
|
send(sendcan, 1872, POLL.replace('\x69', chr(cur_field)))
|
|
|
|
for r in received:
|
|
data = r.dat
|
|
|
|
# Check message type
|
|
t = TYPES[ord(data[1]) >> 4]
|
|
if t == 'single':
|
|
l = ord(data[1]) & 0x0F
|
|
elif t == 'first':
|
|
a = ord(data[1]) & 0x0F
|
|
b = ord(data[2])
|
|
l = b + (a << 8)
|
|
recv_data = []
|
|
|
|
prev_rcv_t = t
|
|
|
|
if t == 'single':
|
|
recv_data = data[2: 2 + l]
|
|
results.append(recv_data)
|
|
if t == 'first':
|
|
index = 0
|
|
recv_data += data[3: min(8, 3 + l)]
|
|
if t == 'consecutive':
|
|
index += 1
|
|
assert index == ord(data[1]) & 0x0F
|
|
|
|
pending_l = l - len(recv_data)
|
|
recv_data += data[2: min(8, 2 + pending_l)]
|
|
|
|
if len(recv_data) == l:
|
|
prev_rcv_t = ""
|
|
results.append(recv_data)
|
|
|