Vehicle Researcher
6 years ago
parent
d34a9b934c
commit
0932b367bd
177 changed files with 2915 additions and 832 deletions
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:2d45eb035af4adeaf31fe96ab36fc310962cc8c375319ac2a5e707b2cdc03097 |
oid sha256:61fc21ef8cec4ca15be05b1b5fdb9c4d95f3a78be891a36d84bd2dd89441e28e |
||||||
size 2588994 |
size 2596571 |
||||||
|
|||||||
@ -0,0 +1,11 @@ |
|||||||
|
#!/bin/bash |
||||||
|
|
||||||
|
pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda") |
||||||
|
RESULT=$? |
||||||
|
if [ $RESULT -eq 0 ]; then |
||||||
|
pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda") |
||||||
|
RESULT=$? & 3 |
||||||
|
fi |
||||||
|
|
||||||
|
[ $RESULT -ne 0 ] && exit 1 |
||||||
|
exit 0 |
||||||
@ -0,0 +1,10 @@ |
|||||||
|
all: simple_kalman_impl.so |
||||||
|
|
||||||
|
simple_kalman_impl.so: simple_kalman_impl.pyx simple_kalman_impl.pxd simple_kalman_setup.py |
||||||
|
python simple_kalman_setup.py build_ext --inplace
|
||||||
|
rm -rf build
|
||||||
|
rm simple_kalman_impl.c
|
||||||
|
|
||||||
|
.PHONY: clean |
||||||
|
clean: |
||||||
|
rm -f simple_kalman_impl.so
|
||||||
@ -1,23 +1,10 @@ |
|||||||
import numpy as np |
# pylint: skip-file |
||||||
|
import os |
||||||
|
import subprocess |
||||||
|
|
||||||
|
kalman_dir = os.path.dirname(os.path.abspath(__file__)) |
||||||
|
subprocess.check_call(["make", "simple_kalman_impl.so"], cwd=kalman_dir) |
||||||
|
|
||||||
class KF1D: |
from simple_kalman_impl import KF1D as KF1D |
||||||
# this EKF assumes constant covariance matrix, so calculations are much simpler |
# Silence pyflakes |
||||||
# the Kalman gain also needs to be precomputed using the control module |
assert KF1D |
||||||
|
|
||||||
def __init__(self, x0, A, C, K): |
|
||||||
self.x = x0 |
|
||||||
self.A = A |
|
||||||
self.C = C |
|
||||||
self.K = K |
|
||||||
|
|
||||||
self.A_K = self.A - np.dot(self.K, self.C) |
|
||||||
|
|
||||||
# K matrix needs to be pre-computed as follow: |
|
||||||
# import control |
|
||||||
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) |
|
||||||
# self.K = np.transpose(K) |
|
||||||
|
|
||||||
def update(self, meas): |
|
||||||
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) |
|
||||||
return self.x |
|
||||||
|
|||||||
@ -0,0 +1,16 @@ |
|||||||
|
cdef class KF1D: |
||||||
|
cdef public: |
||||||
|
double x0_0 |
||||||
|
double x1_0 |
||||||
|
double K0_0 |
||||||
|
double K1_0 |
||||||
|
double A0_0 |
||||||
|
double A0_1 |
||||||
|
double A1_0 |
||||||
|
double A1_1 |
||||||
|
double C0_0 |
||||||
|
double C0_1 |
||||||
|
double A_K_0 |
||||||
|
double A_K_1 |
||||||
|
double A_K_2 |
||||||
|
double A_K_3 |
||||||
@ -0,0 +1,35 @@ |
|||||||
|
|
||||||
|
cdef class KF1D: |
||||||
|
def __init__(self, x0, A, C, K): |
||||||
|
self.x0_0 = x0[0][0] |
||||||
|
self.x1_0 = x0[1][0] |
||||||
|
self.A0_0 = A[0][0] |
||||||
|
self.A0_1 = A[0][1] |
||||||
|
self.A1_0 = A[1][0] |
||||||
|
self.A1_1 = A[1][1] |
||||||
|
self.C0_0 = C[0] |
||||||
|
self.C0_1 = C[1] |
||||||
|
self.K0_0 = K[0][0] |
||||||
|
self.K1_0 = K[1][0] |
||||||
|
|
||||||
|
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0 |
||||||
|
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1 |
||||||
|
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0 |
||||||
|
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1 |
||||||
|
|
||||||
|
def update(self, meas): |
||||||
|
cdef double x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas |
||||||
|
cdef double x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas |
||||||
|
self.x0_0 = x0_0 |
||||||
|
self.x1_0 = x1_0 |
||||||
|
|
||||||
|
return [self.x0_0, self.x1_0] |
||||||
|
|
||||||
|
@property |
||||||
|
def x(self): |
||||||
|
return [[self.x0_0], [self.x1_0]] |
||||||
|
|
||||||
|
@x.setter |
||||||
|
def x(self, x): |
||||||
|
self.x0_0 = x[0][0] |
||||||
|
self.x1_0 = x[1][0] |
||||||
@ -0,0 +1,23 @@ |
|||||||
|
import numpy as np |
||||||
|
|
||||||
|
|
||||||
|
class KF1D: |
||||||
|
# this EKF assumes constant covariance matrix, so calculations are much simpler |
||||||
|
# the Kalman gain also needs to be precomputed using the control module |
||||||
|
|
||||||
|
def __init__(self, x0, A, C, K): |
||||||
|
self.x = x0 |
||||||
|
self.A = A |
||||||
|
self.C = C |
||||||
|
self.K = K |
||||||
|
|
||||||
|
self.A_K = self.A - np.dot(self.K, self.C) |
||||||
|
|
||||||
|
# K matrix needs to be pre-computed as follow: |
||||||
|
# import control |
||||||
|
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) |
||||||
|
# self.K = np.transpose(K) |
||||||
|
|
||||||
|
def update(self, meas): |
||||||
|
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) |
||||||
|
return self.x |
||||||
@ -0,0 +1,5 @@ |
|||||||
|
from distutils.core import setup, Extension |
||||||
|
from Cython.Build import cythonize |
||||||
|
|
||||||
|
setup(name='Simple Kalman Implementation', |
||||||
|
ext_modules=cythonize(Extension("simple_kalman_impl", ["simple_kalman_impl.pyx"]))) |
||||||
@ -0,0 +1,116 @@ |
|||||||
|
import numpy as np |
||||||
|
import numpy.matlib |
||||||
|
import unittest |
||||||
|
import timeit |
||||||
|
|
||||||
|
from common.kalman.ekf import EKF, SimpleSensor, FastEKF1D |
||||||
|
|
||||||
|
class TestEKF(EKF): |
||||||
|
def __init__(self, var_init, Q): |
||||||
|
super(TestEKF, self).__init__(False) |
||||||
|
self.identity = numpy.matlib.identity(2) |
||||||
|
self.state = numpy.matlib.zeros((2, 1)) |
||||||
|
self.covar = self.identity * var_init |
||||||
|
|
||||||
|
self.process_noise = numpy.matlib.diag(Q) |
||||||
|
|
||||||
|
def calc_transfer_fun(self, dt): |
||||||
|
tf = numpy.matlib.identity(2) |
||||||
|
tf[0, 1] = dt |
||||||
|
return tf, tf |
||||||
|
|
||||||
|
|
||||||
|
class EKFTest(unittest.TestCase): |
||||||
|
def test_update_scalar(self): |
||||||
|
ekf = TestEKF(1e3, [0.1, 1]) |
||||||
|
dt = 1. / 100 |
||||||
|
|
||||||
|
sensor = SimpleSensor(0, 1, 2) |
||||||
|
readings = map(sensor.read, np.arange(100, 300)) |
||||||
|
|
||||||
|
for reading in readings: |
||||||
|
ekf.update_scalar(reading) |
||||||
|
ekf.predict(dt) |
||||||
|
|
||||||
|
np.testing.assert_allclose(ekf.state, [[300], [100]], 1e-4) |
||||||
|
np.testing.assert_allclose( |
||||||
|
ekf.covar, |
||||||
|
np.asarray([[0.0563, 0.10278], [0.10278, 0.55779]]), |
||||||
|
atol=1e-4) |
||||||
|
|
||||||
|
def test_unbiased(self): |
||||||
|
ekf = TestEKF(1e3, [0., 0.]) |
||||||
|
dt = np.float64(1. / 100) |
||||||
|
|
||||||
|
sensor = SimpleSensor(0, 1, 2) |
||||||
|
readings = map(sensor.read, np.arange(1000)) |
||||||
|
|
||||||
|
for reading in readings: |
||||||
|
ekf.update_scalar(reading) |
||||||
|
ekf.predict(dt) |
||||||
|
|
||||||
|
np.testing.assert_allclose(ekf.state, [[1000.], [100.]], 1e-4) |
||||||
|
|
||||||
|
|
||||||
|
class FastEKF1DTest(unittest.TestCase): |
||||||
|
def test_correctness(self): |
||||||
|
dt = 1. / 100 |
||||||
|
reading = SimpleSensor(0, 1, 2).read(100) |
||||||
|
|
||||||
|
ekf = TestEKF(1e3, [0.1, 1]) |
||||||
|
fast_ekf = FastEKF1D(dt, 1e3, [0.1, 1]) |
||||||
|
|
||||||
|
ekf.update_scalar(reading) |
||||||
|
fast_ekf.update_scalar(reading) |
||||||
|
self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0]) |
||||||
|
self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1]) |
||||||
|
self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0]) |
||||||
|
self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2]) |
||||||
|
self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1]) |
||||||
|
|
||||||
|
ekf.predict(dt) |
||||||
|
fast_ekf.predict(dt) |
||||||
|
self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0]) |
||||||
|
self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1]) |
||||||
|
self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0]) |
||||||
|
self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2]) |
||||||
|
self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1]) |
||||||
|
|
||||||
|
def test_speed(self): |
||||||
|
setup = """ |
||||||
|
import numpy as np |
||||||
|
from common.kalman.tests.test_ekf import TestEKF |
||||||
|
from common.kalman.ekf import SimpleSensor, FastEKF1D |
||||||
|
|
||||||
|
dt = 1. / 100 |
||||||
|
reading = SimpleSensor(0, 1, 2).read(100) |
||||||
|
|
||||||
|
var_init, Q = 1e3, [0.1, 1] |
||||||
|
ekf = TestEKF(var_init, Q) |
||||||
|
fast_ekf = FastEKF1D(dt, var_init, Q) |
||||||
|
""" |
||||||
|
|
||||||
|
timeit.timeit(""" |
||||||
|
ekf.update_scalar(reading) |
||||||
|
ekf.predict(dt) |
||||||
|
""", setup=setup, number=1000) |
||||||
|
|
||||||
|
ekf_speed = timeit.timeit(""" |
||||||
|
ekf.update_scalar(reading) |
||||||
|
ekf.predict(dt) |
||||||
|
""", setup=setup, number=20000) |
||||||
|
|
||||||
|
timeit.timeit(""" |
||||||
|
fast_ekf.update_scalar(reading) |
||||||
|
fast_ekf.predict(dt) |
||||||
|
""", setup=setup, number=1000) |
||||||
|
|
||||||
|
fast_ekf_speed = timeit.timeit(""" |
||||||
|
fast_ekf.update_scalar(reading) |
||||||
|
fast_ekf.predict(dt) |
||||||
|
""", setup=setup, number=20000) |
||||||
|
|
||||||
|
assert fast_ekf_speed < ekf_speed / 4 |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
unittest.main() |
||||||
@ -0,0 +1,85 @@ |
|||||||
|
import unittest |
||||||
|
import random |
||||||
|
import timeit |
||||||
|
import numpy as np |
||||||
|
|
||||||
|
from common.kalman.simple_kalman import KF1D |
||||||
|
from common.kalman.simple_kalman_old import KF1D as KF1D_old |
||||||
|
|
||||||
|
|
||||||
|
class TestSimpleKalman(unittest.TestCase): |
||||||
|
def setUp(self): |
||||||
|
dt = 0.01 |
||||||
|
x0_0 = 0.0 |
||||||
|
x1_0 = 0.0 |
||||||
|
A0_0 = 1.0 |
||||||
|
A0_1 = dt |
||||||
|
A1_0 = 0.0 |
||||||
|
A1_1 = 1.0 |
||||||
|
C0_0 = 1.0 |
||||||
|
C0_1 = 0.0 |
||||||
|
K0_0 = 0.12287673 |
||||||
|
K1_0 = 0.29666309 |
||||||
|
|
||||||
|
self.kf_old = KF1D_old(x0=np.matrix([[x0_0], [x1_0]]), |
||||||
|
A=np.matrix([[A0_0, A0_1], [A1_0, A1_1]]), |
||||||
|
C=np.matrix([C0_0, C0_1]), |
||||||
|
K=np.matrix([[K0_0], [K1_0]])) |
||||||
|
|
||||||
|
self.kf = KF1D(x0=[[x0_0], [x1_0]], |
||||||
|
A=[[A0_0, A0_1], [A1_0, A1_1]], |
||||||
|
C=[C0_0, C0_1], |
||||||
|
K=[[K0_0], [K1_0]]) |
||||||
|
|
||||||
|
def test_getter_setter(self): |
||||||
|
self.kf.x = [[1.0], [1.0]] |
||||||
|
self.assertEqual(self.kf.x, [[1.0], [1.0]]) |
||||||
|
|
||||||
|
def update_returns_state(self): |
||||||
|
x = self.kf.update(100) |
||||||
|
self.assertEqual(x, self.kf.x) |
||||||
|
|
||||||
|
def test_old_equal_new(self): |
||||||
|
for _ in range(1000): |
||||||
|
v_wheel = random.uniform(0, 200) |
||||||
|
|
||||||
|
x_old = self.kf_old.update(v_wheel) |
||||||
|
x = self.kf.update(v_wheel) |
||||||
|
|
||||||
|
# Compare the output x, verify that the error is less than 1e-4 |
||||||
|
self.assertAlmostEqual(x_old[0], x[0]) |
||||||
|
self.assertAlmostEqual(x_old[1], x[1]) |
||||||
|
|
||||||
|
|
||||||
|
def test_new_is_faster(self): |
||||||
|
setup = """ |
||||||
|
import numpy as np |
||||||
|
|
||||||
|
from common.kalman.simple_kalman import KF1D |
||||||
|
from common.kalman.simple_kalman_old import KF1D as KF1D_old |
||||||
|
|
||||||
|
dt = 0.01 |
||||||
|
x0_0 = 0.0 |
||||||
|
x1_0 = 0.0 |
||||||
|
A0_0 = 1.0 |
||||||
|
A0_1 = dt |
||||||
|
A1_0 = 0.0 |
||||||
|
A1_1 = 1.0 |
||||||
|
C0_0 = 1.0 |
||||||
|
C0_1 = 0.0 |
||||||
|
K0_0 = 0.12287673 |
||||||
|
K1_0 = 0.29666309 |
||||||
|
|
||||||
|
kf_old = KF1D_old(x0=np.matrix([[x0_0], [x1_0]]), |
||||||
|
A=np.matrix([[A0_0, A0_1], [A1_0, A1_1]]), |
||||||
|
C=np.matrix([C0_0, C0_1]), |
||||||
|
K=np.matrix([[K0_0], [K1_0]])) |
||||||
|
|
||||||
|
kf = KF1D(x0=[[x0_0], [x1_0]], |
||||||
|
A=[[A0_0, A0_1], [A1_0, A1_1]], |
||||||
|
C=[C0_0, C0_1], |
||||||
|
K=[[K0_0], [K1_0]]) |
||||||
|
""" |
||||||
|
kf_speed = timeit.timeit("kf.update(1234)", setup=setup, number=10000) |
||||||
|
kf_old_speed = timeit.timeit("kf_old.update(1234)", setup=setup, number=10000) |
||||||
|
self.assertTrue(kf_speed < kf_old_speed / 4) |
||||||
@ -0,0 +1,28 @@ |
|||||||
|
import signal |
||||||
|
|
||||||
|
class TimeoutException(Exception): |
||||||
|
pass |
||||||
|
|
||||||
|
class Timeout: |
||||||
|
""" |
||||||
|
Timeout context manager. |
||||||
|
For example this code will raise a TimeoutException: |
||||||
|
with Timeout(seconds=5, error_msg="Sleep was too long"): |
||||||
|
time.sleep(10) |
||||||
|
""" |
||||||
|
def __init__(self, seconds, error_msg=None): |
||||||
|
if error_msg is None: |
||||||
|
error_msg = 'Timed out after {} seconds'.format(seconds) |
||||||
|
self.seconds = seconds |
||||||
|
self.error_msg = error_msg |
||||||
|
|
||||||
|
def handle_timeout(self, signume, frame): |
||||||
|
raise TimeoutException(self.error_msg) |
||||||
|
|
||||||
|
def __enter__(self): |
||||||
|
signal.signal(signal.SIGALRM, self.handle_timeout) |
||||||
|
signal.alarm(self.seconds) |
||||||
|
|
||||||
|
def __exit__(self, exc_type, exc_val, exc_tb): |
||||||
|
signal.alarm(0) |
||||||
|
|
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:555f74644a70161700a8517ff6351ea8c988ca0a550a23550121147963c7cb6c |
||||||
|
size 159 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:b360bfe8e0769adaa8ee6ae26324e26c40ccbdb557677a6b8ad506214984d2e8 |
||||||
|
size 6245 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:526122b6d0ca17de16c39595741ac34f32d2987d9bc4ca645750a73a3d0494b5 |
||||||
|
size 260714 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:028ce694fe58a051bdc31e915da55fda733acf0ade0ad9da8ffd23deea27f941 |
||||||
|
size 1707 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:e434c73611bb5576b3a9b1a6a40bce70a5ecbe36ea1a510392a2935196af0569 |
||||||
|
size 1275 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:7a7c22cb33dcd531a21837dc70103043012f7b29d1f5b1b7aaf430f9034ef6f9 |
||||||
|
size 5539 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:c6866da03dbd52cfd3eb2b3cd1fcbce6d81756d0d573590065d4a42f212a9356 |
||||||
|
size 19294 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:f5e5e9e2086688abc765e0c13d6621b7648d0a5fe41446af777f9f192fbaeb29 |
||||||
|
size 2473 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:f9696bd0aea94f596a6ad7083b72e7fc30c31b0da16d5731225a31d1c2c4ee11 |
||||||
|
size 3644 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:4a97a24653ef009fb968102d4d4a41fe2bde9efea9d0d9cfffe1abb14b4cf592 |
||||||
|
size 3611 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:c4212faea9f2a2f7ba343cc02987fd96deffe8e23dee35e2c0a088841abffecb |
||||||
|
size 2856 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:450ea0eab5a4fd081d1face3cb4991cbdae8a05721493e5f8a099be208b2515b |
||||||
|
size 4283 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:be988dcc7301ebd693f1485cbe60d22d7cecc2d9fb6d986eadfded77678ff74c |
||||||
|
size 3567 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:c5391efa3017a3fd4d90b73f661e43f0b0664052520f4f983b38766376521d7b |
||||||
|
size 5899 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:7f3f0c545553e4cef2115043f5ba58f5c856889c215fe123ed95d46dae14dfa5 |
||||||
|
size 2465 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:e9b32a750b627e4a621ab3e9a4cefe64b3a367da091b8757450791e8d9361c2b |
||||||
|
size 6591 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:22a41bbe93e99cca59fe8d1617fb27b336cd2c4f2d094b4eea95f98e3470fce3 |
||||||
|
size 2069 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:4ea8ae429fee870ac8c605fe28adf604d7dd22109360f7c089471ec9f07f754d |
||||||
|
size 6129 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:bdf4336095870dba4e5fdb89a7867b26b61a879cf4c0631dab857ebbee79ff9f |
||||||
|
size 2422 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:a66c39280771660524a50e359e2adc6f35df35a11d6cf7d922740e2d3b34e04a |
||||||
|
size 6250 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:6f3b444f0b237c105a7aaef90ba2e09609e5d06a5a278c6cc5a6b8a1aa6c9752 |
||||||
|
size 6538 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:132f3c042ffc2a056405a6857a66ec6bfe40fad7edfc3229ef6b833407a0bfe3 |
||||||
|
size 2964 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:ad6a1d63f4059cd6dd9c18eae4afd6a449c6f1ae4b03a1b697af79708612b9e1 |
||||||
|
size 8178 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:d163c60813069859c1c8ea94e07f7769f35b406147abc342d3dbb9448cfb8dc9 |
||||||
|
size 12557 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:3df305f973face711a307384a310bbe3d74afbd06fc263e4fe78e6886244df73 |
||||||
|
size 2256 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:23ca4ea43fe89ef066c42c0524b21bb2f3857bda386f8df8109f69cb25f13cc4 |
||||||
|
size 6202 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:382c7fa2cfdcb3efdf1631ba6ba1f97bb03297c102cf6e629c3db8519e946a0e |
||||||
|
size 8547 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:32aa6915cea28c1a7b1bd360d0dc7d4981ace423a126c8708718d5058893289d |
||||||
|
size 7185 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:8c4004636f31648f98050e637daa231c70fcf4b64da326548b4f50dccba5ceb1 |
||||||
|
size 1889 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:e00ab4b318526fc2b340ca69cafcd5e5071810f63471a0fe89064ee18e1afafb |
||||||
|
size 24177 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:926abbe8a8ab28df17746ca5bf60440b799b391be172c406e0628cdef3788909 |
||||||
|
size 2316 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:1e08ea0a57a0cede5c2860dd447d5452226b61de22dc778e6e6c602ee940ad20 |
||||||
|
size 11537 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:1b2af935bab05d478ed103138e5a0b5ddce85a270de2eef73c474155a93f612c |
||||||
|
size 3453 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:c18a537b231ec1228cc9d4ca3c8e0d81aad0311bafc2d9f8081d9ad10671c935 |
||||||
|
size 7801 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:61798dbd369cfefd3a90376818f628107bff9131d95cc8b444a924c28cb06115 |
||||||
|
size 3889 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:5aa3120214f9fa79436f9b5ed4db050a5974d774dd99bd0e9bba13f70f04be6d |
||||||
|
size 2903 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:b1e1908455d9810980fb5c9de6726c33a327ff71976d08f696860cd506087c6c |
||||||
|
size 44814 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:ea6f83ec5aa4ae69d95a991f0a9c8912b7f8e1c23db1d73be934c80e086f783a |
||||||
|
size 4557 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:0848d4218ef47604f36f365d1ee83727f2a03494c53a58475803b5f36437c5e6 |
||||||
|
size 16109 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:09649e6b235a6b2af50eb7e7644c38fd5132d16e4b63351affd7adf5009f06ad |
||||||
|
size 3551 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:89dc6a1b5e09267704dc07ed986fb82b5057e51a4e7f0bf801cc204dedf9fb4e |
||||||
|
size 4475 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:6de67e7d53ebbffefe2c330bded67425df97d8bb5d79717d0227e3b6fb1899f9 |
||||||
|
size 2790 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:c55f283766b3a7746306f6599c2094630a6d55c51ca7d3e84d0d3fb099bd57a0 |
||||||
|
size 986208 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:371efa44e737c438c7524139d773d3a2c2481704a5c38c6766fd24f8449ae91c |
||||||
|
size 1084 |
||||||
@ -0,0 +1 @@ |
|||||||
|
libczmq.so.4.0.2 |
||||||
@ -0,0 +1 @@ |
|||||||
|
libczmq.so.4.0.2 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:bde558fe588d8bd4b8016ad4637a7bc683a6957085b2a3528cd681354504b5ab |
||||||
|
size 563416 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:a9ef32858d9dd32d623f448d68901525ac6971a5a790b70d61555b1a8def4eca |
||||||
|
size 5319544 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:6a9380180adcdbf42c77910639d622b44f716e905bc26d25b00d46e1f72b4ea6 |
||||||
|
size 965 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:573977c92c2d4ac52c3df775a216276b1fd050d3abd03906d2b852879c41ce85 |
||||||
|
size 946 |
||||||
@ -0,0 +1 @@ |
|||||||
|
libzmq.so.5.1.2 |
||||||
@ -0,0 +1 @@ |
|||||||
|
libzmq.so.5.1.2 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:89878c51741e047a157fbcb223d57c4ca76441796a53540e86ef9c811c71bc45 |
||||||
|
size 1335280 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:6f17e5e2b5e61fd0efb58d14572bf9eb11ea017ca5e05088422f79bf4548a402 |
||||||
|
size 946 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1 |
||||||
|
oid sha256:77a40af70e122f5db9a4d043811921ff7105fc3bf66d445d195b3a9289f16c11 |
||||||
|
size 220 |
||||||
@ -0,0 +1,25 @@ |
|||||||
|
# distutils: language = c++ |
||||||
|
from libcpp.vector cimport vector |
||||||
|
from libcpp.string cimport string |
||||||
|
from libcpp cimport bool |
||||||
|
|
||||||
|
cdef struct can_frame: |
||||||
|
long address |
||||||
|
string dat |
||||||
|
long busTime |
||||||
|
long src |
||||||
|
|
||||||
|
cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan) |
||||||
|
|
||||||
|
def can_list_to_can_capnp(can_msgs, msgtype='can'): |
||||||
|
cdef vector[can_frame] can_list |
||||||
|
cdef can_frame f |
||||||
|
for can_msg in can_msgs: |
||||||
|
f.address = can_msg[0] |
||||||
|
f.busTime = can_msg[1] |
||||||
|
f.dat = can_msg[2] |
||||||
|
f.src = can_msg[3] |
||||||
|
can_list.push_back(f) |
||||||
|
cdef string out |
||||||
|
can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan') |
||||||
|
return out |
||||||
@ -0,0 +1,25 @@ |
|||||||
|
import subprocess |
||||||
|
from distutils.core import setup, Extension |
||||||
|
from Cython.Build import cythonize |
||||||
|
|
||||||
|
PHONELIBS = '../../phonelibs' |
||||||
|
|
||||||
|
ARCH = subprocess.check_output(["uname", "-m"]).rstrip() |
||||||
|
ARCH_DIR = 'x64' if ARCH == "x86_64" else 'aarch64' |
||||||
|
|
||||||
|
setup(name='Boardd API Implementation', |
||||||
|
ext_modules=cythonize( |
||||||
|
Extension( |
||||||
|
"boardd_api_impl", |
||||||
|
libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libcapnp.a', ':libkj.a'], |
||||||
|
library_dirs=[ |
||||||
|
'./', |
||||||
|
PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/', |
||||||
|
PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/' |
||||||
|
], |
||||||
|
sources=['boardd_api_impl.pyx'], |
||||||
|
language="c++", |
||||||
|
extra_compile_args=["-std=c++11"], |
||||||
|
) |
||||||
|
) |
||||||
|
) |
||||||
@ -0,0 +1,36 @@ |
|||||||
|
#include <vector> |
||||||
|
#include <tuple> |
||||||
|
#include <string> |
||||||
|
#include "common/timing.h" |
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
#include "cereal/gen/cpp/car.capnp.h" |
||||||
|
|
||||||
|
typedef struct { |
||||||
|
long address; |
||||||
|
std::string dat; |
||||||
|
long busTime; |
||||||
|
long src; |
||||||
|
} can_frame; |
||||||
|
|
||||||
|
extern "C" { |
||||||
|
|
||||||
|
void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan) { |
||||||
|
capnp::MallocMessageBuilder msg; |
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
|
||||||
|
auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size()); |
||||||
|
int i = 0; |
||||||
|
for (auto it = can_list.begin(); it != can_list.end(); it++, i++) { |
||||||
|
canData[i].setAddress(it->address); |
||||||
|
canData[i].setBusTime(it->busTime); |
||||||
|
canData[i].setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size())); |
||||||
|
canData[i].setSrc(it->src); |
||||||
|
} |
||||||
|
auto words = capnp::messageToFlatArray(msg); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
out.append((const char *)bytes.begin(), bytes.size()); |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,247 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
|
||||||
|
# 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 zmq |
||||||
|
import time |
||||||
|
|
||||||
|
import selfdrive.messaging as messaging |
||||||
|
from common.realtime import Ratekeeper |
||||||
|
from selfdrive.services import service_list |
||||||
|
from selfdrive.swaglog import cloudlog |
||||||
|
from selfdrive.boardd.boardd import can_capnp_to_can_list |
||||||
|
|
||||||
|
# USB is optional |
||||||
|
try: |
||||||
|
import usb1 |
||||||
|
from usb1 import USBErrorIO, USBErrorOverflow #pylint: disable=no-name-in-module |
||||||
|
except Exception: |
||||||
|
pass |
||||||
|
|
||||||
|
SAFETY_NOOUTPUT = 0 |
||||||
|
SAFETY_HONDA = 1 |
||||||
|
SAFETY_TOYOTA = 2 |
||||||
|
SAFETY_CHRYSLER = 9 |
||||||
|
SAFETY_TOYOTA_NOLIMITS = 0x1336 |
||||||
|
SAFETY_ALLOUTPUT = 0x1337 |
||||||
|
|
||||||
|
# *** serialization functions *** |
||||||
|
def can_list_to_can_capnp(can_msgs, msgtype='can'): |
||||||
|
dat = messaging.new_message() |
||||||
|
dat.init(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 = str(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, 0x10) |
||||||
|
break |
||||||
|
except (USBErrorIO, USBErrorOverflow): |
||||||
|
cloudlog.exception("CAN: BAD HEALTH, RETRYING") |
||||||
|
v, i, started = struct.unpack("IIB", dat[0:9]) |
||||||
|
# TODO: units |
||||||
|
return {"voltage": v, "current": i, "started": bool(started)} |
||||||
|
|
||||||
|
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, '\x00') |
||||||
|
snds.append(snd) |
||||||
|
while 1: |
||||||
|
try: |
||||||
|
handle.bulkWrite(3, ''.join(snds)) |
||||||
|
break |
||||||
|
except (USBErrorIO, USBErrorOverflow): |
||||||
|
cloudlog.exception("CAN: BAD SEND MANY, RETRYING") |
||||||
|
|
||||||
|
def can_recv(): |
||||||
|
dat = "" |
||||||
|
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, SAFETY_ALLOUTPUT, 0, b'') |
||||||
|
|
||||||
|
if handle is None: |
||||||
|
cloudlog.warn("CAN NOT FOUND") |
||||||
|
exit(-1) |
||||||
|
|
||||||
|
cloudlog.info("got handle") |
||||||
|
cloudlog.info("can init done") |
||||||
|
|
||||||
|
def boardd_mock_loop(): |
||||||
|
context = zmq.Context() |
||||||
|
can_init() |
||||||
|
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'') |
||||||
|
|
||||||
|
logcan = messaging.sub_sock(context, service_list['can'].port) |
||||||
|
sendcan = messaging.pub_sock(context, service_list['sendcan'].port) |
||||||
|
|
||||||
|
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 = filter(lambda x: x[-1] <= 1, snd) |
||||||
|
can_send_many(snd) |
||||||
|
|
||||||
|
# recv @ 100hz |
||||||
|
can_msgs = can_recv() |
||||||
|
print("sent %d got %d" % (len(snd), len(can_msgs))) |
||||||
|
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=200): |
||||||
|
rk = Ratekeeper(rate) |
||||||
|
context = zmq.Context() |
||||||
|
|
||||||
|
can_init() |
||||||
|
|
||||||
|
# *** publishes can and health |
||||||
|
logcan = messaging.pub_sock(context, service_list['can'].port) |
||||||
|
health_sock = messaging.pub_sock(context, service_list['health'].port) |
||||||
|
|
||||||
|
# *** subscribes to can send |
||||||
|
sendcan = messaging.sub_sock(context, service_list['sendcan'].port) |
||||||
|
|
||||||
|
# drain sendcan to delete any stale messages from previous runs |
||||||
|
messaging.drain_sock(sendcan) |
||||||
|
|
||||||
|
while 1: |
||||||
|
# health packet @ 1hz |
||||||
|
if (rk.frame%rate) == 0: |
||||||
|
health = can_health() |
||||||
|
msg = messaging.new_message() |
||||||
|
msg.init('health') |
||||||
|
|
||||||
|
# store the health to be logged |
||||||
|
msg.health.voltage = health['voltage'] |
||||||
|
msg.health.current = health['current'] |
||||||
|
msg.health.started = health['started'] |
||||||
|
msg.health.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=200, address="192.168.2.251"): |
||||||
|
rk = Ratekeeper(rate) |
||||||
|
context = zmq.Context() |
||||||
|
|
||||||
|
can_init() |
||||||
|
|
||||||
|
# *** subscribes can |
||||||
|
logcan = messaging.sub_sock(context, service_list['can'].port, addr=address) |
||||||
|
# *** publishes to can send |
||||||
|
sendcan = messaging.pub_sock(context, service_list['sendcan'].port) |
||||||
|
|
||||||
|
# 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(gctx=None): |
||||||
|
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() |
||||||
@ -0,0 +1,19 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
import time |
||||||
|
import random |
||||||
|
|
||||||
|
from boardd_old import can_init, can_recv, can_send_many, can_health |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
can_init() |
||||||
|
while 1: |
||||||
|
c = random.randint(0, 3) |
||||||
|
if c == 0: |
||||||
|
print can_recv() |
||||||
|
elif c == 1: |
||||||
|
print can_health() |
||||||
|
elif c == 2: |
||||||
|
many = [[0x123, 0, "abcdef", 0]] * random.randint(1, 10) |
||||||
|
can_send_many(many) |
||||||
|
elif c == 3: |
||||||
|
time.sleep(random.randint(0, 100) / 1000.0) |
||||||
@ -0,0 +1,76 @@ |
|||||||
|
import random |
||||||
|
import numpy as np |
||||||
|
|
||||||
|
import 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 j in xrange(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 xrange(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 xrange(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 xrange(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, cnt = generate_random_can_data_list() |
||||||
|
recursions = 1000 |
||||||
|
|
||||||
|
n1 = sec_since_boot() |
||||||
|
for i in xrange(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 i in xrange(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() |
||||||
@ -0,0 +1,51 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
"""Run boardd with the BOARDD_LOOPBACK envvar before running this test.""" |
||||||
|
|
||||||
|
import os |
||||||
|
import random |
||||||
|
import zmq |
||||||
|
import time |
||||||
|
|
||||||
|
from selfdrive.boardd.boardd import can_list_to_can_capnp |
||||||
|
from selfdrive.messaging import drain_sock, pub_sock, sub_sock |
||||||
|
from selfdrive.services import service_list |
||||||
|
|
||||||
|
def get_test_string(): |
||||||
|
return b"test"+os.urandom(10) |
||||||
|
|
||||||
|
BUS = 0 |
||||||
|
|
||||||
|
def main(): |
||||||
|
context = zmq.Context() |
||||||
|
|
||||||
|
rcv = sub_sock(context, service_list['can'].port) # port 8006 |
||||||
|
snd = pub_sock(context, service_list['sendcan'].port) # port 8017 |
||||||
|
time.sleep(0.3) # wait to bind before send/recv |
||||||
|
|
||||||
|
for i in range(10): |
||||||
|
print("Loop %d" % i) |
||||||
|
at = random.randint(1024, 2000) |
||||||
|
st = get_test_string()[0:8] |
||||||
|
snd.send(can_list_to_can_capnp([[at, 0, st, 0]], msgtype='sendcan').to_bytes()) |
||||||
|
time.sleep(0.1) |
||||||
|
res = drain_sock(rcv, True) |
||||||
|
assert len(res) == 1 |
||||||
|
|
||||||
|
res = res[0].can |
||||||
|
assert len(res) == 2 |
||||||
|
|
||||||
|
msg0, msg1 = res |
||||||
|
|
||||||
|
assert msg0.dat == st |
||||||
|
assert msg1.dat == st |
||||||
|
|
||||||
|
assert msg0.address == at |
||||||
|
assert msg1.address == at |
||||||
|
|
||||||
|
assert msg0.src == 0x80 | BUS |
||||||
|
assert msg1.src == BUS |
||||||
|
|
||||||
|
print("Success") |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
main() |
||||||
@ -1,68 +1,9 @@ |
|||||||
import six |
# pylint: skip-file |
||||||
import struct |
import os |
||||||
from selfdrive.can.libdbc_py import libdbc, ffi |
import subprocess |
||||||
|
|
||||||
|
can_dir = os.path.dirname(os.path.abspath(__file__)) |
||||||
|
subprocess.check_call(["make", "packer_impl.so"], cwd=can_dir) |
||||||
|
|
||||||
class CANPacker(object): |
from selfdrive.can.packer_impl import CANPacker |
||||||
def __init__(self, dbc_name): |
assert CANPacker |
||||||
self.packer = libdbc.canpack_init(dbc_name) |
|
||||||
self.dbc = libdbc.dbc_lookup(dbc_name) |
|
||||||
self.sig_names = {} |
|
||||||
self.name_to_address_and_size = {} |
|
||||||
|
|
||||||
num_msgs = self.dbc[0].num_msgs |
|
||||||
for i in range(num_msgs): |
|
||||||
msg = self.dbc[0].msgs[i] |
|
||||||
|
|
||||||
name = ffi.string(msg.name) |
|
||||||
address = msg.address |
|
||||||
self.name_to_address_and_size[name] = (address, msg.size) |
|
||||||
self.name_to_address_and_size[address] = (address, msg.size) |
|
||||||
|
|
||||||
def pack(self, addr, values, counter): |
|
||||||
values_thing = [] |
|
||||||
for name, value in six.iteritems(values): |
|
||||||
if name not in self.sig_names: |
|
||||||
self.sig_names[name] = ffi.new("char[]", name) |
|
||||||
|
|
||||||
values_thing.append({ |
|
||||||
'name': self.sig_names[name], |
|
||||||
'value': value |
|
||||||
}) |
|
||||||
|
|
||||||
values_c = ffi.new("SignalPackValue[]", values_thing) |
|
||||||
|
|
||||||
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c, counter) |
|
||||||
|
|
||||||
def pack_bytes(self, addr, values, counter=-1): |
|
||||||
addr, size = self.name_to_address_and_size[addr] |
|
||||||
|
|
||||||
val = self.pack(addr, values, counter) |
|
||||||
r = struct.pack(">Q", val) |
|
||||||
return addr, r[:size] |
|
||||||
|
|
||||||
def make_can_msg(self, addr, bus, values, counter=-1): |
|
||||||
addr, msg = self.pack_bytes(addr, values, counter) |
|
||||||
return [addr, 0, msg, bus] |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
## little endian test |
|
||||||
cp = CANPacker("hyundai_santa_fe_2019_ccan") |
|
||||||
s = cp.pack_bytes(0x340, { |
|
||||||
"CR_Lkas_StrToqReq": -0.06, |
|
||||||
#"CF_Lkas_FcwBasReq": 1, |
|
||||||
"CF_Lkas_MsgCount": 7, |
|
||||||
"CF_Lkas_HbaSysState": 0, |
|
||||||
#"CF_Lkas_Chksum": 3, |
|
||||||
}) |
|
||||||
s = cp.pack_bytes(0x340, { |
|
||||||
"CF_Lkas_MsgCount": 1, |
|
||||||
}) |
|
||||||
# big endian test |
|
||||||
#cp = CANPacker("honda_civic_touring_2016_can_generated") |
|
||||||
#s = cp.pack_bytes(0xe4, { |
|
||||||
# "STEER_TORQUE": -2, |
|
||||||
#}) |
|
||||||
print([hex(ord(v)) for v in s[1]]) |
|
||||||
print(s[1].encode("hex")) |
|
||||||
|
|||||||
@ -0,0 +1,111 @@ |
|||||||
|
# distutils: language = c++ |
||||||
|
from libc.stdint cimport uint32_t, uint64_t |
||||||
|
from libcpp.vector cimport vector |
||||||
|
from libcpp.map cimport map |
||||||
|
from libcpp.string cimport string |
||||||
|
from libcpp cimport bool |
||||||
|
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY |
||||||
|
import os |
||||||
|
import subprocess |
||||||
|
|
||||||
|
cdef struct SignalPackValue: |
||||||
|
const char* name |
||||||
|
double value |
||||||
|
|
||||||
|
ctypedef enum SignalType: |
||||||
|
DEFAULT, |
||||||
|
HONDA_CHECKSUM, |
||||||
|
HONDA_COUNTER, |
||||||
|
TOYOTA_CHECKSUM, |
||||||
|
PEDAL_CHECKSUM, |
||||||
|
PEDAL_COUNTER |
||||||
|
|
||||||
|
cdef struct Signal: |
||||||
|
const char* name |
||||||
|
int b1, b2, bo |
||||||
|
bool is_signed |
||||||
|
double factor, offset |
||||||
|
SignalType type |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
cdef struct Msg: |
||||||
|
const char* name |
||||||
|
uint32_t address |
||||||
|
unsigned int size |
||||||
|
size_t num_sigs |
||||||
|
const Signal *sigs |
||||||
|
|
||||||
|
cdef struct Val: |
||||||
|
const char* name |
||||||
|
uint32_t address |
||||||
|
const char* def_val |
||||||
|
const Signal *sigs |
||||||
|
|
||||||
|
cdef struct DBC: |
||||||
|
const char* name |
||||||
|
size_t num_msgs |
||||||
|
const Msg *msgs |
||||||
|
const Val *vals |
||||||
|
size_t num_vals |
||||||
|
|
||||||
|
ctypedef void * (*canpack_init_func)(const char* dbc_name) |
||||||
|
ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, const vector[SignalPackValue] &signals, int counter) |
||||||
|
ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name) |
||||||
|
|
||||||
|
|
||||||
|
cdef class CANPacker(object): |
||||||
|
cdef void *packer |
||||||
|
cdef const DBC *dbc |
||||||
|
cdef map[string, (int, int)] name_to_address_and_size |
||||||
|
cdef map[int, int] address_to_size |
||||||
|
cdef canpack_init_func canpack_init |
||||||
|
cdef canpack_pack_vector_func canpack_pack_vector |
||||||
|
cdef dbc_lookup_func dbc_lookup |
||||||
|
|
||||||
|
def __init__(self, dbc_name): |
||||||
|
can_dir = os.path.dirname(os.path.abspath(__file__)) |
||||||
|
libdbc_fn = os.path.join(can_dir, "libdbc.so") |
||||||
|
subprocess.check_call(["make"], cwd=can_dir) |
||||||
|
cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY) |
||||||
|
self.canpack_init = <canpack_init_func>dlsym(libdbc, 'canpack_init') |
||||||
|
self.canpack_pack_vector = <canpack_pack_vector_func>dlsym(libdbc, 'canpack_pack_vector') |
||||||
|
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup') |
||||||
|
self.packer = self.canpack_init(dbc_name) |
||||||
|
self.dbc = self.dbc_lookup(dbc_name) |
||||||
|
num_msgs = self.dbc[0].num_msgs |
||||||
|
for i in range(num_msgs): |
||||||
|
msg = self.dbc[0].msgs[i] |
||||||
|
self.name_to_address_and_size[string(msg.name)] = (msg.address, msg.size) |
||||||
|
self.address_to_size[msg.address] = msg.size |
||||||
|
|
||||||
|
cdef uint64_t pack(self, addr, values, counter): |
||||||
|
cdef vector[SignalPackValue] values_thing |
||||||
|
cdef SignalPackValue spv |
||||||
|
for name, value in values.iteritems(): |
||||||
|
spv.name = name |
||||||
|
spv.value = value |
||||||
|
values_thing.push_back(spv) |
||||||
|
|
||||||
|
return self.canpack_pack_vector(self.packer, addr, values_thing, counter) |
||||||
|
|
||||||
|
cdef inline uint64_t ReverseBytes(self, uint64_t x): |
||||||
|
return (((x & 0xff00000000000000ull) >> 56) | |
||||||
|
((x & 0x00ff000000000000ull) >> 40) | |
||||||
|
((x & 0x0000ff0000000000ull) >> 24) | |
||||||
|
((x & 0x000000ff00000000ull) >> 8) | |
||||||
|
((x & 0x00000000ff000000ull) << 8) | |
||||||
|
((x & 0x0000000000ff0000ull) << 24) | |
||||||
|
((x & 0x000000000000ff00ull) << 40) | |
||||||
|
((x & 0x00000000000000ffull) << 56)) |
||||||
|
|
||||||
|
cpdef make_can_msg(self, name_or_addr, bus, values, counter=-1): |
||||||
|
cdef int addr, size |
||||||
|
if type(name_or_addr) == int: |
||||||
|
addr = name_or_addr |
||||||
|
size = self.address_to_size[name_or_addr] |
||||||
|
else: |
||||||
|
addr, size = self.name_to_address_and_size[name_or_addr] |
||||||
|
cdef uint64_t val = self.pack(addr, values, counter) |
||||||
|
val = self.ReverseBytes(val) |
||||||
|
return [addr, 0, (<char *>&val)[:size], bus] |
||||||
@ -0,0 +1,5 @@ |
|||||||
|
from distutils.core import setup, Extension |
||||||
|
from Cython.Build import cythonize |
||||||
|
|
||||||
|
setup(name='CAN Packer API Implementation', |
||||||
|
ext_modules=cythonize(Extension("packer_impl", ["packer_impl.pyx"], language="c++", extra_compile_args=["-std=c++11"]))) |
||||||
@ -0,0 +1,67 @@ |
|||||||
|
import struct |
||||||
|
from selfdrive.can.libdbc_py import libdbc, ffi |
||||||
|
|
||||||
|
|
||||||
|
class CANPacker(object): |
||||||
|
def __init__(self, dbc_name): |
||||||
|
self.packer = libdbc.canpack_init(dbc_name) |
||||||
|
self.dbc = libdbc.dbc_lookup(dbc_name) |
||||||
|
self.sig_names = {} |
||||||
|
self.name_to_address_and_size = {} |
||||||
|
|
||||||
|
num_msgs = self.dbc[0].num_msgs |
||||||
|
for i in range(num_msgs): |
||||||
|
msg = self.dbc[0].msgs[i] |
||||||
|
|
||||||
|
name = ffi.string(msg.name) |
||||||
|
address = msg.address |
||||||
|
self.name_to_address_and_size[name] = (address, msg.size) |
||||||
|
self.name_to_address_and_size[address] = (address, msg.size) |
||||||
|
|
||||||
|
def pack(self, addr, values, counter): |
||||||
|
values_thing = [] |
||||||
|
for name, value in values.iteritems(): |
||||||
|
if name not in self.sig_names: |
||||||
|
self.sig_names[name] = ffi.new("char[]", name) |
||||||
|
|
||||||
|
values_thing.append({ |
||||||
|
'name': self.sig_names[name], |
||||||
|
'value': value |
||||||
|
}) |
||||||
|
|
||||||
|
values_c = ffi.new("SignalPackValue[]", values_thing) |
||||||
|
|
||||||
|
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c, counter) |
||||||
|
|
||||||
|
def pack_bytes(self, addr, values, counter=-1): |
||||||
|
addr, size = self.name_to_address_and_size[addr] |
||||||
|
|
||||||
|
val = self.pack(addr, values, counter) |
||||||
|
r = struct.pack(">Q", val) |
||||||
|
return addr, r[:size] |
||||||
|
|
||||||
|
def make_can_msg(self, addr, bus, values, counter=-1): |
||||||
|
addr, msg = self.pack_bytes(addr, values, counter) |
||||||
|
return [addr, 0, msg, bus] |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
## little endian test |
||||||
|
cp = CANPacker("hyundai_santa_fe_2019_ccan") |
||||||
|
s = cp.pack_bytes(0x340, { |
||||||
|
"CR_Lkas_StrToqReq": -0.06, |
||||||
|
#"CF_Lkas_FcwBasReq": 1, |
||||||
|
"CF_Lkas_MsgCount": 7, |
||||||
|
"CF_Lkas_HbaSysState": 0, |
||||||
|
#"CF_Lkas_Chksum": 3, |
||||||
|
}) |
||||||
|
s = cp.pack_bytes(0x340, { |
||||||
|
"CF_Lkas_MsgCount": 1, |
||||||
|
}) |
||||||
|
# big endian test |
||||||
|
#cp = CANPacker("honda_civic_touring_2016_can_generated") |
||||||
|
#s = cp.pack_bytes(0xe4, { |
||||||
|
# "STEER_TORQUE": -2, |
||||||
|
#}) |
||||||
|
print([hex(ord(v)) for v in s[1]]) |
||||||
|
print(s[1].encode("hex")) |
||||||
@ -0,0 +1,35 @@ |
|||||||
|
import unittest |
||||||
|
import random |
||||||
|
|
||||||
|
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld |
||||||
|
from selfdrive.can.packer import CANPacker |
||||||
|
import selfdrive.car.chrysler.chryslercan as chryslercan |
||||||
|
|
||||||
|
|
||||||
|
class TestPackerMethods(unittest.TestCase): |
||||||
|
def setUp(self): |
||||||
|
self.chrysler_cp_old = CANPackerOld("chrysler_pacifica_2017_hybrid") |
||||||
|
self.chrysler_cp = CANPacker("chrysler_pacifica_2017_hybrid") |
||||||
|
|
||||||
|
def test_correctness(self): |
||||||
|
# Test all commands, randomize the params. |
||||||
|
for _ in xrange(1000): |
||||||
|
gear = ('drive', 'reverse', 'low')[random.randint(0, 3) % 3] |
||||||
|
lkas_active = (random.randint(0, 2) % 2 == 0) |
||||||
|
hud_alert = random.randint(0, 6) |
||||||
|
hud_count = random.randint(0, 65536) |
||||||
|
lkas_car_model = random.randint(0, 65536) |
||||||
|
m_old = chryslercan.create_lkas_hud(self.chrysler_cp_old, gear, lkas_active, hud_alert, hud_count, lkas_car_model) |
||||||
|
m = chryslercan.create_lkas_hud(self.chrysler_cp, gear, lkas_active, hud_alert, hud_count, lkas_car_model) |
||||||
|
self.assertEqual(m_old, m) |
||||||
|
|
||||||
|
apply_steer = (random.randint(0, 2) % 2 == 0) |
||||||
|
moving_fast = (random.randint(0, 2) % 2 == 0) |
||||||
|
frame = random.randint(0, 65536) |
||||||
|
m_old = chryslercan.create_lkas_command(self.chrysler_cp_old, apply_steer, moving_fast, frame) |
||||||
|
m = chryslercan.create_lkas_command(self.chrysler_cp, apply_steer, moving_fast, frame) |
||||||
|
self.assertEqual(m_old, m) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
unittest.main() |
||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue