Adeeb Shihadeh
3 years ago
parent
bd5e28909f
commit
ddb7f91c9b
3 changed files with 0 additions and 405 deletions
@ -1,245 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
# pylint: skip-file |
|
||||||
|
|
||||||
# This file is not used by openpilot. Only boardd.cc is used. |
|
||||||
# The python version is slower, but has more options for development. |
|
||||||
|
|
||||||
# TODO: merge the extra functionalities of this file (like MOCK) in boardd.c and |
|
||||||
# delete this python version of boardd |
|
||||||
|
|
||||||
import os |
|
||||||
import struct |
|
||||||
import time |
|
||||||
|
|
||||||
import cereal.messaging as messaging |
|
||||||
from common.realtime import Ratekeeper |
|
||||||
from system.swaglog import cloudlog |
|
||||||
from selfdrive.boardd.boardd import can_capnp_to_can_list |
|
||||||
from cereal import car |
|
||||||
|
|
||||||
SafetyModel = car.CarParams.SafetyModel |
|
||||||
|
|
||||||
# USB is optional |
|
||||||
try: |
|
||||||
import usb1 |
|
||||||
from usb1 import USBErrorIO, USBErrorOverflow # pylint: disable=no-name-in-module |
|
||||||
except Exception: |
|
||||||
pass |
|
||||||
|
|
||||||
# *** serialization functions *** |
|
||||||
def can_list_to_can_capnp(can_msgs, msgtype='can'): |
|
||||||
dat = messaging.new_message(msgtype, len(can_msgs)) |
|
||||||
for i, can_msg in enumerate(can_msgs): |
|
||||||
if msgtype == 'sendcan': |
|
||||||
cc = dat.sendcan[i] |
|
||||||
else: |
|
||||||
cc = dat.can[i] |
|
||||||
cc.address = can_msg[0] |
|
||||||
cc.busTime = can_msg[1] |
|
||||||
cc.dat = bytes(can_msg[2]) |
|
||||||
cc.src = can_msg[3] |
|
||||||
return dat |
|
||||||
|
|
||||||
|
|
||||||
# *** can driver *** |
|
||||||
def can_health(): |
|
||||||
while 1: |
|
||||||
try: |
|
||||||
dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x16) |
|
||||||
break |
|
||||||
except (USBErrorIO, USBErrorOverflow): |
|
||||||
cloudlog.exception("CAN: BAD HEALTH, RETRYING") |
|
||||||
v, i = struct.unpack("II", dat[0:8]) |
|
||||||
ign_line, ign_can = struct.unpack("BB", dat[20:22]) |
|
||||||
return {"voltage": v, "current": i, "ignition_line": bool(ign_line), "ignition_can": bool(ign_can)} |
|
||||||
|
|
||||||
def __parse_can_buffer(dat): |
|
||||||
ret = [] |
|
||||||
for j in range(0, len(dat), 0x10): |
|
||||||
ddat = dat[j:j+0x10] |
|
||||||
f1, f2 = struct.unpack("II", ddat[0:8]) |
|
||||||
ret.append((f1 >> 21, f2 >> 16, ddat[8:8 + (f2 & 0xF)], (f2 >> 4) & 0xFF)) |
|
||||||
return ret |
|
||||||
|
|
||||||
def can_send_many(arr): |
|
||||||
snds = [] |
|
||||||
for addr, _, dat, alt in arr: |
|
||||||
if addr < 0x800: # only support 11 bit addr |
|
||||||
snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat |
|
||||||
snd = snd.ljust(0x10, b'\x00') |
|
||||||
snds.append(snd) |
|
||||||
while 1: |
|
||||||
try: |
|
||||||
handle.bulkWrite(3, b''.join(snds)) |
|
||||||
break |
|
||||||
except (USBErrorIO, USBErrorOverflow): |
|
||||||
cloudlog.exception("CAN: BAD SEND MANY, RETRYING") |
|
||||||
|
|
||||||
def can_recv(): |
|
||||||
dat = b"" |
|
||||||
while 1: |
|
||||||
try: |
|
||||||
dat = handle.bulkRead(1, 0x10*256) |
|
||||||
break |
|
||||||
except (USBErrorIO, USBErrorOverflow): |
|
||||||
cloudlog.exception("CAN: BAD RECV, RETRYING") |
|
||||||
return __parse_can_buffer(dat) |
|
||||||
|
|
||||||
def can_init(): |
|
||||||
global handle, context |
|
||||||
handle = None |
|
||||||
cloudlog.info("attempting can init") |
|
||||||
|
|
||||||
context = usb1.USBContext() |
|
||||||
#context.setDebug(9) |
|
||||||
|
|
||||||
for device in context.getDeviceList(skip_on_error=True): |
|
||||||
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc: |
|
||||||
handle = device.open() |
|
||||||
handle.claimInterface(0) |
|
||||||
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'') |
|
||||||
|
|
||||||
if handle is None: |
|
||||||
cloudlog.warning("CAN NOT FOUND") |
|
||||||
exit(-1) |
|
||||||
|
|
||||||
cloudlog.info("got handle") |
|
||||||
cloudlog.info("can init done") |
|
||||||
|
|
||||||
def boardd_mock_loop(): |
|
||||||
can_init() |
|
||||||
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'') |
|
||||||
|
|
||||||
logcan = messaging.sub_sock('can') |
|
||||||
sendcan = messaging.pub_sock('sendcan') |
|
||||||
|
|
||||||
while 1: |
|
||||||
tsc = messaging.drain_sock(logcan, wait_for_one=True) |
|
||||||
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc) |
|
||||||
snd = [] |
|
||||||
for s in snds: |
|
||||||
snd += s |
|
||||||
snd = list(filter(lambda x: x[-1] <= 2, snd)) |
|
||||||
snd_0 = len(list(filter(lambda x: x[-1] == 0, snd))) |
|
||||||
snd_1 = len(list(filter(lambda x: x[-1] == 1, snd))) |
|
||||||
snd_2 = len(list(filter(lambda x: x[-1] == 2, snd))) |
|
||||||
can_send_many(snd) |
|
||||||
|
|
||||||
# recv @ 100hz |
|
||||||
can_msgs = can_recv() |
|
||||||
got_0 = len(list(filter(lambda x: x[-1] == 0+0x80, can_msgs))) |
|
||||||
got_1 = len(list(filter(lambda x: x[-1] == 1+0x80, can_msgs))) |
|
||||||
got_2 = len(list(filter(lambda x: x[-1] == 2+0x80, can_msgs))) |
|
||||||
print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" % |
|
||||||
(len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1, got_2)) |
|
||||||
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan') |
|
||||||
sendcan.send(m.to_bytes()) |
|
||||||
|
|
||||||
def boardd_test_loop(): |
|
||||||
can_init() |
|
||||||
cnt = 0 |
|
||||||
while 1: |
|
||||||
can_send_many([[0xbb, 0, "\xaa\xaa\xaa\xaa", 0], [0xaa, 0, "\xaa\xaa\xaa\xaa"+struct.pack("!I", cnt), 1]]) |
|
||||||
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",0]]) |
|
||||||
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",1]]) |
|
||||||
# recv @ 100hz |
|
||||||
can_msgs = can_recv() |
|
||||||
print("got %d" % (len(can_msgs))) |
|
||||||
time.sleep(0.01) |
|
||||||
cnt += 1 |
|
||||||
|
|
||||||
# *** main loop *** |
|
||||||
def boardd_loop(rate=100): |
|
||||||
rk = Ratekeeper(rate) |
|
||||||
|
|
||||||
can_init() |
|
||||||
|
|
||||||
# *** publishes can and health |
|
||||||
logcan = messaging.pub_sock('can') |
|
||||||
health_sock = messaging.pub_sock('pandaStates') |
|
||||||
|
|
||||||
# *** subscribes to can send |
|
||||||
sendcan = messaging.sub_sock('sendcan') |
|
||||||
|
|
||||||
# drain sendcan to delete any stale messages from previous runs |
|
||||||
messaging.drain_sock(sendcan) |
|
||||||
|
|
||||||
while 1: |
|
||||||
# health packet @ 2hz |
|
||||||
if (rk.frame % (rate // 2)) == 0: |
|
||||||
health = can_health() |
|
||||||
msg = messaging.new_message('pandaStates', 1) |
|
||||||
|
|
||||||
# store the health to be logged |
|
||||||
msg.pandaState[0].voltage = health['voltage'] |
|
||||||
msg.pandaState[0].current = health['current'] |
|
||||||
msg.pandaState[0].ignitionLine = health['ignition_line'] |
|
||||||
msg.pandaState[0].ignitionCan = health['ignition_can'] |
|
||||||
msg.pandaState[0].controlsAllowed = True |
|
||||||
|
|
||||||
health_sock.send(msg.to_bytes()) |
|
||||||
|
|
||||||
# recv @ 100hz |
|
||||||
can_msgs = can_recv() |
|
||||||
|
|
||||||
# publish to logger |
|
||||||
# TODO: refactor for speed |
|
||||||
if len(can_msgs) > 0: |
|
||||||
dat = can_list_to_can_capnp(can_msgs).to_bytes() |
|
||||||
logcan.send(dat) |
|
||||||
|
|
||||||
# send can if we have a packet |
|
||||||
tsc = messaging.recv_sock(sendcan) |
|
||||||
if tsc is not None: |
|
||||||
can_send_many(can_capnp_to_can_list(tsc.sendcan)) |
|
||||||
|
|
||||||
rk.keep_time() |
|
||||||
|
|
||||||
# *** main loop *** |
|
||||||
def boardd_proxy_loop(rate=100, address="192.168.2.251"): |
|
||||||
rk = Ratekeeper(rate) |
|
||||||
|
|
||||||
can_init() |
|
||||||
|
|
||||||
# *** subscribes can |
|
||||||
logcan = messaging.sub_sock('can', addr=address) |
|
||||||
# *** publishes to can send |
|
||||||
sendcan = messaging.pub_sock('sendcan') |
|
||||||
|
|
||||||
# drain sendcan to delete any stale messages from previous runs |
|
||||||
messaging.drain_sock(sendcan) |
|
||||||
|
|
||||||
while 1: |
|
||||||
# recv @ 100hz |
|
||||||
can_msgs = can_recv() |
|
||||||
#for m in can_msgs: |
|
||||||
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) |
|
||||||
|
|
||||||
# publish to logger |
|
||||||
# TODO: refactor for speed |
|
||||||
if len(can_msgs) > 0: |
|
||||||
dat = can_list_to_can_capnp(can_msgs, "sendcan") |
|
||||||
sendcan.send(dat) |
|
||||||
|
|
||||||
# send can if we have a packet |
|
||||||
tsc = messaging.recv_sock(logcan) |
|
||||||
if tsc is not None: |
|
||||||
cl = can_capnp_to_can_list(tsc.can) |
|
||||||
#for m in cl: |
|
||||||
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) |
|
||||||
can_send_many(cl) |
|
||||||
|
|
||||||
rk.keep_time() |
|
||||||
|
|
||||||
def main(): |
|
||||||
if os.getenv("MOCK") is not None: |
|
||||||
boardd_mock_loop() |
|
||||||
elif os.getenv("PROXY") is not None: |
|
||||||
boardd_proxy_loop() |
|
||||||
elif os.getenv("BOARDTEST") is not None: |
|
||||||
boardd_test_loop() |
|
||||||
else: |
|
||||||
boardd_loop() |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
main() |
|
||||||
@ -1,83 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import os |
|
||||||
import sys |
|
||||||
import time |
|
||||||
import signal |
|
||||||
import traceback |
|
||||||
import usb1 |
|
||||||
from panda import Panda, PandaDFU |
|
||||||
from multiprocessing import Pool |
|
||||||
|
|
||||||
jungle = "JUNGLE" in os.environ |
|
||||||
if jungle: |
|
||||||
from panda_jungle import PandaJungle # pylint: disable=import-error |
|
||||||
|
|
||||||
import cereal.messaging as messaging |
|
||||||
from selfdrive.boardd.boardd import can_capnp_to_can_list |
|
||||||
|
|
||||||
def initializer(): |
|
||||||
"""Ignore CTRL+C in the worker process. |
|
||||||
source: https://stackoverflow.com/a/44869451 """ |
|
||||||
signal.signal(signal.SIGINT, signal.SIG_IGN) |
|
||||||
|
|
||||||
def send_thread(sender_serial): |
|
||||||
while True: |
|
||||||
try: |
|
||||||
if jungle: |
|
||||||
sender = PandaJungle(sender_serial) |
|
||||||
else: |
|
||||||
sender = Panda(sender_serial) |
|
||||||
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) |
|
||||||
|
|
||||||
sender.set_can_loopback(False) |
|
||||||
can_sock = messaging.sub_sock('can') |
|
||||||
|
|
||||||
while True: |
|
||||||
tsc = messaging.recv_one(can_sock) |
|
||||||
snd = can_capnp_to_can_list(tsc.can) |
|
||||||
snd = list(filter(lambda x: x[-1] <= 2, snd)) |
|
||||||
|
|
||||||
try: |
|
||||||
sender.can_send_many(snd) |
|
||||||
except usb1.USBErrorTimeout: |
|
||||||
pass |
|
||||||
|
|
||||||
# Drain panda message buffer |
|
||||||
sender.can_recv() |
|
||||||
except Exception: |
|
||||||
traceback.print_exc() |
|
||||||
time.sleep(1) |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
if jungle: |
|
||||||
serials = PandaJungle.list() |
|
||||||
else: |
|
||||||
serials = Panda.list() |
|
||||||
num_senders = len(serials) |
|
||||||
|
|
||||||
if num_senders == 0: |
|
||||||
print("No senders found. Exiting") |
|
||||||
sys.exit(1) |
|
||||||
else: |
|
||||||
print("%d senders found. Starting broadcast" % num_senders) |
|
||||||
|
|
||||||
if "FLASH" in os.environ: |
|
||||||
for s in PandaDFU.list(): |
|
||||||
PandaDFU(s).recover() |
|
||||||
|
|
||||||
time.sleep(1) |
|
||||||
for s in serials: |
|
||||||
Panda(s).recover() |
|
||||||
Panda(s).flash() |
|
||||||
|
|
||||||
pool = Pool(num_senders, initializer=initializer) |
|
||||||
pool.map_async(send_thread, serials) |
|
||||||
|
|
||||||
while True: |
|
||||||
try: |
|
||||||
time.sleep(10) |
|
||||||
except KeyboardInterrupt: |
|
||||||
pool.terminate() |
|
||||||
pool.join() |
|
||||||
raise |
|
||||||
|
|
||||||
@ -1,77 +0,0 @@ |
|||||||
import random |
|
||||||
import numpy as np |
|
||||||
|
|
||||||
import selfdrive.boardd.tests.boardd_old as boardd_old |
|
||||||
import selfdrive.boardd.boardd as boardd |
|
||||||
|
|
||||||
from common.realtime import sec_since_boot |
|
||||||
from cereal import log |
|
||||||
import unittest |
|
||||||
|
|
||||||
|
|
||||||
def generate_random_can_data_list(): |
|
||||||
can_list = [] |
|
||||||
cnt = random.randint(1, 64) |
|
||||||
for _ in range(cnt): |
|
||||||
can_data = np.random.bytes(random.randint(1, 8)) |
|
||||||
can_list.append([random.randint(0, 128), random.randint(0, 128), can_data, random.randint(0, 128)]) |
|
||||||
return can_list, cnt |
|
||||||
|
|
||||||
|
|
||||||
class TestBoarddApiMethods(unittest.TestCase): |
|
||||||
def test_correctness(self): |
|
||||||
for i in range(1000): |
|
||||||
can_list, _ = generate_random_can_data_list() |
|
||||||
|
|
||||||
# Sendcan |
|
||||||
# Old API |
|
||||||
m_old = boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes() |
|
||||||
# new API |
|
||||||
m = boardd.can_list_to_can_capnp(can_list, 'sendcan') |
|
||||||
|
|
||||||
ev_old = log.Event.from_bytes(m_old) |
|
||||||
ev = log.Event.from_bytes(m) |
|
||||||
|
|
||||||
self.assertEqual(ev_old.which(), ev.which()) |
|
||||||
self.assertEqual(len(ev.sendcan), len(ev_old.sendcan)) |
|
||||||
for i in range(len(ev.sendcan)): |
|
||||||
attrs = ['address', 'busTime', 'dat', 'src'] |
|
||||||
for attr in attrs: |
|
||||||
self.assertEqual(getattr(ev.sendcan[i], attr, 'new'), getattr(ev_old.sendcan[i], attr, 'old')) |
|
||||||
|
|
||||||
# Can |
|
||||||
m_old = boardd_old.can_list_to_can_capnp(can_list, 'can').to_bytes() |
|
||||||
# new API |
|
||||||
m = boardd.can_list_to_can_capnp(can_list, 'can') |
|
||||||
|
|
||||||
ev_old = log.Event.from_bytes(m_old) |
|
||||||
ev = log.Event.from_bytes(m) |
|
||||||
self.assertEqual(ev_old.which(), ev.which()) |
|
||||||
self.assertEqual(len(ev.can), len(ev_old.can)) |
|
||||||
for i in range(len(ev.can)): |
|
||||||
attrs = ['address', 'busTime', 'dat', 'src'] |
|
||||||
for attr in attrs: |
|
||||||
self.assertEqual(getattr(ev.can[i], attr, 'new'), getattr(ev_old.can[i], attr, 'old')) |
|
||||||
|
|
||||||
def test_performance(self): |
|
||||||
can_list, _ = generate_random_can_data_list() |
|
||||||
recursions = 1000 |
|
||||||
|
|
||||||
n1 = sec_since_boot() |
|
||||||
for _ in range(recursions): |
|
||||||
boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes() |
|
||||||
n2 = sec_since_boot() |
|
||||||
elapsed_old = n2 - n1 |
|
||||||
|
|
||||||
# print('Old API, elapsed time: {} secs'.format(elapsed_old)) |
|
||||||
n1 = sec_since_boot() |
|
||||||
for _ in range(recursions): |
|
||||||
boardd.can_list_to_can_capnp(can_list) |
|
||||||
n2 = sec_since_boot() |
|
||||||
elapsed_new = n2 - n1 |
|
||||||
# print('New API, elapsed time: {} secs'.format(elapsed_new)) |
|
||||||
self.assertTrue(elapsed_new < elapsed_old / 2) |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__': |
|
||||||
unittest.main() |
|
||||||
Loading…
Reference in new issue