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.
62 lines
1.3 KiB
62 lines
1.3 KiB
5 years ago
|
#!/usr/bin/env python3
|
||
|
|
import zmq
|
||
|
|
import time
|
||
|
|
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
|
||
|
|
|
||
|
|
|
||
|
|
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_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.1:
|
||
|
|
received = True
|
||
|
|
|
||
|
|
return r
|
||
|
|
|
||
|
|
|
||
|
|
can = messaging.sub_sock('can')
|
||
|
|
sendcan = messaging.pub_sock('sendcan')
|
||
|
|
|
||
|
|
PUBLIC = 0
|
||
|
|
PRIVATE = 1
|
||
|
|
|
||
|
|
time.sleep(0.5)
|
||
|
|
|
||
|
|
# 1, 112
|
||
|
|
|
||
|
|
TEST_ON = "\xFF\x02\x10\x70\x00\x00\x00\x00"
|
||
|
|
POLL = "\xFF\x02\x21\x69\x00\x00\x00\x00"
|
||
|
|
send(sendcan, 1872, TEST_ON)
|
||
|
|
r = recv_timeout(can, 1880)
|
||
|
|
print(r)
|
||
|
|
|
||
|
|
|
||
|
|
for i in range(0, 256):
|
||
|
|
send(sendcan, 1872, POLL.replace('\x69', chr(i)))
|
||
|
|
r = recv_timeout(can, 1880)
|
||
|
|
if len(r):
|
||
|
|
print(i, end=' ')
|
||
|
|
for m in r:
|
||
|
|
print(m.dat.encode('hex'))
|