mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-15 15:22:08 +08:00
Vehicle Researcher
@@ -13,6 +13,11 @@ script:
|
||||
tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda")'
|
||||
- docker run
|
||||
tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda"); exit $(($? & 3))'
|
||||
- docker run tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover common'
|
||||
- docker run tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/can'
|
||||
- docker run tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/boardd'
|
||||
- docker run tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/controls'
|
||||
- docker run tmppilot /bin/sh -c 'cd /tmp/openpilot/ && python -m unittest discover selfdrive/loggerd'
|
||||
- docker run
|
||||
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out
|
||||
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
|
||||
|
||||
+14
-1
@@ -10,9 +10,22 @@ Most open source development activity is coordinated through our [Discord](https
|
||||
* Make sure you have a [GitHub account](https://github.com/signup/free)
|
||||
* Fork [our repositories](https://github.com/commaai) on GitHub
|
||||
|
||||
## Testing
|
||||
|
||||
### Local Testing
|
||||
|
||||
You can test your changes on your machine by running `run_docker_tests.sh`. This will run some automated tests in docker against your code.
|
||||
|
||||
### Automated Testing
|
||||
|
||||
All PRs are automatically checked by travis. Check out `.travis.yml` for what travis runs. Any new tests sould be added to travis.
|
||||
|
||||
### Code Style and Linting
|
||||
|
||||
Code is automatically check for style by travis as part of the automated tests. You can also run these yourself by running `check_code_quality.sh`.
|
||||
|
||||
## Car Ports (openpilot)
|
||||
|
||||
We've released a [Model Port guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for porting to Toyota/Lexus models.
|
||||
|
||||
If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty. See our bounties at [comma.ai/bounties.html](https://comma.ai/bounties.html)
|
||||
|
||||
|
||||
@@ -2,17 +2,15 @@ FROM ubuntu:16.04
|
||||
ENV PYTHONUNBUFFERED 1
|
||||
|
||||
RUN apt-get update && apt-get install -y \
|
||||
autoconf \
|
||||
build-essential \
|
||||
clang \
|
||||
vim \
|
||||
screen \
|
||||
wget \
|
||||
bzip2 \
|
||||
git \
|
||||
libglib2.0-0 \
|
||||
libtool \
|
||||
python-pip \
|
||||
capnproto \
|
||||
libcapnp-dev \
|
||||
libzmq5-dev \
|
||||
libffi-dev \
|
||||
libusb-1.0-0 \
|
||||
@@ -21,6 +19,9 @@ RUN apt-get update && apt-get install -y \
|
||||
ocl-icd-opencl-dev \
|
||||
opencl-headers
|
||||
|
||||
COPY phonelibs/install_capnp.sh /tmp/install_capnp.sh
|
||||
RUN /tmp/install_capnp.sh
|
||||
|
||||
RUN pip install --upgrade pip==18.0
|
||||
RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib==2.1.2
|
||||
|
||||
|
||||
@@ -92,10 +92,13 @@ Supported Cars
|
||||
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
|
||||
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
|
||||
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-18<sup>4</sup> | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Toyota | Highlander 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Highlander Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
@@ -103,6 +106,7 @@ Supported Cars
|
||||
| Toyota | Prius Prime 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
|
||||
<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
|
||||
@@ -159,6 +163,7 @@ Directory structure
|
||||
├── pyextra # Libraries used on EON
|
||||
└── selfdrive # Code needed to drive the car
|
||||
├── assets # Fonts and images for UI
|
||||
├── athena # Allows communication with the app
|
||||
├── boardd # Daemon to talk to the board
|
||||
├── can # Helpers for parsing CAN messages
|
||||
├── car # Car specific code to read states and control actuators
|
||||
@@ -169,7 +174,6 @@ Directory structure
|
||||
├── logcatd # Android logcat as a service
|
||||
├── loggerd # Logger and uploader of car data
|
||||
├── mapd # Fetches map data and computes next global path
|
||||
├── orbd # Computes ORB features from frames
|
||||
├── proclogd # Logs information from proc
|
||||
├── sensord # IMU / GPS interface code
|
||||
├── test # Car simulator running code through virtual maneuvers
|
||||
|
||||
+14
@@ -1,3 +1,17 @@
|
||||
Version 0.5.12 (2019-05-16)
|
||||
==========================
|
||||
* Improve lateral control for the Prius and Prius Prime
|
||||
* Compress logs before writing to disk
|
||||
* Remove old driving data when storage reaches 90% full
|
||||
* Fix small offset in following distance
|
||||
* Various small CPU optimizations
|
||||
* Improve offroad power consumption: require NEOS Update
|
||||
* Add default speed limits for Estonia thanks to martinl!
|
||||
* Subaru Crosstrek support thanks to martinl!
|
||||
* Toyota Avalon support thanks to njbrown09!
|
||||
* Toyota Rav4 with TSS 2.0 support thansk to wocsor!
|
||||
* Toyota Corolla with TSS 2.0 support thansk to wocsor!
|
||||
|
||||
Version 0.5.11 (2019-04-17)
|
||||
========================
|
||||
* Add support for Subaru
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:2d45eb035af4adeaf31fe96ab36fc310962cc8c375319ac2a5e707b2cdc03097
|
||||
size 2588994
|
||||
oid sha256:61fc21ef8cec4ca15be05b1b5fdb9c4d95f3a78be891a36d84bd2dd89441e28e
|
||||
size 2596571
|
||||
|
||||
+12
-12
@@ -6,24 +6,24 @@ GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
|
||||
JS := gen/js/car.capnp.js gen/js/log.capnp.js
|
||||
|
||||
UNAME_M ?= $(shell uname -m)
|
||||
# only generate C++ for docker tests
|
||||
ifneq ($(OPTEST),1)
|
||||
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
|
||||
|
||||
ifeq ($(UNAME_M),x86_64)
|
||||
ifneq (, $(shell which capnpc-java))
|
||||
GENS += gen/java/Car.java gen/java/Log.java
|
||||
else
|
||||
$(warning capnpc-java not found, skipping java build)
|
||||
endif
|
||||
endif
|
||||
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
|
||||
|
||||
ifeq ($(UNAME_M),x86_64)
|
||||
|
||||
ifneq (, $(shell which capnpc-java))
|
||||
GENS += gen/java/Car.java gen/java/Log.java
|
||||
else
|
||||
$(warning capnpc-java not found, skipping java build)
|
||||
endif
|
||||
|
||||
endif
|
||||
|
||||
|
||||
ifeq ($(UNAME_M),aarch64)
|
||||
CAPNPC=PATH=$(PWD)/../phonelibs/capnp-cpp/aarch64/bin/:$$PATH capnpc
|
||||
CAPNPC=PATH=$(PWD)/../phonelibs/capnp-cpp/aarch64/bin/:$$PATH capnpc
|
||||
else
|
||||
CAPNPC=capnpc
|
||||
CAPNPC=capnpc
|
||||
endif
|
||||
|
||||
.PHONY: all
|
||||
|
||||
+74
-61
@@ -73,6 +73,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
||||
lowBattery @48;
|
||||
invalidGiraffeHonda @49;
|
||||
vehicleModelInvalid @50;
|
||||
controlsFailed @51;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -281,31 +282,83 @@ struct CarControl {
|
||||
|
||||
struct CarParams {
|
||||
carName @0 :Text;
|
||||
radarNameDEPRECATED @1 :Text;
|
||||
carFingerprint @2 :Text;
|
||||
carFingerprint @1 :Text;
|
||||
|
||||
enableSteerDEPRECATED @3 :Bool;
|
||||
enableGasInterceptor @4 :Bool;
|
||||
enableBrakeDEPRECATED @5 :Bool;
|
||||
enableCruise @6 :Bool;
|
||||
enableCamera @26 :Bool;
|
||||
enableDsu @27 :Bool; # driving support unit
|
||||
enableApgs @28 :Bool; # advanced parking guidance system
|
||||
enableGasInterceptor @2 :Bool;
|
||||
enableCruise @3 :Bool;
|
||||
enableCamera @4 :Bool;
|
||||
enableDsu @5 :Bool; # driving support unit
|
||||
enableApgs @6 :Bool; # advanced parking guidance system
|
||||
|
||||
minEnableSpeed @17 :Float32;
|
||||
minSteerSpeed @49 :Float32;
|
||||
safetyModel @18 :Int16;
|
||||
safetyParam @41 :Int16;
|
||||
minEnableSpeed @7 :Float32;
|
||||
minSteerSpeed @8 :Float32;
|
||||
safetyModel @9 :Int16;
|
||||
safetyParam @10 :Int16;
|
||||
|
||||
steerMaxBP @19 :List(Float32);
|
||||
steerMaxV @20 :List(Float32);
|
||||
gasMaxBP @21 :List(Float32);
|
||||
gasMaxV @22 :List(Float32);
|
||||
brakeMaxBP @23 :List(Float32);
|
||||
brakeMaxV @24 :List(Float32);
|
||||
steerMaxBP @11 :List(Float32);
|
||||
steerMaxV @12 :List(Float32);
|
||||
gasMaxBP @13 :List(Float32);
|
||||
gasMaxV @14 :List(Float32);
|
||||
brakeMaxBP @15 :List(Float32);
|
||||
brakeMaxV @16 :List(Float32);
|
||||
|
||||
|
||||
# things about the car in the manual
|
||||
mass @17 :Float32; # [kg] running weight
|
||||
wheelbase @18 :Float32; # [m] distance from rear to front axle
|
||||
centerToFront @19 :Float32; # [m] GC distance to front axle
|
||||
steerRatio @20 :Float32; # [] ratio between front wheels and steering wheel angles
|
||||
steerRatioRear @21 :Float32; # [] rear steering ratio wrt front steering (usually 0)
|
||||
|
||||
# things we can derive
|
||||
rotationalInertia @22 :Float32; # [kg*m2] body rotational inertia
|
||||
tireStiffnessFront @23 :Float32; # [N/rad] front tire coeff of stiff
|
||||
tireStiffnessRear @24 :Float32; # [N/rad] rear tire coeff of stiff
|
||||
|
||||
longitudinalTuning @25 :LongitudinalPIDTuning;
|
||||
lateralTuning :union {
|
||||
pid @26 :LateralPIDTuning;
|
||||
indi @27 :LateralINDITuning;
|
||||
}
|
||||
|
||||
steerLimitAlert @28 :Bool;
|
||||
|
||||
vEgoStopping @29 :Float32; # Speed at which the car goes into stopping state
|
||||
directAccelControl @30 :Bool; # Does the car have direct accel control or just gas/brake
|
||||
stoppingControl @31 :Bool; # Does the car allows full control even at lows speeds when stopping
|
||||
startAccel @32 :Float32; # Required acceleraton to overcome creep braking
|
||||
steerRateCost @33 :Float32; # Lateral MPC cost on steering rate
|
||||
steerControlType @34 :SteerControlType;
|
||||
radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN
|
||||
|
||||
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
|
||||
openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
|
||||
|
||||
struct LateralPIDTuning {
|
||||
kpBP @0 :List(Float32);
|
||||
kpV @1 :List(Float32);
|
||||
kiBP @2 :List(Float32);
|
||||
kiV @3 :List(Float32);
|
||||
kf @4 :Float32;
|
||||
}
|
||||
|
||||
struct LongitudinalPIDTuning {
|
||||
kpBP @0 :List(Float32);
|
||||
kpV @1 :List(Float32);
|
||||
kiBP @2 :List(Float32);
|
||||
kiV @3 :List(Float32);
|
||||
deadzoneBP @4 :List(Float32);
|
||||
deadzoneV @5 :List(Float32);
|
||||
}
|
||||
|
||||
|
||||
struct LateralINDITuning {
|
||||
outerLoopGain @0 :Float32;
|
||||
innerLoopGain @1 :Float32;
|
||||
timeConstant @2 :Float32;
|
||||
actuatorEffectiveness @3 :Float32;
|
||||
}
|
||||
|
||||
longPidDeadzoneBP @32 :List(Float32);
|
||||
longPidDeadzoneV @33 :List(Float32);
|
||||
|
||||
enum SafetyModels {
|
||||
# does NOT match board setting
|
||||
@@ -323,46 +376,6 @@ struct CarParams {
|
||||
subaru @11;
|
||||
}
|
||||
|
||||
# things about the car in the manual
|
||||
mass @7 :Float32; # [kg] running weight
|
||||
wheelbase @8 :Float32; # [m] distance from rear to front axle
|
||||
centerToFront @9 :Float32; # [m] GC distance to front axle
|
||||
steerRatio @10 :Float32; # [] ratio between front wheels and steering wheel angles
|
||||
steerRatioRear @11 :Float32; # [] rear steering ratio wrt front steering (usually 0)
|
||||
|
||||
# things we can derive
|
||||
rotationalInertia @12 :Float32; # [kg*m2] body rotational inertia
|
||||
tireStiffnessFront @13 :Float32; # [N/rad] front tire coeff of stiff
|
||||
tireStiffnessRear @14 :Float32; # [N/rad] rear tire coeff of stiff
|
||||
|
||||
# Kp and Ki for the lateral control
|
||||
steerKpBP @42 :List(Float32);
|
||||
steerKpV @43 :List(Float32);
|
||||
steerKiBP @44 :List(Float32);
|
||||
steerKiV @45 :List(Float32);
|
||||
steerKpDEPRECATED @15 :Float32;
|
||||
steerKiDEPRECATED @16 :Float32;
|
||||
steerKf @25 :Float32;
|
||||
|
||||
# Kp and Ki for the longitudinal control
|
||||
longitudinalKpBP @36 :List(Float32);
|
||||
longitudinalKpV @37 :List(Float32);
|
||||
longitudinalKiBP @38 :List(Float32);
|
||||
longitudinalKiV @39 :List(Float32);
|
||||
|
||||
steerLimitAlert @29 :Bool;
|
||||
|
||||
vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state
|
||||
directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake
|
||||
stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping
|
||||
startAccel @35 :Float32; # Required acceleraton to overcome creep braking
|
||||
steerRateCost @40 :Float32; # Lateral MPC cost on steering rate
|
||||
steerControlType @46 :SteerControlType;
|
||||
radarOffCan @47 :Bool; # True when radar objects aren't visible on CAN
|
||||
|
||||
steerActuatorDelay @48 :Float32; # Steering wheel actuator delay in seconds
|
||||
openpilotLongitudinalControl @50 :Bool; # is openpilot doing the longitudinal control?
|
||||
|
||||
enum SteerControlType {
|
||||
torque @0;
|
||||
angle @1;
|
||||
|
||||
+35
-4
@@ -388,9 +388,9 @@ struct Live100Data {
|
||||
ufAccelCmd @33 :Float32;
|
||||
yActualDEPRECATED @6 :Float32;
|
||||
yDesDEPRECATED @7 :Float32;
|
||||
upSteer @8 :Float32;
|
||||
uiSteer @9 :Float32;
|
||||
ufSteer @34 :Float32;
|
||||
upSteerDEPRECATED @8 :Float32;
|
||||
uiSteerDEPRECATED @9 :Float32;
|
||||
ufSteerDEPRECATED @34 :Float32;
|
||||
aTargetMinDEPRECATED @10 :Float32;
|
||||
aTargetMaxDEPRECATED @11 :Float32;
|
||||
aTarget @35 :Float32;
|
||||
@@ -428,6 +428,11 @@ struct Live100Data {
|
||||
vCurvature @46 :Float32;
|
||||
decelForTurn @47 :Bool;
|
||||
|
||||
lateralControlState :union {
|
||||
indiState @52 :LateralINDIState;
|
||||
pidState @53 :LateralPIDState;
|
||||
}
|
||||
|
||||
enum ControlState {
|
||||
disabled @0;
|
||||
preEnabled @1;
|
||||
@@ -455,6 +460,31 @@ struct Live100Data {
|
||||
full @3; # full screen
|
||||
}
|
||||
|
||||
struct LateralINDIState {
|
||||
active @0 :Bool;
|
||||
steerAngle @1 :Float32;
|
||||
steerRate @2 :Float32;
|
||||
steerAccel @3 :Float32;
|
||||
rateSetPoint @4 :Float32;
|
||||
accelSetPoint @5 :Float32;
|
||||
accelError @6 :Float32;
|
||||
delayedOutput @7 :Float32;
|
||||
delta @8 :Float32;
|
||||
output @9 :Float32;
|
||||
}
|
||||
|
||||
struct LateralPIDState {
|
||||
active @0 :Bool;
|
||||
steerAngle @1 :Float32;
|
||||
steerRate @2 :Float32;
|
||||
angleError @3 :Float32;
|
||||
p @4 :Float32;
|
||||
i @5 :Float32;
|
||||
f @6 :Float32;
|
||||
output @7 :Float32;
|
||||
saturated @8 :Bool;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
struct LiveEventData {
|
||||
@@ -545,7 +575,7 @@ struct LogRotate {
|
||||
struct Plan {
|
||||
mdMonoTime @9 :UInt64;
|
||||
l20MonoTime @10 :UInt64;
|
||||
events @13 :List(Car.CarEvent);
|
||||
eventsDEPRECATED @13 :List(Car.CarEvent);
|
||||
|
||||
# lateral, 3rd order polynomial
|
||||
lateralValidDEPRECATED @0 :Bool;
|
||||
@@ -583,6 +613,7 @@ struct Plan {
|
||||
decelForTurn @22 :Bool;
|
||||
mapValid @25 :Bool;
|
||||
radarValid @28 :Bool;
|
||||
radarCommIssue @30 :Bool;
|
||||
|
||||
processingDelay @29 :Float32;
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -200,3 +200,14 @@ def transform_img(base_img,
|
||||
augmented_rgb[:cyy] = cv2.warpPerspective(base_img, M, (output_size[0], cyy), borderMode=cv2.BORDER_REPLICATE)
|
||||
|
||||
return augmented_rgb
|
||||
|
||||
def yuv_crop(frame, output_size, center=None):
|
||||
# output_size in camera coordinates so u,v
|
||||
# center in array coordinates so row, column
|
||||
rgb = cv2.cvtColor(frame, cv2.COLOR_YUV2RGB_I420)
|
||||
if not center:
|
||||
center = (rgb.shape[0]/2, rgb.shape[1]/2)
|
||||
rgb_crop = rgb[center[0] - output_size[1]/2: center[0] + output_size[1]/2,
|
||||
center[1] - output_size[0]/2: center[1] + output_size[0]/2]
|
||||
return cv2.cvtColor(rgb_crop, cv2.COLOR_RGB2YUV_I420)
|
||||
|
||||
|
||||
@@ -1,8 +1,7 @@
|
||||
import numpy as np
|
||||
|
||||
from common.transformations.camera import eon_focal_length, \
|
||||
vp_from_ke, \
|
||||
get_view_frame_from_road_frame, \
|
||||
vp_from_ke, get_view_frame_from_road_frame, \
|
||||
FULL_FRAME_SIZE
|
||||
|
||||
# segnet
|
||||
@@ -117,6 +116,16 @@ def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model
|
||||
return np.dot(camera_from_model_camera, model_camera_from_model_frame)
|
||||
|
||||
|
||||
def get_camera_frame_from_medmodel_frame(camera_frame_from_road_frame):
|
||||
camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
|
||||
medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
|
||||
|
||||
ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
|
||||
camera_frame_from_medmodel_frame = np.dot(camera_frame_from_ground, ground_from_medmodel_frame)
|
||||
|
||||
return camera_frame_from_medmodel_frame
|
||||
|
||||
|
||||
def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
|
||||
camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
|
||||
bigmodel_frame_from_ground = bigmodel_frame_from_road_frame[:, (0, 1, 3)]
|
||||
|
||||
@@ -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
|
||||
Executable
+3
@@ -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
|
||||
Executable
+3
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:371efa44e737c438c7524139d773d3a2c2481704a5c38c6766fd24f8449ae91c
|
||||
size 1084
|
||||
Symlink
+1
@@ -0,0 +1 @@
|
||||
libczmq.so.4.0.2
|
||||
+1
@@ -0,0 +1 @@
|
||||
libczmq.so.4.0.2
|
||||
Executable
+3
@@ -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
|
||||
Symlink
+1
@@ -0,0 +1 @@
|
||||
libzmq.so.5.1.2
|
||||
+1
@@ -0,0 +1 @@
|
||||
libzmq.so.5.1.2
|
||||
Executable
+3
@@ -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
|
||||
@@ -3,6 +3,12 @@ set -e
|
||||
|
||||
docker build -t tmppilot -f Dockerfile.openpilot .
|
||||
|
||||
docker run --rm \
|
||||
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
|
||||
docker run --rm \
|
||||
tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda")'
|
||||
docker run --rm \
|
||||
tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda"); exit $(($? & 3))'
|
||||
docker run --rm \
|
||||
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out \
|
||||
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
|
||||
|
||||
@@ -6,9 +6,11 @@ import time
|
||||
import threading
|
||||
import traceback
|
||||
import zmq
|
||||
import requests
|
||||
import six.moves.queue
|
||||
from jsonrpc import JSONRPCResponseManager, dispatcher
|
||||
from websocket import create_connection, WebSocketTimeoutException
|
||||
from selfdrive.loggerd.config import ROOT
|
||||
|
||||
import selfdrive.crash as crash
|
||||
import selfdrive.messaging as messaging
|
||||
@@ -71,6 +73,19 @@ def getMessage(service=None, timeout=1000):
|
||||
ret = messaging.recv_one(socket)
|
||||
return ret.to_dict()
|
||||
|
||||
@dispatcher.add_method
|
||||
def listDataDirectory():
|
||||
files = [os.path.relpath(os.path.join(dp, f), ROOT) for dp, dn, fn in os.walk(ROOT) for f in fn]
|
||||
return files
|
||||
|
||||
@dispatcher.add_method
|
||||
def uploadFileToUrl(fn, url, headers):
|
||||
if len(fn) == 0 or fn[0] == '/' or '..' in fn:
|
||||
return 500
|
||||
with open(os.path.join(ROOT, fn), "rb") as f:
|
||||
ret = requests.put(url, data=f, headers=headers, timeout=10)
|
||||
return ret.status_code
|
||||
|
||||
def ws_recv(ws, end_event):
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
|
||||
@@ -46,6 +46,7 @@ all: boardd
|
||||
include ../common/cereal.mk
|
||||
|
||||
OBJS = boardd.o \
|
||||
can_list_to_can_capnp.o \
|
||||
../common/swaglog.o \
|
||||
../common/params.o \
|
||||
../common/util.o \
|
||||
@@ -72,6 +73,15 @@ boardd.o: boardd.cc
|
||||
-I../../ \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
|
||||
boardd_api_impl.so: libcan_list_to_can_capnp.a boardd_api_impl.pyx boardd_setup.py
|
||||
python boardd_setup.py build_ext --inplace
|
||||
rm -rf build
|
||||
rm -f boardd_api_impl.cpp
|
||||
|
||||
libcan_list_to_can_capnp.a: can_list_to_can_capnp.o $(CEREAL_OBJS)
|
||||
ar rcs '$@' $^
|
||||
|
||||
%.o: %.c
|
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -MMD \
|
||||
@@ -81,8 +91,16 @@ boardd.o: boardd.cc
|
||||
$(JSON_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
%.o: %.cc
|
||||
@echo "[ CC ] $@"
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
-Iinclude -I.. -I../.. \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
.PHONY: clean
|
||||
clean:
|
||||
rm -f boardd $(OBJS) $(DEPS)
|
||||
rm -f boardd libcan_list_to_can_capnp.a boardd_api_impl.so $(OBJS) $(DEPS)
|
||||
|
||||
-include $(DEPS)
|
||||
|
||||
@@ -85,6 +85,7 @@ void *safety_setter_thread(void *s) {
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), value, value_sz);
|
||||
free(value);
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
|
||||
|
||||
Executable → Regular
+7
-239
@@ -1,49 +1,13 @@
|
||||
#!/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
|
||||
|
||||
# pylint: skip-file
|
||||
import os
|
||||
import struct
|
||||
import zmq
|
||||
import time
|
||||
import subprocess
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from common.realtime import Ratekeeper
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
# Cython
|
||||
boardd_api_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
subprocess.check_call(["make", "boardd_api_impl.so"], cwd=boardd_api_dir)
|
||||
from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
|
||||
assert can_list_to_can_capnp
|
||||
|
||||
# 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
|
||||
|
||||
def can_capnp_to_can_list(can, src_filter=None):
|
||||
ret = []
|
||||
@@ -51,199 +15,3 @@ def can_capnp_to_can_list(can, src_filter=None):
|
||||
if src_filter is None or msg.src in src_filter:
|
||||
ret.append((msg.address, msg.busTime, msg.dat, msg.src))
|
||||
return ret
|
||||
|
||||
# *** 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)&0xF))
|
||||
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 = [can_capnp_to_can_list(x.can) for x in tsc]
|
||||
snds = [x for x in snds if x[-1] <= 1]
|
||||
can_send_many(snds)
|
||||
|
||||
# recv @ 100hz
|
||||
can_msgs = can_recv()
|
||||
print("sent %d got %d" % (len(snds), len(can_msgs)))
|
||||
m = can_list_to_can_capnp(can_msgs)
|
||||
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)
|
||||
logcan.send(dat.to_bytes())
|
||||
|
||||
# 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.to_bytes())
|
||||
|
||||
# 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,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());
|
||||
}
|
||||
|
||||
}
|
||||
Executable
+247
@@ -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()
|
||||
Executable
+19
@@ -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()
|
||||
+51
@@ -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()
|
||||
@@ -23,9 +23,8 @@ ifeq ($(UNAME_S),Darwin)
|
||||
else ifeq ($(OPTEST),1)
|
||||
ZMQ_LIBS = -lzmq
|
||||
else ifeq ($(UNAME_M),x86_64)
|
||||
EXTERNAL := ../../external
|
||||
ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include
|
||||
ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib -l:libzmq.a
|
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include
|
||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/x64/lib -l:libzmq.a
|
||||
else ifeq ($(UNAME_M),aarch64)
|
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a
|
||||
@@ -67,6 +66,11 @@ libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS)
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(CEREAL_LIBS)
|
||||
|
||||
packer_impl.so: packer_impl.pyx packer_setup.py
|
||||
python packer_setup.py build_ext --inplace
|
||||
rm -rf build
|
||||
rm -f packer_impl.cpp
|
||||
|
||||
$(OBJDIR)/%.o: %.cc
|
||||
@echo "[ CXX ] $@"
|
||||
$(CXX) -fPIC -c -o '$@' $^ \
|
||||
|
||||
@@ -129,4 +129,8 @@ extern "C" {
|
||||
return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals), counter);
|
||||
}
|
||||
|
||||
uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
|
||||
CANPacker *cp = (CANPacker*)inst;
|
||||
return cp->pack(address, signals, counter);
|
||||
}
|
||||
}
|
||||
|
||||
+7
-66
@@ -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"])))
|
||||
@@ -1,4 +1,3 @@
|
||||
import six
|
||||
import time
|
||||
from collections import defaultdict
|
||||
import numbers
|
||||
@@ -59,7 +58,7 @@ class CANParser(object):
|
||||
{
|
||||
'address': msg_address,
|
||||
'check_frequency': freq,
|
||||
} for msg_address, freq in six.iteritems(message_options)])
|
||||
} for msg_address, freq in message_options.items()])
|
||||
|
||||
self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c,
|
||||
len(signal_options_c), signal_options_c, sendcan, tcp_addr)
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
import os
|
||||
import glob
|
||||
import sys
|
||||
import six
|
||||
|
||||
import jinja2
|
||||
|
||||
from collections import Counter
|
||||
@@ -39,10 +39,10 @@ def main():
|
||||
continue #skip output is newer than template and dbc
|
||||
|
||||
msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
|
||||
for address, ((msg_name, msg_size), msg_sigs) in sorted(six.iteritems(can_dbc.msgs)) if msg_sigs]
|
||||
for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs]
|
||||
|
||||
def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
|
||||
def_vals = [(address, sig) for address, sig in sorted(six.iteritems(def_vals))]
|
||||
def_vals = [(address, sig) for address, sig in sorted(def_vals.items())]
|
||||
|
||||
if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
|
||||
checksum_type = "honda"
|
||||
|
||||
@@ -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
Reference in New Issue
Block a user