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 |
||||
oid sha256:2d45eb035af4adeaf31fe96ab36fc310962cc8c375319ac2a5e707b2cdc03097 |
||||
size 2588994 |
||||
oid sha256:61fc21ef8cec4ca15be05b1b5fdb9c4d95f3a78be891a36d84bd2dd89441e28e |
||||
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: |
||||
# 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 |
||||
from simple_kalman_impl import KF1D as KF1D |
||||
# Silence pyflakes |
||||
assert KF1D |
||||
|
||||
@ -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 |
||||
import struct |
||||
from selfdrive.can.libdbc_py import libdbc, ffi |
||||
# pylint: skip-file |
||||
import os |
||||
import subprocess |
||||
|
||||
can_dir = os.path.dirname(os.path.abspath(__file__)) |
||||
subprocess.check_call(["make", "packer_impl.so"], cwd=can_dir) |
||||
|
||||
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 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")) |
||||
from selfdrive.can.packer_impl import CANPacker |
||||
assert CANPacker |
||||
|
||||
@ -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