diff --git a/.travis.yml b/.travis.yml index 179beaa8e5..cfaf6580e3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -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' diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 5305a72237..738679aca5 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -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) - diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 62c655d7c7..f4c667fa03 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -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 diff --git a/README.md b/README.md index 3a69dc1c0b..797622c324 100644 --- a/README.md +++ b/README.md @@ -92,10 +92,13 @@ Supported Cars | Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| | Lexus | RX Hybrid 2016-19 | All | Yes | Yes2| 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 | Yes2| 20mph1| 0mph | Toyota | | Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | Toyota | | Toyota | C-HR 2017-184 | All | Yes | Stock | 0mph | 0mph | Toyota | | Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | | Toyota | Highlander 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | @@ -103,6 +106,7 @@ Supported Cars | Toyota | Prius Prime 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | 1[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 diff --git a/RELEASES.md b/RELEASES.md index c63b3ef793..3799ad3c68 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -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 diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index 9e03fd142d..21454e97ca 100644 --- a/apk/ai.comma.plus.frame.apk +++ b/apk/ai.comma.plus.frame.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2d45eb035af4adeaf31fe96ab36fc310962cc8c375319ac2a5e707b2cdc03097 -size 2588994 +oid sha256:61fc21ef8cec4ca15be05b1b5fdb9c4d95f3a78be891a36d84bd2dd89441e28e +size 2596571 diff --git a/cereal/Makefile b/cereal/Makefile index dc6b7f9d51..83318bef33 100644 --- a/cereal/Makefile +++ b/cereal/Makefile @@ -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 diff --git a/cereal/car.capnp b/cereal/car.capnp index 2915d68713..fe0d84e256 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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; - - 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 - - minEnableSpeed @17 :Float32; - minSteerSpeed @49 :Float32; - safetyModel @18 :Int16; - safetyParam @41 :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); - - longPidDeadzoneBP @32 :List(Float32); - longPidDeadzoneV @33 :List(Float32); + carFingerprint @1 :Text; + + enableGasInterceptor @2 :Bool; + enableCruise @3 :Bool; + enableCamera @4 :Bool; + enableDsu @5 :Bool; # driving support unit + enableApgs @6 :Bool; # advanced parking guidance system + + minEnableSpeed @7 :Float32; + minSteerSpeed @8 :Float32; + safetyModel @9 :Int16; + safetyParam @10 :Int16; + + 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; + } + 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; diff --git a/cereal/log.capnp b/cereal/log.capnp index 3f79deefe5..84f0a3e809 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; diff --git a/check_code_quality.sh b/check_code_quality.sh new file mode 100644 index 0000000000..854d757a84 --- /dev/null +++ b/check_code_quality.sh @@ -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 diff --git a/common/kalman/Makefile b/common/kalman/Makefile new file mode 100644 index 0000000000..cb8f092650 --- /dev/null +++ b/common/kalman/Makefile @@ -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 diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py index 3f7d049cc5..97b6d792dc 100644 --- a/common/kalman/simple_kalman.py +++ b/common/kalman/simple_kalman.py @@ -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 diff --git a/common/kalman/simple_kalman_impl.pxd b/common/kalman/simple_kalman_impl.pxd new file mode 100644 index 0000000000..c81f959272 --- /dev/null +++ b/common/kalman/simple_kalman_impl.pxd @@ -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 \ No newline at end of file diff --git a/common/kalman/simple_kalman_impl.pyx b/common/kalman/simple_kalman_impl.pyx new file mode 100644 index 0000000000..43acf7e2a0 --- /dev/null +++ b/common/kalman/simple_kalman_impl.pyx @@ -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] \ No newline at end of file diff --git a/common/kalman/simple_kalman_old.py b/common/kalman/simple_kalman_old.py new file mode 100644 index 0000000000..3f7d049cc5 --- /dev/null +++ b/common/kalman/simple_kalman_old.py @@ -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 diff --git a/common/kalman/simple_kalman_setup.py b/common/kalman/simple_kalman_setup.py new file mode 100644 index 0000000000..bccd9f888f --- /dev/null +++ b/common/kalman/simple_kalman_setup.py @@ -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"]))) \ No newline at end of file diff --git a/common/kalman/tests/__init__.py b/common/kalman/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/common/kalman/tests/test_ekf.py b/common/kalman/tests/test_ekf.py new file mode 100644 index 0000000000..3d6da323d4 --- /dev/null +++ b/common/kalman/tests/test_ekf.py @@ -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() diff --git a/common/kalman/tests/test_simple_kalman.py b/common/kalman/tests/test_simple_kalman.py new file mode 100644 index 0000000000..c1f9f7b03c --- /dev/null +++ b/common/kalman/tests/test_simple_kalman.py @@ -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) diff --git a/common/timeout.py b/common/timeout.py new file mode 100644 index 0000000000..c2c1f69712 --- /dev/null +++ b/common/timeout.py @@ -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) + diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 78c9df9890..1ac9bc5b28 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -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) + diff --git a/common/transformations/model.py b/common/transformations/model.py index afb4d66716..e832cb7ee9 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -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)] diff --git a/phonelibs/bzip2/build.txt b/phonelibs/bzip2/build.txt new file mode 100644 index 0000000000..4a157474b8 --- /dev/null +++ b/phonelibs/bzip2/build.txt @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:555f74644a70161700a8517ff6351ea8c988ca0a550a23550121147963c7cb6c +size 159 diff --git a/phonelibs/bzip2/bzlib.h b/phonelibs/bzip2/bzlib.h new file mode 100644 index 0000000000..1c245f338b --- /dev/null +++ b/phonelibs/bzip2/bzlib.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b360bfe8e0769adaa8ee6ae26324e26c40ccbdb557677a6b8ad506214984d2e8 +size 6245 diff --git a/phonelibs/bzip2/libbz2.a b/phonelibs/bzip2/libbz2.a new file mode 100644 index 0000000000..c3869db1fb --- /dev/null +++ b/phonelibs/bzip2/libbz2.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:526122b6d0ca17de16c39595741ac34f32d2987d9bc4ca645750a73a3d0494b5 +size 260714 diff --git a/phonelibs/install_capnp.sh b/phonelibs/install_capnp.sh new file mode 100755 index 0000000000..be0aed74f1 --- /dev/null +++ b/phonelibs/install_capnp.sh @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:028ce694fe58a051bdc31e915da55fda733acf0ade0ad9da8ffd23deea27f941 +size 1707 diff --git a/phonelibs/zmq/x64/include/czmq.h b/phonelibs/zmq/x64/include/czmq.h new file mode 100644 index 0000000000..2b3bf3ea5b --- /dev/null +++ b/phonelibs/zmq/x64/include/czmq.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e434c73611bb5576b3a9b1a6a40bce70a5ecbe36ea1a510392a2935196af0569 +size 1275 diff --git a/phonelibs/zmq/x64/include/czmq_library.h b/phonelibs/zmq/x64/include/czmq_library.h new file mode 100644 index 0000000000..e3b5a35c09 --- /dev/null +++ b/phonelibs/zmq/x64/include/czmq_library.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7a7c22cb33dcd531a21837dc70103043012f7b29d1f5b1b7aaf430f9034ef6f9 +size 5539 diff --git a/phonelibs/zmq/x64/include/czmq_prelude.h b/phonelibs/zmq/x64/include/czmq_prelude.h new file mode 100644 index 0000000000..48faa989c0 --- /dev/null +++ b/phonelibs/zmq/x64/include/czmq_prelude.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c6866da03dbd52cfd3eb2b3cd1fcbce6d81756d0d573590065d4a42f212a9356 +size 19294 diff --git a/phonelibs/zmq/x64/include/zactor.h b/phonelibs/zmq/x64/include/zactor.h new file mode 100644 index 0000000000..b29ec9afe1 --- /dev/null +++ b/phonelibs/zmq/x64/include/zactor.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f5e5e9e2086688abc765e0c13d6621b7648d0a5fe41446af777f9f192fbaeb29 +size 2473 diff --git a/phonelibs/zmq/x64/include/zarmour.h b/phonelibs/zmq/x64/include/zarmour.h new file mode 100644 index 0000000000..44f6ba24fb --- /dev/null +++ b/phonelibs/zmq/x64/include/zarmour.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f9696bd0aea94f596a6ad7083b72e7fc30c31b0da16d5731225a31d1c2c4ee11 +size 3644 diff --git a/phonelibs/zmq/x64/include/zauth.h b/phonelibs/zmq/x64/include/zauth.h new file mode 100644 index 0000000000..493cc40195 --- /dev/null +++ b/phonelibs/zmq/x64/include/zauth.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4a97a24653ef009fb968102d4d4a41fe2bde9efea9d0d9cfffe1abb14b4cf592 +size 3611 diff --git a/phonelibs/zmq/x64/include/zbeacon.h b/phonelibs/zmq/x64/include/zbeacon.h new file mode 100644 index 0000000000..dd238d8abd --- /dev/null +++ b/phonelibs/zmq/x64/include/zbeacon.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c4212faea9f2a2f7ba343cc02987fd96deffe8e23dee35e2c0a088841abffecb +size 2856 diff --git a/phonelibs/zmq/x64/include/zcert.h b/phonelibs/zmq/x64/include/zcert.h new file mode 100644 index 0000000000..19f4fe553f --- /dev/null +++ b/phonelibs/zmq/x64/include/zcert.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:450ea0eab5a4fd081d1face3cb4991cbdae8a05721493e5f8a099be208b2515b +size 4283 diff --git a/phonelibs/zmq/x64/include/zcertstore.h b/phonelibs/zmq/x64/include/zcertstore.h new file mode 100644 index 0000000000..311f70ebdd --- /dev/null +++ b/phonelibs/zmq/x64/include/zcertstore.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:be988dcc7301ebd693f1485cbe60d22d7cecc2d9fb6d986eadfded77678ff74c +size 3567 diff --git a/phonelibs/zmq/x64/include/zchunk.h b/phonelibs/zmq/x64/include/zchunk.h new file mode 100644 index 0000000000..dc923ae56a --- /dev/null +++ b/phonelibs/zmq/x64/include/zchunk.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c5391efa3017a3fd4d90b73f661e43f0b0664052520f4f983b38766376521d7b +size 5899 diff --git a/phonelibs/zmq/x64/include/zclock.h b/phonelibs/zmq/x64/include/zclock.h new file mode 100644 index 0000000000..2884660075 --- /dev/null +++ b/phonelibs/zmq/x64/include/zclock.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7f3f0c545553e4cef2115043f5ba58f5c856889c215fe123ed95d46dae14dfa5 +size 2465 diff --git a/phonelibs/zmq/x64/include/zconfig.h b/phonelibs/zmq/x64/include/zconfig.h new file mode 100644 index 0000000000..7382618626 --- /dev/null +++ b/phonelibs/zmq/x64/include/zconfig.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e9b32a750b627e4a621ab3e9a4cefe64b3a367da091b8757450791e8d9361c2b +size 6591 diff --git a/phonelibs/zmq/x64/include/zdigest.h b/phonelibs/zmq/x64/include/zdigest.h new file mode 100644 index 0000000000..bce0ecf829 --- /dev/null +++ b/phonelibs/zmq/x64/include/zdigest.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:22a41bbe93e99cca59fe8d1617fb27b336cd2c4f2d094b4eea95f98e3470fce3 +size 2069 diff --git a/phonelibs/zmq/x64/include/zdir.h b/phonelibs/zmq/x64/include/zdir.h new file mode 100644 index 0000000000..6635303478 --- /dev/null +++ b/phonelibs/zmq/x64/include/zdir.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4ea8ae429fee870ac8c605fe28adf604d7dd22109360f7c089471ec9f07f754d +size 6129 diff --git a/phonelibs/zmq/x64/include/zdir_patch.h b/phonelibs/zmq/x64/include/zdir_patch.h new file mode 100644 index 0000000000..1157d28dca --- /dev/null +++ b/phonelibs/zmq/x64/include/zdir_patch.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bdf4336095870dba4e5fdb89a7867b26b61a879cf4c0631dab857ebbee79ff9f +size 2422 diff --git a/phonelibs/zmq/x64/include/zfile.h b/phonelibs/zmq/x64/include/zfile.h new file mode 100644 index 0000000000..6c0b217987 --- /dev/null +++ b/phonelibs/zmq/x64/include/zfile.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a66c39280771660524a50e359e2adc6f35df35a11d6cf7d922740e2d3b34e04a +size 6250 diff --git a/phonelibs/zmq/x64/include/zframe.h b/phonelibs/zmq/x64/include/zframe.h new file mode 100644 index 0000000000..58a74b27c2 --- /dev/null +++ b/phonelibs/zmq/x64/include/zframe.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6f3b444f0b237c105a7aaef90ba2e09609e5d06a5a278c6cc5a6b8a1aa6c9752 +size 6538 diff --git a/phonelibs/zmq/x64/include/zgossip.h b/phonelibs/zmq/x64/include/zgossip.h new file mode 100644 index 0000000000..c49417587d --- /dev/null +++ b/phonelibs/zmq/x64/include/zgossip.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:132f3c042ffc2a056405a6857a66ec6bfe40fad7edfc3229ef6b833407a0bfe3 +size 2964 diff --git a/phonelibs/zmq/x64/include/zhash.h b/phonelibs/zmq/x64/include/zhash.h new file mode 100644 index 0000000000..ae77b94364 --- /dev/null +++ b/phonelibs/zmq/x64/include/zhash.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad6a1d63f4059cd6dd9c18eae4afd6a449c6f1ae4b03a1b697af79708612b9e1 +size 8178 diff --git a/phonelibs/zmq/x64/include/zhashx.h b/phonelibs/zmq/x64/include/zhashx.h new file mode 100644 index 0000000000..5790f7141d --- /dev/null +++ b/phonelibs/zmq/x64/include/zhashx.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d163c60813069859c1c8ea94e07f7769f35b406147abc342d3dbb9448cfb8dc9 +size 12557 diff --git a/phonelibs/zmq/x64/include/ziflist.h b/phonelibs/zmq/x64/include/ziflist.h new file mode 100644 index 0000000000..0a01a19c62 --- /dev/null +++ b/phonelibs/zmq/x64/include/ziflist.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3df305f973face711a307384a310bbe3d74afbd06fc263e4fe78e6886244df73 +size 2256 diff --git a/phonelibs/zmq/x64/include/zlist.h b/phonelibs/zmq/x64/include/zlist.h new file mode 100644 index 0000000000..19376fb3d7 --- /dev/null +++ b/phonelibs/zmq/x64/include/zlist.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:23ca4ea43fe89ef066c42c0524b21bb2f3857bda386f8df8109f69cb25f13cc4 +size 6202 diff --git a/phonelibs/zmq/x64/include/zlistx.h b/phonelibs/zmq/x64/include/zlistx.h new file mode 100644 index 0000000000..df1ab07816 --- /dev/null +++ b/phonelibs/zmq/x64/include/zlistx.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:382c7fa2cfdcb3efdf1631ba6ba1f97bb03297c102cf6e629c3db8519e946a0e +size 8547 diff --git a/phonelibs/zmq/x64/include/zloop.h b/phonelibs/zmq/x64/include/zloop.h new file mode 100644 index 0000000000..394f0f16dc --- /dev/null +++ b/phonelibs/zmq/x64/include/zloop.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:32aa6915cea28c1a7b1bd360d0dc7d4981ace423a126c8708718d5058893289d +size 7185 diff --git a/phonelibs/zmq/x64/include/zmonitor.h b/phonelibs/zmq/x64/include/zmonitor.h new file mode 100644 index 0000000000..21c138037f --- /dev/null +++ b/phonelibs/zmq/x64/include/zmonitor.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8c4004636f31648f98050e637daa231c70fcf4b64da326548b4f50dccba5ceb1 +size 1889 diff --git a/phonelibs/zmq/x64/include/zmq.h b/phonelibs/zmq/x64/include/zmq.h new file mode 100644 index 0000000000..f70ef9d7f4 --- /dev/null +++ b/phonelibs/zmq/x64/include/zmq.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e00ab4b318526fc2b340ca69cafcd5e5071810f63471a0fe89064ee18e1afafb +size 24177 diff --git a/phonelibs/zmq/x64/include/zmq_utils.h b/phonelibs/zmq/x64/include/zmq_utils.h new file mode 100644 index 0000000000..947923e6dd --- /dev/null +++ b/phonelibs/zmq/x64/include/zmq_utils.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:926abbe8a8ab28df17746ca5bf60440b799b391be172c406e0628cdef3788909 +size 2316 diff --git a/phonelibs/zmq/x64/include/zmsg.h b/phonelibs/zmq/x64/include/zmsg.h new file mode 100644 index 0000000000..4a9b43ae94 --- /dev/null +++ b/phonelibs/zmq/x64/include/zmsg.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1e08ea0a57a0cede5c2860dd447d5452226b61de22dc778e6e6c602ee940ad20 +size 11537 diff --git a/phonelibs/zmq/x64/include/zpoller.h b/phonelibs/zmq/x64/include/zpoller.h new file mode 100644 index 0000000000..9ece76cf03 --- /dev/null +++ b/phonelibs/zmq/x64/include/zpoller.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1b2af935bab05d478ed103138e5a0b5ddce85a270de2eef73c474155a93f612c +size 3453 diff --git a/phonelibs/zmq/x64/include/zproc.h b/phonelibs/zmq/x64/include/zproc.h new file mode 100644 index 0000000000..419c9aeb8a --- /dev/null +++ b/phonelibs/zmq/x64/include/zproc.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c18a537b231ec1228cc9d4ca3c8e0d81aad0311bafc2d9f8081d9ad10671c935 +size 7801 diff --git a/phonelibs/zmq/x64/include/zproxy.h b/phonelibs/zmq/x64/include/zproxy.h new file mode 100644 index 0000000000..04dd99d803 --- /dev/null +++ b/phonelibs/zmq/x64/include/zproxy.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:61798dbd369cfefd3a90376818f628107bff9131d95cc8b444a924c28cb06115 +size 3889 diff --git a/phonelibs/zmq/x64/include/zrex.h b/phonelibs/zmq/x64/include/zrex.h new file mode 100644 index 0000000000..48f2dd4676 --- /dev/null +++ b/phonelibs/zmq/x64/include/zrex.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5aa3120214f9fa79436f9b5ed4db050a5974d774dd99bd0e9bba13f70f04be6d +size 2903 diff --git a/phonelibs/zmq/x64/include/zsock.h b/phonelibs/zmq/x64/include/zsock.h new file mode 100644 index 0000000000..738d1d69f8 --- /dev/null +++ b/phonelibs/zmq/x64/include/zsock.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b1e1908455d9810980fb5c9de6726c33a327ff71976d08f696860cd506087c6c +size 44814 diff --git a/phonelibs/zmq/x64/include/zstr.h b/phonelibs/zmq/x64/include/zstr.h new file mode 100644 index 0000000000..1aaf84f04d --- /dev/null +++ b/phonelibs/zmq/x64/include/zstr.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ea6f83ec5aa4ae69d95a991f0a9c8912b7f8e1c23db1d73be934c80e086f783a +size 4557 diff --git a/phonelibs/zmq/x64/include/zsys.h b/phonelibs/zmq/x64/include/zsys.h new file mode 100644 index 0000000000..adc1da73ef --- /dev/null +++ b/phonelibs/zmq/x64/include/zsys.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0848d4218ef47604f36f365d1ee83727f2a03494c53a58475803b5f36437c5e6 +size 16109 diff --git a/phonelibs/zmq/x64/include/ztimerset.h b/phonelibs/zmq/x64/include/ztimerset.h new file mode 100644 index 0000000000..4621e8f084 --- /dev/null +++ b/phonelibs/zmq/x64/include/ztimerset.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:09649e6b235a6b2af50eb7e7644c38fd5132d16e4b63351affd7adf5009f06ad +size 3551 diff --git a/phonelibs/zmq/x64/include/ztrie.h b/phonelibs/zmq/x64/include/ztrie.h new file mode 100644 index 0000000000..7f5f61c27b --- /dev/null +++ b/phonelibs/zmq/x64/include/ztrie.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:89dc6a1b5e09267704dc07ed986fb82b5057e51a4e7f0bf801cc204dedf9fb4e +size 4475 diff --git a/phonelibs/zmq/x64/include/zuuid.h b/phonelibs/zmq/x64/include/zuuid.h new file mode 100644 index 0000000000..4676899834 --- /dev/null +++ b/phonelibs/zmq/x64/include/zuuid.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6de67e7d53ebbffefe2c330bded67425df97d8bb5d79717d0227e3b6fb1899f9 +size 2790 diff --git a/phonelibs/zmq/x64/lib/libczmq.a b/phonelibs/zmq/x64/lib/libczmq.a new file mode 100644 index 0000000000..71dcaa686d --- /dev/null +++ b/phonelibs/zmq/x64/lib/libczmq.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c55f283766b3a7746306f6599c2094630a6d55c51ca7d3e84d0d3fb099bd57a0 +size 986208 diff --git a/phonelibs/zmq/x64/lib/libczmq.la b/phonelibs/zmq/x64/lib/libczmq.la new file mode 100755 index 0000000000..3bda761185 --- /dev/null +++ b/phonelibs/zmq/x64/lib/libczmq.la @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:371efa44e737c438c7524139d773d3a2c2481704a5c38c6766fd24f8449ae91c +size 1084 diff --git a/phonelibs/zmq/x64/lib/libczmq.so b/phonelibs/zmq/x64/lib/libczmq.so new file mode 120000 index 0000000000..db9aa60f9b --- /dev/null +++ b/phonelibs/zmq/x64/lib/libczmq.so @@ -0,0 +1 @@ +libczmq.so.4.0.2 \ No newline at end of file diff --git a/phonelibs/zmq/x64/lib/libczmq.so.4 b/phonelibs/zmq/x64/lib/libczmq.so.4 new file mode 120000 index 0000000000..db9aa60f9b --- /dev/null +++ b/phonelibs/zmq/x64/lib/libczmq.so.4 @@ -0,0 +1 @@ +libczmq.so.4.0.2 \ No newline at end of file diff --git a/phonelibs/zmq/x64/lib/libczmq.so.4.0.2 b/phonelibs/zmq/x64/lib/libczmq.so.4.0.2 new file mode 100755 index 0000000000..3c1bd1c242 --- /dev/null +++ b/phonelibs/zmq/x64/lib/libczmq.so.4.0.2 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bde558fe588d8bd4b8016ad4637a7bc683a6957085b2a3528cd681354504b5ab +size 563416 diff --git a/phonelibs/zmq/x64/lib/libzmq.a b/phonelibs/zmq/x64/lib/libzmq.a new file mode 100644 index 0000000000..eccdfae49c --- /dev/null +++ b/phonelibs/zmq/x64/lib/libzmq.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a9ef32858d9dd32d623f448d68901525ac6971a5a790b70d61555b1a8def4eca +size 5319544 diff --git a/phonelibs/zmq/x64/lib/libzmq.la b/phonelibs/zmq/x64/lib/libzmq.la new file mode 100644 index 0000000000..0d914c3913 --- /dev/null +++ b/phonelibs/zmq/x64/lib/libzmq.la @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6a9380180adcdbf42c77910639d622b44f716e905bc26d25b00d46e1f72b4ea6 +size 965 diff --git a/phonelibs/zmq/x64/lib/libzmq.lai b/phonelibs/zmq/x64/lib/libzmq.lai new file mode 100644 index 0000000000..05872b5ba1 --- /dev/null +++ b/phonelibs/zmq/x64/lib/libzmq.lai @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:573977c92c2d4ac52c3df775a216276b1fd050d3abd03906d2b852879c41ce85 +size 946 diff --git a/phonelibs/zmq/x64/lib/libzmq.so b/phonelibs/zmq/x64/lib/libzmq.so new file mode 120000 index 0000000000..ef44cafc6a --- /dev/null +++ b/phonelibs/zmq/x64/lib/libzmq.so @@ -0,0 +1 @@ +libzmq.so.5.1.2 \ No newline at end of file diff --git a/phonelibs/zmq/x64/lib/libzmq.so.5 b/phonelibs/zmq/x64/lib/libzmq.so.5 new file mode 120000 index 0000000000..ef44cafc6a --- /dev/null +++ b/phonelibs/zmq/x64/lib/libzmq.so.5 @@ -0,0 +1 @@ +libzmq.so.5.1.2 \ No newline at end of file diff --git a/phonelibs/zmq/x64/lib/libzmq.so.5.1.2 b/phonelibs/zmq/x64/lib/libzmq.so.5.1.2 new file mode 100755 index 0000000000..cfe577a00c --- /dev/null +++ b/phonelibs/zmq/x64/lib/libzmq.so.5.1.2 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:89878c51741e047a157fbcb223d57c4ca76441796a53540e86ef9c811c71bc45 +size 1335280 diff --git a/phonelibs/zmq/x64/lib/pkgconfig/libczmq.pc b/phonelibs/zmq/x64/lib/pkgconfig/libczmq.pc new file mode 100644 index 0000000000..f624a45c89 --- /dev/null +++ b/phonelibs/zmq/x64/lib/pkgconfig/libczmq.pc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6f17e5e2b5e61fd0efb58d14572bf9eb11ea017ca5e05088422f79bf4548a402 +size 946 diff --git a/phonelibs/zmq/x64/lib/pkgconfig/libzmq.pc b/phonelibs/zmq/x64/lib/pkgconfig/libzmq.pc new file mode 100644 index 0000000000..96bec0075f --- /dev/null +++ b/phonelibs/zmq/x64/lib/pkgconfig/libzmq.pc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:77a40af70e122f5db9a4d043811921ff7105fc3bf66d445d195b3a9289f16c11 +size 220 diff --git a/run_docker_tests.sh b/run_docker_tests.sh index a153aac73b..dc629e6137 100755 --- a/run_docker_tests.sh +++ b/run_docker_tests.sh @@ -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' diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 501806969c..2671672c52 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.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: diff --git a/selfdrive/boardd/Makefile b/selfdrive/boardd/Makefile index 893edde545..7dfbb225c0 100644 --- a/selfdrive/boardd/Makefile +++ b/selfdrive/boardd/Makefile @@ -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) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index dd89d4d6d4..3b293307ab 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -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((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(); diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py old mode 100755 new mode 100644 index 2ebd2e9ffa..d3bcb14710 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -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() diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/boardd/boardd_api_impl.pyx new file mode 100644 index 0000000000..75aa1081c3 --- /dev/null +++ b/selfdrive/boardd/boardd_api_impl.pyx @@ -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 diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py new file mode 100644 index 0000000000..f614a06161 --- /dev/null +++ b/selfdrive/boardd/boardd_setup.py @@ -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"], + ) + ) +) diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc new file mode 100644 index 0000000000..626c1e2a98 --- /dev/null +++ b/selfdrive/boardd/can_list_to_can_capnp.cc @@ -0,0 +1,36 @@ +#include +#include +#include +#include "common/timing.h" +#include +#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_list, std::string &out, bool sendCan) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + 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()); +} + +} diff --git a/selfdrive/boardd/tests/__init__.py b/selfdrive/boardd/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py new file mode 100755 index 0000000000..9ce5f7f3e1 --- /dev/null +++ b/selfdrive/boardd/tests/boardd_old.py @@ -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() diff --git a/selfdrive/boardd/tests/fuzzer.py b/selfdrive/boardd/tests/fuzzer.py new file mode 100755 index 0000000000..cb6edf7106 --- /dev/null +++ b/selfdrive/boardd/tests/fuzzer.py @@ -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) diff --git a/selfdrive/boardd/tests/test_boardd_api.py b/selfdrive/boardd/tests/test_boardd_api.py new file mode 100644 index 0000000000..0e862b0952 --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_api.py @@ -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() diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py new file mode 100755 index 0000000000..1b6f1b1387 --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -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() diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile index 1909a7bb93..918c445288 100644 --- a/selfdrive/can/Makefile +++ b/selfdrive/can/Makefile @@ -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 '$@' $^ \ diff --git a/selfdrive/can/packer.cc b/selfdrive/can/packer.cc index 17171f1550..d0e8419788 100644 --- a/selfdrive/can/packer.cc +++ b/selfdrive/can/packer.cc @@ -129,4 +129,8 @@ extern "C" { return cp->pack(address, std::vector(vals, vals+num_vals), counter); } + uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector &signals, int counter) { + CANPacker *cp = (CANPacker*)inst; + return cp->pack(address, signals, counter); + } } diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py index 7f3c8d2858..81a8c34270 100644 --- a/selfdrive/can/packer.py +++ b/selfdrive/can/packer.py @@ -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 diff --git a/selfdrive/can/packer_impl.pyx b/selfdrive/can/packer_impl.pyx new file mode 100644 index 0000000000..26ce8c1267 --- /dev/null +++ b/selfdrive/can/packer_impl.pyx @@ -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 = dlsym(libdbc, 'canpack_init') + self.canpack_pack_vector = dlsym(libdbc, 'canpack_pack_vector') + self.dbc_lookup = 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, (&val)[:size], bus] diff --git a/selfdrive/can/packer_setup.py b/selfdrive/can/packer_setup.py new file mode 100644 index 0000000000..a36d4199ee --- /dev/null +++ b/selfdrive/can/packer_setup.py @@ -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"]))) diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py index 83d0f66237..2e8384f637 100644 --- a/selfdrive/can/parser.py +++ b/selfdrive/can/parser.py @@ -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) diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py index 31b1315968..3f3f2ddcad 100755 --- a/selfdrive/can/process_dbc.py +++ b/selfdrive/can/process_dbc.py @@ -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" diff --git a/selfdrive/can/tests/__init__.py b/selfdrive/can/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/can/tests/packer_old.py b/selfdrive/can/tests/packer_old.py new file mode 100644 index 0000000000..0c17c2e4d9 --- /dev/null +++ b/selfdrive/can/tests/packer_old.py @@ -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")) diff --git a/selfdrive/can/tests/test_packer_chrysler.py b/selfdrive/can/tests/test_packer_chrysler.py new file mode 100644 index 0000000000..da85a98415 --- /dev/null +++ b/selfdrive/can/tests/test_packer_chrysler.py @@ -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() diff --git a/selfdrive/can/tests/test_packer_gm.py b/selfdrive/can/tests/test_packer_gm.py new file mode 100644 index 0000000000..db5631fbbe --- /dev/null +++ b/selfdrive/can/tests/test_packer_gm.py @@ -0,0 +1,66 @@ +import unittest +import random + +from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld +from selfdrive.can.packer import CANPacker +import selfdrive.car.gm.gmcan as gmcan +from selfdrive.car.gm.interface import CanBus as GMCanBus + + +class TestPackerMethods(unittest.TestCase): + def setUp(self): + self.gm_cp_old = CANPackerOld("gm_global_a_powertrain") + self.gm_cp = CANPacker("gm_global_a_powertrain") + + self.ct6_cp_old = CANPackerOld("cadillac_ct6_chassis") + self.ct6_cp = CANPacker("cadillac_ct6_chassis") + + def test_correctness(self): + # Test all cars' commands, randomize the params. + for _ in xrange(1000): + bus = random.randint(0, 65536) + apply_steer = (random.randint(0, 2) % 2 == 0) + idx = random.randint(0, 65536) + lkas_active = (random.randint(0, 2) % 2 == 0) + m_old = gmcan.create_steering_control(self.gm_cp_old, bus, apply_steer, idx, lkas_active) + m = gmcan.create_steering_control(self.gm_cp, bus, apply_steer, idx, lkas_active) + self.assertEqual(m_old, m) + + canbus = GMCanBus() + apply_steer = (random.randint(0, 2) % 2 == 0) + v_ego = random.randint(0, 65536) + idx = random.randint(0, 65536) + enabled = (random.randint(0, 2) % 2 == 0) + m_old = gmcan.create_steering_control_ct6(self.ct6_cp_old, canbus, apply_steer, v_ego, idx, enabled) + m = gmcan.create_steering_control_ct6(self.ct6_cp, canbus, apply_steer, v_ego, idx, enabled) + self.assertEqual(m_old, m) + + bus = random.randint(0, 65536) + throttle = random.randint(0, 65536) + idx = random.randint(0, 65536) + acc_engaged = (random.randint(0, 2) % 2 == 0) + at_full_stop = (random.randint(0, 2) % 2 == 0) + m_old = gmcan.create_gas_regen_command(self.gm_cp_old, bus, throttle, idx, acc_engaged, at_full_stop) + m = gmcan.create_gas_regen_command(self.gm_cp, bus, throttle, idx, acc_engaged, at_full_stop) + self.assertEqual(m_old, m) + + bus = random.randint(0, 65536) + apply_brake = (random.randint(0, 2) % 2 == 0) + idx = random.randint(0, 65536) + near_stop = (random.randint(0, 2) % 2 == 0) + at_full_stop = (random.randint(0, 2) % 2 == 0) + m_old = gmcan.create_friction_brake_command(self.ct6_cp_old, bus, apply_brake, idx, near_stop, at_full_stop) + m = gmcan.create_friction_brake_command(self.ct6_cp, bus, apply_brake, idx, near_stop, at_full_stop) + self.assertEqual(m_old, m) + + bus = random.randint(0, 65536) + acc_engaged = (random.randint(0, 2) % 2 == 0) + target_speed_kph = random.randint(0, 65536) + lead_car_in_sight = (random.randint(0, 2) % 2 == 0) + m_old = gmcan.create_acc_dashboard_command(self.gm_cp_old, bus, acc_engaged, target_speed_kph, lead_car_in_sight) + m = gmcan.create_acc_dashboard_command(self.gm_cp, bus, acc_engaged, target_speed_kph, lead_car_in_sight) + self.assertEqual(m_old, m) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/can/tests/test_packer_honda.py b/selfdrive/can/tests/test_packer_honda.py new file mode 100644 index 0000000000..58b841f5a2 --- /dev/null +++ b/selfdrive/can/tests/test_packer_honda.py @@ -0,0 +1,56 @@ +import unittest +import random + +from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld +from selfdrive.can.packer import CANPacker +import selfdrive.car.honda.hondacan as hondacan +from selfdrive.car.honda.values import HONDA_BOSCH +from selfdrive.car.honda.carcontroller import HUDData + + +class TestPackerMethods(unittest.TestCase): + def setUp(self): + self.honda_cp_old = CANPackerOld("honda_pilot_touring_2017_can_generated") + self.honda_cp = CANPacker("honda_pilot_touring_2017_can_generated") + + def test_correctness(self): + # Test all commands, randomize the params. + for _ in xrange(1000): + apply_brake = (random.randint(0, 2) % 2 == 0) + pump_on = (random.randint(0, 2) % 2 == 0) + pcm_override = (random.randint(0, 2) % 2 == 0) + pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0) + chime = random.randint(0, 65536) + fcw = random.randint(0, 65536) + idx = random.randint(0, 65536) + m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx) + m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx) + self.assertEqual(m_old, m) + + apply_steer = (random.randint(0, 2) % 2 == 0) + lkas_active = (random.randint(0, 2) % 2 == 0) + car_fingerprint = HONDA_BOSCH[0] + idx = random.randint(0, 65536) + m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx) + m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx) + self.assertEqual(m_old, m) + + pcm_speed = random.randint(0, 65536) + hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536), + 0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), + random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536)) + car_fingerprint = HONDA_BOSCH[0] + idx = random.randint(0, 65536) + m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, idx) + m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, idx) + self.assertEqual(m_old, m) + + button_val = random.randint(0, 65536) + idx = random.randint(0, 65536) + m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx) + m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx) + self.assertEqual(m_old, m) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/can/tests/test_packer_hyundai.py b/selfdrive/can/tests/test_packer_hyundai.py new file mode 100644 index 0000000000..d9e829ad59 --- /dev/null +++ b/selfdrive/can/tests/test_packer_hyundai.py @@ -0,0 +1,70 @@ +import unittest +import random + +from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld +from selfdrive.can.packer import CANPacker +import selfdrive.car.hyundai.hyundaican as hyundaican +from selfdrive.car.hyundai.values import CHECKSUM as hyundai_checksum + + +class TestPackerMethods(unittest.TestCase): + def setUp(self): + self.hyundai_cp_old = CANPackerOld("hyundai_kia_generic") + self.hyundai_cp = CANPacker("hyundai_kia_generic") + + def test_correctness(self): + # Test all commands, randomize the params. + for _ in xrange(1000): + # Hyundai + car_fingerprint = hyundai_checksum["crc8"][0] + apply_steer = (random.randint(0, 2) % 2 == 0) + steer_req = (random.randint(0, 2) % 2 == 0) + cnt = random.randint(0, 65536) + enabled = (random.randint(0, 2) % 2 == 0) + lkas11 = { + "CF_Lkas_LdwsSysState": random.randint(0,65536), + "CF_Lkas_SysWarning": random.randint(0,65536), + "CF_Lkas_LdwsLHWarning": random.randint(0,65536), + "CF_Lkas_LdwsRHWarning": random.randint(0,65536), + "CF_Lkas_HbaLamp": random.randint(0,65536), + "CF_Lkas_FcwBasReq": random.randint(0,65536), + "CF_Lkas_ToiFlt": random.randint(0,65536), + "CF_Lkas_HbaSysState": random.randint(0,65536), + "CF_Lkas_FcwOpt": random.randint(0,65536), + "CF_Lkas_HbaOpt": random.randint(0,65536), + "CF_Lkas_FcwSysState": random.randint(0,65536), + "CF_Lkas_FcwCollisionWarning": random.randint(0,65536), + "CF_Lkas_FusionState": random.randint(0,65536), + "CF_Lkas_FcwOpt_USM": random.randint(0,65536), + "CF_Lkas_LdwsOpt_USM": random.randint(0,65536) + } + hud_alert = random.randint(0, 65536) + keep_stock = (random.randint(0, 2) % 2 == 0) + m_old = hyundaican.create_lkas11(self.hyundai_cp_old, car_fingerprint, apply_steer, steer_req, cnt, enabled, + lkas11, hud_alert, keep_stock) + m = hyundaican.create_lkas11(self.hyundai_cp, car_fingerprint, apply_steer, steer_req, cnt, enabled, + lkas11, hud_alert, keep_stock) + self.assertEqual(m_old, m) + + clu11 = { + "CF_Clu_CruiseSwState": random.randint(0,65536), + "CF_Clu_CruiseSwMain": random.randint(0,65536), + "CF_Clu_SldMainSW": random.randint(0,65536), + "CF_Clu_ParityBit1": random.randint(0,65536), + "CF_Clu_VanzDecimal": random.randint(0,65536), + "CF_Clu_Vanz": random.randint(0,65536), + "CF_Clu_SPEED_UNIT": random.randint(0,65536), + "CF_Clu_DetentOut": random.randint(0,65536), + "CF_Clu_RheostatLevel": random.randint(0,65536), + "CF_Clu_CluInfo": random.randint(0,65536), + "CF_Clu_AmpInfo": random.randint(0,65536), + "CF_Clu_AliveCnt1": random.randint(0,65536), + } + button = random.randint(0, 65536) + m_old = hyundaican.create_clu11(self.hyundai_cp_old, clu11, button) + m = hyundaican.create_clu11(self.hyundai_cp, clu11, button) + self.assertEqual(m_old, m) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/can/tests/test_packer_subaru.py b/selfdrive/can/tests/test_packer_subaru.py new file mode 100644 index 0000000000..65dd77cf83 --- /dev/null +++ b/selfdrive/can/tests/test_packer_subaru.py @@ -0,0 +1,37 @@ +import unittest +import random + +from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld +from selfdrive.can.packer import CANPacker +import selfdrive.car.subaru.subarucan as subarucan +from selfdrive.car.subaru.values import CAR as subaru_car + + +class TestPackerMethods(unittest.TestCase): + def setUp(self): + self.subaru_cp_old = CANPackerOld("subaru_global_2017") + self.subaru_cp = CANPacker("subaru_global_2017") + + def test_correctness(self): + # Test all cars' commands, randomize the params. + for _ in xrange(1000): + apply_steer = (random.randint(0, 2) % 2 == 0) + frame = random.randint(1, 65536) + steer_step = random.randint(1, 65536) + m_old = subarucan.create_steering_control(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step) + m = subarucan.create_steering_control(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step) + self.assertEqual(m_old, m) + + m_old = subarucan.create_steering_status(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step) + m = subarucan.create_steering_status(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step) + self.assertEqual(m_old, m) + + es_distance_msg = {} + pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0) + m_old = subarucan.create_es_distance(self.subaru_cp_old, es_distance_msg, pcm_cancel_cmd) + m = subarucan.create_es_distance(self.subaru_cp, es_distance_msg, pcm_cancel_cmd) + self.assertEqual(m_old, m) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/can/tests/test_packer_toyota.py b/selfdrive/can/tests/test_packer_toyota.py new file mode 100644 index 0000000000..42e36f3171 --- /dev/null +++ b/selfdrive/can/tests/test_packer_toyota.py @@ -0,0 +1,88 @@ +import unittest +import random + +from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld +from selfdrive.can.packer import CANPacker +from selfdrive.car.toyota.toyotacan import ( + create_ipas_steer_command, create_steer_command, create_accel_command, + create_fcw_command, create_ui_command +) +from common.realtime import sec_since_boot + + +class TestPackerMethods(unittest.TestCase): + def setUp(self): + self.cp_old = CANPackerOld("toyota_rav4_hybrid_2017_pt_generated") + self.cp = CANPacker("toyota_rav4_hybrid_2017_pt_generated") + + def test_correctness(self): + # Test all commands, randomize the params. + for _ in xrange(1000): + # Toyota + steer = random.randint(-1, 1) + enabled = (random.randint(0, 2) % 2 == 0) + apgs_enabled = (random.randint(0, 2) % 2 == 0) + m_old = create_ipas_steer_command(self.cp_old, steer, enabled, apgs_enabled) + m = create_ipas_steer_command(self.cp, steer, enabled, apgs_enabled) + self.assertEqual(m_old, m) + + steer = (random.randint(0, 2) % 2 == 0) + steer_req = (random.randint(0, 2) % 2 == 0) + raw_cnt = random.randint(1, 65536) + m_old = create_steer_command(self.cp_old, steer, steer_req, raw_cnt) + m = create_steer_command(self.cp, steer, steer_req, raw_cnt) + self.assertEqual(m_old, m) + + accel = (random.randint(0, 2) % 2 == 0) + pcm_cancel = (random.randint(0, 2) % 2 == 0) + standstill_req = (random.randint(0, 2) % 2 == 0) + lead = (random.randint(0, 2) % 2 == 0) + m_old = create_accel_command(self.cp_old, accel, pcm_cancel, standstill_req, lead) + m = create_accel_command(self.cp, accel, pcm_cancel, standstill_req, lead) + self.assertEqual(m_old, m) + + fcw = random.randint(1, 65536) + m_old = create_fcw_command(self.cp_old, fcw) + m = create_fcw_command(self.cp, fcw) + self.assertEqual(m_old, m) + + steer = (random.randint(0, 2) % 2 == 0) + sound1 = random.randint(1, 65536) + sound2 = random.randint(1, 65536) + left_line = (random.randint(0, 2) % 2 == 0) + right_line = (random.randint(0, 2) % 2 == 0) + left_lane_depart = (random.randint(0, 2) % 2 == 0) + right_lane_depart = (random.randint(0, 2) % 2 == 0) + m_old = create_ui_command(self.cp_old, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart) + m = create_ui_command(self.cp, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart) + self.assertEqual(m_old, m) + + def test_performance(self): + n1 = sec_since_boot() + recursions = 100000 + steer = (random.randint(0, 2) % 2 == 0) + sound1 = random.randint(1, 65536) + sound2 = random.randint(1, 65536) + left_line = (random.randint(0, 2) % 2 == 0) + right_line = (random.randint(0, 2) % 2 == 0) + left_lane_depart = (random.randint(0, 2) % 2 == 0) + right_lane_depart = (random.randint(0, 2) % 2 == 0) + + for _ in xrange(recursions): + create_ui_command(self.cp_old, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart) + n2 = sec_since_boot() + elapsed_old = n2 - n1 + + # print('Old API, elapsed time: {} secs'.format(elapsed_old)) + n1 = sec_since_boot() + for _ in xrange(recursions): + create_ui_command(self.cp, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart) + 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() diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 7db36634c0..c6a1f9d293 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -97,4 +97,4 @@ class CarController(object): self.ccframe += 1 self.prev_frame = frame - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index fd3c081f3d..94effcf931 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -1,7 +1,6 @@ from selfdrive.can.parser import CANParser from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD from common.kalman.simple_kalman import KF1D -import numpy as np def parse_gear_shifter(can_gear): @@ -90,10 +89,10 @@ class CarState(object): dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 - self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), - A=np.matrix([[1.0, dt], [0.0, 1.0]]), - C=np.matrix([1.0, 0.0]), - K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], + A=[[1.0, dt], [0.0, 1.0]], + C=[1.0, 0.0], + K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 @@ -127,7 +126,7 @@ class CarState(object): # Kalman filter if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]]) + self.v_ego_kf.x = [[v_wheel], [0.0]] self.v_ego_raw = v_wheel v_ego_x = self.v_ego_kf.update(v_wheel) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index e0551ef0cf..2c3d7455a0 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -70,12 +70,12 @@ class CarInterface(object): tireStiffnessRear_civic = 90000 * 2.0 # Speed conversion: 20, 45 mph - ret.steerKpBP, ret.steerKiBP = [[9., 20.], [9., 20.]] ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 ret.mass = 2858 + std_cargo # kg curb weight Pacifica Hybrid 2017 - ret.steerKpV, ret.steerKiV = [[0.15,0.30], [0.03,0.05]] - ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15,0.30], [0.03,0.05]] + ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.7 @@ -86,8 +86,6 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.44 - ret.longPidDeadzoneBP = [0., 9.] - ret.longPidDeadzoneV = [0., .15] ret.minSteerSpeed = 3.8 # m/s ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this @@ -129,10 +127,12 @@ class CarInterface(object): ret.stoppingControl = False ret.startAccel = 0.0 - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [3.6, 2.4, 1.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.54, 0.36] + ret.longitudinalTuning.deadzoneBP = [0., 9.] + ret.longitudinalTuning.deadzoneV = [0., .15] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.54, 0.36] return ret diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 4636b89a31..c3e1c35805 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -34,6 +34,10 @@ FINGERPRINTS = { # Based on 0607d2516fc2148f|2019-02-13--23-03-16 { 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8 + }, + # Based on 3c7ce223e3571b54|2019-05-11--20-16-14 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8 } ], CAR.JEEP_CHEROKEE: [ diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 5a8f1f142c..2c7a27eb3a 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -46,10 +46,10 @@ class CarState(object): dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 - self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), - A=np.matrix([[1.0, dt], [0.0, 1.0]]), - C=np.matrix([1.0, 0.0]), - K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], + A=[[1.0, dt], [0.0, 1.0]], + C=[1.0, 0.0], + K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 def update(self, cp): @@ -69,7 +69,7 @@ class CarState(object): # Kalman filter if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]]) + self.v_ego_kf.x = [[v_wheel], [0.0]] self.v_ego_raw = v_wheel v_ego_x = self.v_ego_kf.update(v_wheel) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 25e4d2b9cc..a46681eb4b 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -69,12 +69,12 @@ class CarInterface(object): tireStiffnessFront_civic = 85400 tireStiffnessRear_civic = 90000 - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.wheelbase = 2.85 ret.steerRatio = 14.8 ret.mass = 3045. * CV.LB_TO_KG + std_cargo - ret.steerKpV, ret.steerKiV = [[0.01], [0.005]] # TODO: tune this - ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this + ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 @@ -84,8 +84,6 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.44 - ret.longPidDeadzoneBP = [0., 9.] - ret.longPidDeadzoneV = [0., .15] # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. @@ -126,10 +124,12 @@ class CarInterface(object): ret.stoppingControl = False ret.startAccel = 0.0 - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [3.6, 2.4, 1.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.54, 0.36] + ret.longitudinalTuning.deadzoneBP = [0., 9.] + ret.longitudinalTuning.deadzoneV = [0., .15] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.54, 0.36] return ret diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 67b2405364..26cfd84d35 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -195,4 +195,4 @@ class CarController(object): # issued for the same chime type and duration self.chime = chime - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index ae97a38b9c..7a18f320cf 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -63,10 +63,10 @@ class CarState(object): # vEgo kalman filter dt = 0.01 - self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]), - A=np.matrix([[1., dt], [0., 1.]]), - C=np.matrix([1., 0.]), - K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego_kf = KF1D(x0=[[0.], [0.]], + A=[[1., dt], [0., 1.]], + C=[1., 0.], + K=[[0.12287673], [0.29666309]]) self.v_ego = 0. def update(self, pt_cp): diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d5afffbaa9..a21b12d315 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -4,7 +4,8 @@ from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, SUPERCRUISE_CARS +from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, \ + SUPERCRUISE_CARS, AccState from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser try: @@ -162,25 +163,24 @@ class CarInterface(object): ret.mass / mass_civic * \ (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) - # same tuning for Volt and CT6 for now - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.2], [0.00]] - ret.steerKf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] + ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 - ret.steerMaxBP = [0.] # m/s + ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] ret.gasMaxBP = [0.] ret.gasMaxV = [.5] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] - ret.longPidDeadzoneBP = [0.] - ret.longPidDeadzoneV = [0.] - ret.longitudinalKpBP = [5., 35.] - ret.longitudinalKpV = [2.4, 1.5] - ret.longitudinalKiBP = [0.] - ret.longitudinalKiV = [0.36] + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kpV = [2.4, 1.5] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.36] + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] ret.steerLimitAlert = True @@ -231,7 +231,7 @@ class CarInterface(object): # cruise state ret.cruiseState.available = bool(self.CS.main_on) - cruiseEnabled = self.CS.pcm_acc_status != 0 + cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF ret.cruiseState.enabled = cruiseEnabled ret.cruiseState.standstill = self.CS.pcm_acc_status == 4 @@ -323,6 +323,8 @@ class CarInterface(object): events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) + if self.CS.pcm_acc_status == AccState.FAULTED: + events.append(create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index f6aa0658ff..b41919acb9 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -30,6 +30,12 @@ class CM: LOW_CHIME = 0x86 HIGH_CHIME = 0x87 +class AccState: + OFF = 0 + ACTIVE = 1 + FAULTED = 3 + STANDSTILL = 4 + AUDIO_HUD = { AudibleAlert.none: (0, 0), AudibleAlert.chimeEngage: (CM.HIGH_CHIME, 1), diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 7167583136..a164f399f9 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -184,4 +184,4 @@ class CarController(object): # This prevents unexpected pedal range rescaling can_sends.append(create_gas_command(self.packer, apply_gas, idx)) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index b8164e8c52..72bca7e3e7 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -192,7 +192,7 @@ class CarState(object): # R = 1e3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], A=[[1.0, dt], [0.0, 1.0]], - C=[[1.0, 0.0]], + C=[1.0, 0.0], K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 6c4b4a7672..8c3a2bd76a 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -183,9 +183,8 @@ class CarInterface(object): # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - - ret.steerKf = 0.00006 # conservative feed-forward + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: stop_and_go = True @@ -197,13 +196,13 @@ class CarInterface(object): # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] if is_fw_modified: - ret.steerKf = 0.00004 + ret.lateralTuning.pid.kf = 0.00004 - ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [3.6, 2.4, 1.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.54, 0.36] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.54, 0.36] elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): stop_and_go = True @@ -214,11 +213,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 15.96 # 11.82 is spec end-to-end tire_stiffness_factor = 0.8467 - ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.ACURA_ILX: stop_and_go = False @@ -227,11 +226,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end tire_stiffness_factor = 0.72 - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.CRV: stop_and_go = False @@ -240,11 +239,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.3 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.CRV_5G: stop_and_go = True @@ -254,11 +253,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 - ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.CRV_HYBRID: stop_and_go = True @@ -268,11 +267,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 - ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.ACURA_RDX: stop_and_go = False @@ -281,11 +280,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY: stop_and_go = False @@ -294,11 +293,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec tire_stiffness_factor = 0.82 - ret.steerKpV, ret.steerKiV = [[0.45], [0.135]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.ODYSSEY_CHN: stop_and_go = False @@ -307,11 +306,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY ret.steerRatio = 14.35 # from CAR.ODYSSEY tire_stiffness_factor = 0.82 # from CAR.ODYSSEY - ret.steerKpV, ret.steerKiV = [[0.45], [0.135]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate in (CAR.PILOT, CAR.PILOT_2019): stop_and_go = False @@ -320,11 +319,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate == CAR.RIDGELINE: stop_and_go = False @@ -333,11 +332,11 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiBP = [0., 35.] - ret.longitudinalKiV = [0.18, 0.12] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] else: raise ValueError("unsupported car %s" % candidate) @@ -376,8 +375,8 @@ class CarInterface(object): ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed - ret.longPidDeadzoneBP = [0.] - ret.longPidDeadzoneV = [0.] + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] ret.stoppingControl = True ret.steerLimitAlert = True diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 627e27b4c4..82dbc0aed8 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -71,6 +71,6 @@ class CarController(object): can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) ### Send messages to canbus - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) self.cnt += 1 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index bd3e63740c..041360f4bb 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -2,7 +2,6 @@ from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from common.kalman.simple_kalman import KF1D -import numpy as np def get_can_parser(CP): @@ -133,10 +132,10 @@ class CarState(object): dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 - self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), - A=np.matrix([[1.0, dt], [0.0, 1.0]]), - C=np.matrix([1.0, 0.0]), - K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], + A=[[1.0, dt], [0.0, 1.0]], + C=[1.0, 0.0], + K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 self.left_blinker_on = 0 self.left_blinker_flash = 0 @@ -173,7 +172,7 @@ class CarState(object): # Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]]) + self.v_ego_kf.x = [[v_wheel], [0.0]] self.v_ego_raw = v_wheel v_ego_x = self.v_ego_kf.update(v_wheel) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6186565584..cc5d95d06d 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -74,7 +74,7 @@ class CarInterface(object): ret.steerRateCost = 0.5 if candidate == CAR.SANTA_FE: - ret.steerKf = 0.00005 + ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3982 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.766 @@ -82,56 +82,58 @@ class CarInterface(object): ret.steerRatio = 16.55 # 13.8 is spec end-to-end tire_stiffness_factor = 0.82 - ret.steerKiBP, ret.steerKpBP = [[9., 22.], [9., 22.]] - ret.steerKpV, ret.steerKiV = [[0.2, 0.35], [0.05, 0.09]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] ret.minSteerSpeed = 0. elif candidate == CAR.KIA_SORENTO: - ret.steerKf = 0.00005 + ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1985 + std_cargo ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.25], [0.05]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. elif candidate == CAR.ELANTRA: - ret.steerKf = 0.00006 + ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275 + std_cargo ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec tire_stiffness_factor = 0.385 - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.25], [0.05]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.GENESIS: - ret.steerKf = 0.00005 + ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060 + std_cargo ret.wheelbase = 3.01 ret.steerRatio = 16.5 - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.16], [0.01]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] ret.minSteerSpeed = 35 * CV.MPH_TO_MS elif candidate == CAR.KIA_OPTIMA: - ret.steerKf = 0.00005 + ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558 * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.25], [0.05]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_STINGER: - ret.steerKf = 0.00005 + ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1825 + std_cargo ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.25], [0.05]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this - ret.longitudinalKpBP = [0.] - ret.longitudinalKpV = [0.] - ret.longitudinalKiBP = [0.] - ret.longitudinalKiV = [0.] + ret.longitudinalTuning.kpBP = [0.] + ret.longitudinalTuning.kpV = [0.] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.] + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] ret.centerToFront = ret.wheelbase * 0.4 @@ -163,8 +165,6 @@ class CarInterface(object): ret.gasMaxV = [1.] ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] - ret.longPidDeadzoneBP = [0.] - ret.longPidDeadzoneV = [0.] ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.openpilotLongitudinalControl = False diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 3a5e05257c..bf9b8a5e74 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -56,8 +56,6 @@ class CarInterface(object): ret.wheelbase = 2.70 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 13. # reasonable - ret.longPidDeadzoneBP = [0.] - ret.longPidDeadzoneV = [0.] ret.tireStiffnessFront = 1e6 # very stiff to neglect slip ret.tireStiffnessRear = 1e6 # very stiff to neglect slip ret.steerRatioRear = 0. @@ -69,10 +67,12 @@ class CarInterface(object): ret.brakeMaxBP = [0.] ret.brakeMaxV = [0.] - ret.longitudinalKpBP = [0.] - ret.longitudinalKpV = [0.] - ret.longitudinalKiBP = [0.] - ret.longitudinalKiV = [0.] + ret.longitudinalTuning.kpBP = [0.] + ret.longitudinalTuning.kpV = [0.] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.] + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] ret.steerActuatorDelay = 0. return ret diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 33a0fde868..98deb56467 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -73,4 +73,4 @@ class CarController(object): can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 38d2fe4794..722b171182 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,5 +1,4 @@ import copy -import numpy as np from common.kalman.simple_kalman import KF1D from selfdrive.config import Conversions as CV from selfdrive.can.parser import CANParser @@ -26,6 +25,7 @@ def get_powertrain_can_parser(CP): ("DOOR_OPEN_FL", "BodyInfo", 1), ("DOOR_OPEN_RR", "BodyInfo", 1), ("DOOR_OPEN_RL", "BodyInfo", 1), + ("Units", "Dash_State", 1), ] checks = [ @@ -61,6 +61,10 @@ def get_camera_can_parser(CP): ("Signal3", "ES_LKAS_State", 0), ("LKAS_ENABLE_2", "ES_LKAS_State", 0), ("Signal4", "ES_LKAS_State", 0), + ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), + ("Signal6", "ES_LKAS_State", 0), + ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), + ("Signal7", "ES_LKAS_State", 0), ("FCW_Cont_Beep", "ES_LKAS_State", 0), ("FCW_Repeated_Beep", "ES_LKAS_State", 0), ("Throttle_Management_Activated", "ES_LKAS_State", 0), @@ -92,10 +96,10 @@ class CarState(object): # vEgo kalman filter dt = 0.01 - self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]), - A=np.matrix([[1., dt], [0., 1.]]), - C=np.matrix([1., 0.]), - K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego_kf = KF1D(x0=[[0.], [0.]], + A=[[1., dt], [0., 1.]], + C=[1., 0.], + K=[[0.12287673], [0.29666309]]) self.v_ego = 0. def update(self, cp, cp_cam): @@ -114,12 +118,15 @@ class CarState(object): self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS - self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.MPH_TO_KPH + self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] + # 1 = imperial, 6 = metric + if cp.vl["Dash_State"]['Units'] == 1: + self.v_cruise_pcm *= CV.MPH_TO_KPH v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. # Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]]) + self.v_ego_kf.x = [[v_wheel], [0.0]] self.v_ego_raw = v_wheel v_ego_x = self.v_ego_kf.update(v_wheel) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index ef3da2f398..8de0760b4b 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -66,9 +66,9 @@ class CarInterface(object): ret.steerRatio = 15 tire_stiffness_factor = 1.0 ret.steerActuatorDelay = 0.4 # end-to-end angle controller - ret.steerKf = 0.00005 - ret.steerKiBP, ret.steerKpBP = [[0., 20.], [0., 20.]] - ret.steerKpV, ret.steerKiV = [[0.2, 0.3], [0.02, 0.03]] + ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] @@ -81,12 +81,12 @@ class CarInterface(object): ret.gasMaxV = [0.] ret.brakeMaxBP = [0.] ret.brakeMaxV = [0.] - ret.longPidDeadzoneBP = [0.] - ret.longPidDeadzoneV = [0.] - ret.longitudinalKpBP = [0.] - ret.longitudinalKpV = [0.] - ret.longitudinalKiBP = [0.] - ret.longitudinalKiV = [0.] + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] + ret.longitudinalTuning.kpBP = [0.] + ret.longitudinalTuning.kpV = [0.] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.] # end from gm diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 18b8929564..0e444d7361 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -6,6 +6,10 @@ class CAR: FINGERPRINTS = { CAR.IMPREZA: [{ 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8 + }, + # Crosstrek 2018 (same platform as Subaru) + { + 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 256: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8 }], } diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d69636d32d..0bd5d90023 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -7,7 +7,7 @@ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ create_fcw_command -from selfdrive.car.toyota.values import ECU, STATIC_MSGS +from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSSP2_CAR from selfdrive.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -249,7 +249,7 @@ class CarController(object): if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)) - if frame % 100 == 0 and ECU.DSU in self.fake_ecus: + if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR: can_sends.append(create_fcw_command(self.packer, fcw)) #*** static msgs *** @@ -271,4 +271,4 @@ class CarController(object): can_sends.append(make_can_msg(addr, vl, bus, False)) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 02debc47bf..88471b61a5 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -97,10 +97,10 @@ class CarState(object): dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 - self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), - A=np.matrix([[1.0, dt], [0.0, 1.0]]), - C=np.matrix([1.0, 0.0]), - K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], + A=[[1.0, dt], [0.0, 1.0]], + C=[1.0, 0.0], + K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 def update(self, cp, cp_cam): @@ -133,7 +133,7 @@ class CarState(object): # Kalman filter if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]]) + self.v_ego_kf.x = [[v_wheel], [0.0]] self.v_ego_raw = v_wheel v_ego_x = self.v_ego_kf.update(v_wheel) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 0fe0d0821a..ea5b748d53 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser -from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR +from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR from selfdrive.swaglog import cloudlog try: @@ -73,8 +73,10 @@ class CarInterface(object): tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 - ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay + if candidate != CAR.PRIUS: + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] if candidate == CAR.PRIUS: stop_and_go = True @@ -83,10 +85,15 @@ class CarInterface(object): ret.steerRatio = 15.00 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045 * CV.LB_TO_KG + std_cargo - ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] - ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 - # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis - ret.steerActuatorDelay = 0.25 + + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGain = 4.0 + ret.lateralTuning.indi.outerLoopGain = 3.0 + ret.lateralTuning.indi.timeConstant = 1.0 + ret.lateralTuning.indi.actuatorEffectiveness = 1.0 + + ret.steerActuatorDelay = 0.5 + ret.steerRateCost = 0.5 elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -95,8 +102,8 @@ class CarInterface(object): ret.steerRatio = 16.30 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid - ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] - ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] + ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate == CAR.COROLLA: stop_and_go = False @@ -105,8 +112,8 @@ class CarInterface(object): ret.steerRatio = 17.8 tire_stiffness_factor = 0.444 ret.mass = 2860 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid - ret.steerKpV, ret.steerKiV = [[0.2], [0.05]] - ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] + ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RXH: stop_and_go = True @@ -115,8 +122,8 @@ class CarInterface(object): ret.steerRatio = 16. # 14.8 is spec end-to-end tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4481 * CV.LB_TO_KG + std_cargo # mean between min and max - ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] - ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] + ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 elif candidate in [CAR.CHR, CAR.CHRH]: stop_and_go = True @@ -125,8 +132,8 @@ class CarInterface(object): ret.steerRatio = 13.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + std_cargo - ret.steerKpV, ret.steerKiV = [[0.723], [0.0428]] - ret.steerKf = 0.00006 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]] + ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.CAMRY, CAR.CAMRYH]: stop_and_go = True @@ -135,8 +142,8 @@ class CarInterface(object): ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid - ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] - ret.steerKf = 0.00006 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] + ret.lateralTuning.pid.kf = 0.00006 elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: stop_and_go = True @@ -145,15 +152,42 @@ class CarInterface(object): ret.steerRatio = 16.0 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4607 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid limited - ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] - ret.steerKf = 0.00006 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] + ret.lateralTuning.pid.kf = 0.00006 + + elif candidate == CAR.AVALON: + stop_and_go = False + ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file + ret.wheelbase = 2.82 + ret.steerRatio = 14.8 #Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download + tire_stiffness_factor = 0.7983 + ret.mass = 3505 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]] + ret.lateralTuning.pid.kf = 0.00006 + + elif candidate == CAR.RAV4_2019: + stop_and_go = True + ret.safetyParam = 100 + ret.wheelbase = 2.68986 + ret.steerRatio = 14.3 + tire_stiffness_factor = 0.7933 + ret.mass = 3370. * CV.LB_TO_KG + std_cargo + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] + ret.lateralTuning.pid.kf = 0.00007818594 + + elif candidate == CAR.COROLLA_HATCH: + stop_and_go = True + ret.safetyParam = 100 + ret.wheelbase = 2.63906 + ret.steerRatio = 13.9 + tire_stiffness_factor = 0.444 + ret.mass = 3060. * CV.LB_TO_KG + std_cargo + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] + ret.lateralTuning.pid.kf = 0.00007818594 ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 - ret.longPidDeadzoneBP = [0., 9.] - ret.longPidDeadzoneV = [0., .15] - #detect the Pedal address ret.enableGasInterceptor = 0x201 in fingerprint @@ -196,21 +230,24 @@ class CarInterface(object): cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.steerLimitAlert = False - ret.longitudinalKpBP = [0., 5., 35.] - ret.longitudinalKiBP = [0., 35.] + + ret.longitudinalTuning.deadzoneBP = [0., 9.] + ret.longitudinalTuning.deadzoneV = [0., .15] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kiBP = [0., 35.] ret.stoppingControl = False ret.startAccel = 0.0 if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] ret.gasMaxV = [0.2, 0.5, 0.7] - ret.longitudinalKpV = [1.2, 0.8, 0.5] - ret.longitudinalKiV = [0.18, 0.12] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiV = [0.18, 0.12] else: ret.gasMaxBP = [0.] ret.gasMaxV = [0.5] - ret.longitudinalKpV = [3.6, 2.4, 1.5] - ret.longitudinalKiV = [0.54, 0.36] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiV = [0.54, 0.36] return ret @@ -270,7 +307,7 @@ class CarInterface(object): ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. - if self.CP.carFingerprint in [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER] or self.CP.enableGasInterceptor: + if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: # ignore standstill in hybrid vehicles, since pcm allows to restart without # receiving any special command # also if interceptor is detected @@ -369,7 +406,7 @@ class CarInterface(object): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert, self.forwarding_camera, - c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, + c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 054ce0b7be..fc6f3e5234 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -7,39 +7,48 @@ from cereal import car from common.realtime import sec_since_boot from selfdrive.services import service_list import selfdrive.messaging as messaging -from selfdrive.car.toyota.values import NO_DSU_CAR +from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSSP2_CAR +def _create_radard_can_parser(car_fingerprint): + dbc_f = DBC[car_fingerprint]['radar'] -RADAR_A_MSGS = list(range(0x210, 0x220)) -RADAR_B_MSGS = list(range(0x220, 0x230)) - -def _create_radard_can_parser(): - dbc_f = 'toyota_prius_2017_adas.dbc' + if car_fingerprint in TSSP2_CAR: + RADAR_A_MSGS = list(range(0x180, 0x190)) + RADAR_B_MSGS = list(range(0x190, 0x1a0)) + else: + RADAR_A_MSGS = list(range(0x210, 0x220)) + RADAR_B_MSGS = list(range(0x220, 0x230)) msg_a_n = len(RADAR_A_MSGS) msg_b_n = len(RADAR_B_MSGS) - signals = list(zip( - ['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + - ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, - RADAR_A_MSGS * 5 + RADAR_B_MSGS, - [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)) + signals = zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + + ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, + RADAR_A_MSGS * 5 + RADAR_B_MSGS, + [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n) - checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n))) + checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) - class RadarInterface(object): def __init__(self, CP): # radar self.pts = {} - self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.track_id = 0 self.delay = 0.0 # Delay of radar - self.rcp = _create_radard_can_parser() + if CP.carFingerprint in TSSP2_CAR: + self.RADAR_A_MSGS = list(range(0x180, 0x190)) + self.RADAR_B_MSGS = list(range(0x190, 0x1a0)) + else: + self.RADAR_A_MSGS = list(range(0x210, 0x220)) + self.RADAR_B_MSGS = list(range(0x220, 0x230)) + + self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS} + + self.rcp = _create_radard_can_parser(CP.carFingerprint) self.no_dsu_car = CP.carFingerprint in NO_DSU_CAR context = zmq.Context() @@ -48,6 +57,7 @@ class RadarInterface(object): def update(self): ret = car.RadarState.new_message() + if self.no_dsu_car: # TODO: make a adas dbc file for dsu-less models time.sleep(0.05) @@ -58,7 +68,7 @@ class RadarInterface(object): while 1: tm = int(sec_since_boot() * 1e9) updated_messages.update(self.rcp.update(tm, True)) - if RADAR_B_MSGS[-1] in updated_messages: + if self.RADAR_B_MSGS[-1] in updated_messages: break errors = [] @@ -68,7 +78,7 @@ class RadarInterface(object): ret.canMonoTimes = canMonoTimes for ii in updated_messages: - if ii in RADAR_A_MSGS: + if ii in self.RADAR_A_MSGS: cpt = self.rcp.vl[ii] if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 72224334a4..52e3d1510c 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -12,6 +12,9 @@ class CAR: CAMRYH = "TOYOTA CAMRY HYBRID 2018" HIGHLANDER = "TOYOTA HIGHLANDER 2017" HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" + AVALON = "TOYOTA AVALON 2016" + RAV4_2019 = "TOYOTA RAV4 2019" + COROLLA_HATCH = "TOYOTA COROLLA HATCH 2019" class ECU: @@ -22,39 +25,39 @@ class ECU: # addr: (ecu, cars, bus, 1/freq*100, vl) STATIC_MSGS = [ - (0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), - (0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), - (0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 40, '\x06\x00'), - (0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), + (0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), + (0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), + (0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, '\x06\x00'), + (0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), (0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x20\x20\xAD'), - (0x466, ECU.CAM, (CAR.COROLLA), 1, 100, '\x24\x20\xB1'), - (0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - (0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - (0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), - (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), + (0x466, ECU.CAM, (CAR.COROLLA, CAR.AVALON), 1, 100, '\x24\x20\xB1'), + (0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + (0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + (0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), + (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), - (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 3, '\xf4\x01\x90\x83\x00\x37'), + (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, '\xf4\x01\x90\x83\x00\x37'), (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 3, '\x03\x00\x20\x00\x00\x52'), - (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 2, '\x00\x00\x00\x46'), - (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), + (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 2, '\x00\x00\x00\x46'), + (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), + (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), (0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), (0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), (0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), (0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), - (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), + (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), (0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), + (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), (0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'), (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H), 1, 100, '\x00\x00\x01\x79'), - (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), (0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), (0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), @@ -123,9 +126,9 @@ FINGERPRINTS = { 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.CAMRYH: [ - #LE and LE with Blindspot Monitor + #SE, LE and LE with Blindspot Monitor { - 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, #SL { @@ -141,6 +144,15 @@ FINGERPRINTS = { CAR.HIGHLANDERH: [{ 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.AVALON: [{ + 36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], + CAR.RAV4_2019: [{ + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], + CAR.COROLLA_HATCH: [{ + 36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8 , 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7 , 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000:8, 1001: 8, 1002: 8, 1017:8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059:1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }] } STEER_THRESHOLD = 100 @@ -157,6 +169,11 @@ DBC = { CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'), CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_prius_2017_adas'), CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'), + CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.RAV4_2019: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_rav4_2019_adas'), + CAR.COROLLA_HATCH: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_rav4_2019_adas'), } NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH] +TSSP2_CAR = [CAR.RAV4_2019, CAR.COROLLA_HATCH] +NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_2019, CAR.COROLLA_HATCH] # no resume button press required diff --git a/selfdrive/common/cereal.mk b/selfdrive/common/cereal.mk index 46f23be551..0a3958a23f 100644 --- a/selfdrive/common/cereal.mk +++ b/selfdrive/common/cereal.mk @@ -5,11 +5,7 @@ UNAME_S ?= $(shell uname -s) CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include -ifeq ($(OPTEST),1) - -CEREAL_LIBS = -lcapnp -lkj - -else ifeq ($(UNAME_S),Darwin) +ifeq ($(UNAME_S),Darwin) CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \ @@ -33,9 +29,9 @@ ifeq ($(CEREAL_LIBS),) -L$(PHONELIBS)/capnp-c/aarch64/lib/ \ -l:libcapn.a -l:libcapnp.a -l:libkj.a endif - endif + CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++ diff --git a/selfdrive/common/util.c b/selfdrive/common/util.c index 6f8f0a9a6c..01b8a0b6d9 100644 --- a/selfdrive/common/util.c +++ b/selfdrive/common/util.c @@ -26,6 +26,7 @@ void* read_file(const char* path, size_t* out_len) { fclose(f); if (num_read != 1) { + free(buf); return NULL; } diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index fd8219990d..f2f73bbe08 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.5.11-release" +#define COMMA_VERSION "0.5.12-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 31dcbef075..bbc90976b3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,6 +2,8 @@ import gc import zmq import json +from collections import defaultdict + from cereal import car, log from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper @@ -19,7 +21,8 @@ from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \ update_v_cruise, \ initialize_v_cruise from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED -from selfdrive.controls.lib.latcontrol import LatControl +from selfdrive.controls.lib.latcontrol_pid import LatControlPID +from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.driver_monitor import DriverStatus @@ -40,7 +43,7 @@ def isEnabled(state): return (isActive(state) or state == State.preEnabled) -def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor, +def data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params, plan, path_plan): """Receive data from sockets and create events for battery, temperature and disk space""" @@ -57,18 +60,21 @@ def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, dm = None for socket, event in poller.poll(0): + msg = messaging.recv_one(socket) + rcv_times[msg.which()] = sec_since_boot() + if socket is thermal: - td = messaging.recv_one(socket) + td = msg elif socket is calibration: - cal = messaging.recv_one(socket) + cal = msg elif socket is health: - hh = messaging.recv_one(socket) + hh = msg elif socket is driver_monitor: - dm = messaging.recv_one(socket) + dm = msg elif socket is plan_sock: - plan = messaging.recv_one(socket) + plan = msg elif socket is path_plan_sock: - path_plan = messaging.recv_one(socket) + path_plan = msg if td is not None: overtemp = td.thermal.thermalStatus >= ThermalStatus.red @@ -202,7 +208,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last -def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, +def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc): """Given the state, this function returns an actuators packet""" @@ -246,11 +252,11 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise path_plan.cPoly, path_plan.cProb, CS.steeringAngle, CS.steeringPressed) - cur_time = sec_since_boot() # TODO: This won't work in replay - mpc_time = plan.l20MonoTime / 1e9 + cur_time = sec_since_boot() + radar_time = rcv_times['plan'] - plan.processingDelay # Subtract processing delay to get the original measurement time _DT = 0.01 # 100Hz - dt = min(cur_time - mpc_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps + dt = min(cur_time - radar_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps a_acc_sol = plan.aStart + (dt / _DT_MPC) * (plan.aTarget - plan.aStart) v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 @@ -258,8 +264,8 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, - CS.steeringPressed, CP, VM, path_plan) + actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, + CS.steeringPressed, CP, VM, path_plan) # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: @@ -278,12 +284,12 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise AM.process_alerts(sec_since_boot()) - return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol + return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol, lac_log def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, AM, driver_status, - LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc): + LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log): """Send actuators and hud commands to the car, send live100 and MPC logging""" plan_ts = plan.logMonoTime plan = plan.plan @@ -361,9 +367,6 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), - "upSteer": float(LaC.pid.p), - "uiSteer": float(LaC.pid.i), - "ufSteer": float(LaC.pid.f), "vTargetLead": float(v_acc), "aTarget": float(a_acc), "jerkFactor": float(plan.jerkFactor), @@ -372,10 +375,15 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis "vCurvature": plan.vCurvature, "decelForTurn": plan.decelForTurn, "cumLagMs": -rk.remaining * 1000., - "startMonoTime": start_time, + "startMonoTime": int(start_time * 1e9), "mapValid": plan.mapValid, "forceDecel": bool(force_decel), } + + if CP.lateralTuning.which() == 'pid': + dat.live100.lateralControlState.pidState = lac_log + else: + dat.live100.lateralControlState.indiState = lac_log live100.send(dat.to_bytes()) # carState @@ -443,7 +451,12 @@ def controlsd_thread(gctx=None, rate=100): LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) - LaC = LatControl(CP) + + if CP.lateralTuning.which() == 'pid': + LaC = LatControlPID(CP) + else: + LaC = LatControlINDI(CP) + AM = AlertManager() driver_status = DriverStatus() @@ -465,6 +478,8 @@ def controlsd_thread(gctx=None, rate=100): mismatch_counter = 0 low_battery = False + rcv_times = defaultdict(int) + plan = messaging.new_message() plan.init('plan') path_plan = messaging.new_message() @@ -485,23 +500,30 @@ def controlsd_thread(gctx=None, rate=100): prof = Profiler(False) # off by default while True: - start_time = int(sec_since_boot() * 1e9) + start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\ - data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor, + data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params, plan, path_plan) prof.checkpoint("Sample") - path_plan_age = (start_time - path_plan.logMonoTime) / 1e9 - plan_age = (start_time - plan.logMonoTime) / 1e9 + # Create alerts + path_plan_age = start_time - rcv_times['pathPlan'] + plan_age = start_time - rcv_times['plan'] + if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5: events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not path_plan.pathPlan.paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) - events += list(plan.plan.events) + if not path_plan.pathPlan.modelValid: + events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not plan.plan.radarValid: + events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if plan.plan.radarCommIssue: + events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: @@ -514,8 +536,8 @@ def controlsd_thread(gctx=None, rate=100): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc = \ - state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph, + actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc, lac_log = \ + state_control(rcv_times, plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc) @@ -523,7 +545,7 @@ def controlsd_thread(gctx=None, rate=100): # Publish data CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, - live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc) + live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log) prof.checkpoint("Sent") rk.keep_time() # Run at 100Hz diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 15d5a202f7..28e35dddf8 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,7 +1,9 @@ from cereal import car -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from selfdrive.config import Conversions as CV +DT = 0.01 # Controlsd runs at 100Hz + # kph V_CRUISE_MAX = 144 V_CRUISE_MIN = 8 @@ -55,6 +57,10 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) +def get_steer_max(CP, v_ego): + return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) + + def learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override): # simple integral controller that learns how much steering offset to put to have the car going straight # while being in the middle of the lane diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 9af3cb1b10..814d77a6a6 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -16,6 +16,7 @@ _PITCH_WEIGHT = 1.5 # pitch matters a lot more _METRIC_THRESHOLD = 0.4 _PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch _PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up +_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) _STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid _DISTRACTED_FILTER_TS = 0.25 # 0.6Hz _VARIANCE_FILTER_TS = 20. # 0.008Hz @@ -89,7 +90,7 @@ class DriverStatus(): def _is_driver_distracted(self, pose): # to be tuned and to learn the driver's normal pose pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET - yaw_error = pose.yaw + yaw_error = pose.yaw - _YAW_NATURAL_OFFSET # add positive pitch allowance if pitch_error > 0.: pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.) diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py new file mode 100644 index 0000000000..2e95489bc5 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -0,0 +1,104 @@ +import math +import numpy as np + +from selfdrive.car.toyota.carcontroller import SteerLimitParams +from selfdrive.car import apply_toyota_steer_torque_limits +from selfdrive.controls.lib.drive_helpers import get_steer_max, DT +from common.numpy_fast import clip +from cereal import log + + +class LatControlINDI(object): + def __init__(self, CP): + self.angle_steers_des = 0. + + A = np.matrix([[1.0, DT, 0.0], + [0.0, 1.0, DT], + [0.0, 0.0, 1.0]]) + C = np.matrix([[1.0, 0.0, 0.0], + [0.0, 1.0, 0.0]]) + + # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]]) + # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]]) + + # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) + # K = np.transpose(K) + K = np.matrix([[7.30262179e-01, 2.07003658e-04], + [7.29394177e+00, 1.39159419e-02], + [1.71022442e+01, 3.38495381e-02]]) + + self.K = K + self.A_K = A - np.dot(K, C) + self.x = np.matrix([[0.], [0.], [0.]]) + + self.enfore_rate_limit = CP.carName == "toyota" + + self.RC = CP.lateralTuning.indi.timeConstant + self.G = CP.lateralTuning.indi.actuatorEffectiveness + self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain + self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain + self.alpha = 1. - DT / (self.RC + DT) + + self.reset() + + def reset(self): + self.delayed_output = 0. + self.output_steer = 0. + self.counter = 0 + + def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan): + # Update Kalman filter + y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]]) + self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) + + indi_log = log.Live100Data.LateralINDIState.new_message() + indi_log.steerAngle = math.degrees(self.x[0]) + indi_log.steerRate = math.degrees(self.x[1]) + indi_log.steerAccel = math.degrees(self.x[2]) + + if v_ego < 0.3 or not active: + indi_log.active = False + self.output_steer = 0.0 + self.delayed_output = 0.0 + else: + self.angle_steers_des = path_plan.angleSteers + self.rate_steers_des = path_plan.rateSteers + + steers_des = math.radians(self.angle_steers_des) + rate_des = math.radians(self.rate_steers_des) + + # Expected actuator value + self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha) + + # Compute acceleration error + rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des + accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) + accel_error = accel_sp - self.x[2] + + # Compute change in actuator + g_inv = 1. / self.G + delta_u = g_inv * accel_error + + # Enforce rate limit + if self.enfore_rate_limit: + steer_max = float(SteerLimitParams.STEER_MAX) + new_output_steer_cmd = steer_max * (self.delayed_output + delta_u) + prev_output_steer_cmd = steer_max * self.output_steer + new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams) + self.output_steer = new_output_steer_cmd / steer_max + else: + self.output_steer = self.delayed_output + delta_u + + steers_max = get_steer_max(CP, v_ego) + self.output_steer = clip(self.output_steer, -steers_max, steers_max) + + indi_log.active = True + indi_log.rateSetPoint = float(rate_sp) + indi_log.accelSetPoint = float(accel_sp) + indi_log.accelError = float(accel_error) + indi_log.delayedOutput = float(self.delayed_output) + indi_log.delta = float(delta_u) + indi_log.output = float(self.output_steer) + + self.sat_flag = False + return float(self.output_steer), float(self.angle_steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol_pid.py similarity index 63% rename from selfdrive/controls/lib/latcontrol.py rename to selfdrive/controls/lib/latcontrol_pid.py index 6949438708..600990aaa3 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,29 +1,27 @@ from selfdrive.controls.lib.pid import PIController -from common.numpy_fast import interp +from selfdrive.controls.lib.drive_helpers import get_steer_max from cereal import car +from cereal import log -_DT = 0.01 # 100Hz -_DT_MPC = 0.05 # 20Hz - -def get_steer_max(CP, v_ego): - return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) - - -class LatControl(object): +class LatControlPID(object): def __init__(self, CP): - self.pid = PIController((CP.steerKpBP, CP.steerKpV), - (CP.steerKiBP, CP.steerKiV), - k_f=CP.steerKf, pos_limit=1.0) - self.last_cloudlog_t = 0.0 + self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), + (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0) self.angle_steers_des = 0. def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan): + def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan): + pid_log = log.Live100Data.LateralPIDState.new_message() + pid_log.steerAngle = float(angle_steers) + pid_log.steerRate = float(angle_steers_rate) + if v_ego < 0.3 or not active: output_steer = 0.0 + pid_log.active = False self.pid.reset() else: # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution @@ -43,6 +41,12 @@ class LatControl(object): deadzone = 0.0 output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) + pid_log.active = True + pid_log.p = self.pid.p + pid_log.i = self.pid.i + pid_log.f = self.pid.f + pid_log.output = output_steer + pid_log.saturated = bool(self.pid.saturated) self.sat_flag = self.pid.saturated - return output_steer, float(self.angle_steers_des) + return output_steer, float(self.angle_steers_des), pid_log diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index da324a39a1..0b1eefea77 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -58,8 +58,8 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, class LongControl(object): def __init__(self, CP, compute_gb): self.long_control_state = LongCtrlState.off # initialized to off - self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV), - (CP.longitudinalKiBP, CP.longitudinalKiV), + self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), + (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=RATE, sat_limit=0.8, convert=compute_gb) @@ -99,7 +99,7 @@ class LongControl(object): # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 - deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV) + deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp index 1f0e28d005..a34fbc3510 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -32,7 +32,7 @@ int main( ) // Running cost Function h; - h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)); + h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1; h << (d_l - desired) / (0.05 * v_ego + 0.5); h << a_ego * (0.1 * v_ego + 1.0); h << j_ego * (0.1 * v_ego + 1.0); @@ -42,7 +42,7 @@ int main( ) // Terminal cost Function hN; - hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)); + hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1; hN << (d_l - desired) / (0.05 * v_ego + 0.5); hN << a_ego * (0.1 * v_ego + 1.0); diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h index a7e8651b34..7766b9c973 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c50ba5b1353617c7a18d17f1417c4036d8e5c41db79f82cb9a9b4a19032e6cc6 +oid sha256:c595a94faa677114ead7debf35865d576e4eab09c0969a2ab1f2c9adfd143163 size 8752 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o index 839a64035d..1792de3769 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d8648ab44187e2be8e7cdb128249dfac75338d98d0d93ca9878ea4a4b3f2ad3c +oid sha256:825e0c17d93fe11ec1d537da26ae0a0ecbd92ed2662c20fb0da9e5c9f9e29b1e size 2992 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c index 64bc6db8df..a8e9583a01 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a58b02a90b3776ce308ab77094ebd8cdd23e788e1501a4c050862e65e827b185 -size 349038 +oid sha256:a0628f3d3f8287d3c8807edbeb918e50e11e52cbae6dd3e5f42ab849f96385a3 +size 349063 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o index c1cf63df7c..325eb847be 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:16e942a5676e23d49e01b17ff664a55bddffa9e7d054caf4f6dbb03d16ec2e9e -size 274768 +oid sha256:b65f59656003d5dca321aabebff4f3ba09b45813f74381ff3fba0f0b15c92931 +size 274744 diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so index c2d92939b6..31490dc83c 100755 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6c30e9afb75af10d72d0ea5e30b06c0d5310cce9312a441a2d260eb82d66f5ea +oid sha256:c980d0f55f9208270b328c358cfbc5f681d9a8da90b78568dc222e9352561b75 size 405136 diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o index db80688516..09380d29fd 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3b76bbb87fa3725ded4f4372174c3c7e4e5a58b86b950cd8867222f4cf3c375b +oid sha256:88e99546f68b7838c5a32a60bb8f209ed4e9e30d6461930f5c7ea80224899ec6 size 3136 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 40628ad09f..5a0330ff21 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -49,7 +49,7 @@ class PathPlanner(object): self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 - def update(self, CP, VM, CS, md, live100, live_parameters): + def update(self, rcv_times, CP, VM, CS, md, live100, live_parameters): v_ego = CS.carState.vEgo angle_steers = CS.carState.steeringAngle active = live100.live100.active @@ -104,6 +104,8 @@ class PathPlanner(object): else: self.invalid_counter = 0 + cur_time = sec_since_boot() + model_dead = cur_time - rcv_times['model'] > 0.5 plan_valid = self.invalid_counter < 2 plan_send = messaging.new_message() @@ -121,6 +123,7 @@ class PathPlanner(object): plan_send.pathPlan.angleOffset = float(angle_offset_average) plan_send.pathPlan.valid = bool(plan_valid) plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid) + plan_send.pathPlan.modelValid = bool(not model_dead) self.plan.send(plan_send.to_bytes()) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 1d5b32c8fb..f4b2bf88c6 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -6,10 +6,11 @@ from common.params import Params from common.numpy_fast import interp import selfdrive.messaging as messaging +from cereal import car +from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.services import service_list -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED from selfdrive.controls.lib.fcw import FCWChecker @@ -113,9 +114,9 @@ class Planner(object): self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) - def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data): + def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data): """Gets called when new live20 is available""" - cur_time = live20.logMonoTime / 1e9 + cur_time = sec_since_boot() v_ego = CS.carState.vEgo long_control_state = live100.live100.longControlState @@ -131,11 +132,13 @@ class Planner(object): v_speedlimit = NO_CURVATURE_SPEED v_curvature = NO_CURVATURE_SPEED - map_valid = live_map_data.liveMapData.mapValid + + map_age = cur_time - rcv_times['liveMapData'] + map_valid = live_map_data.liveMapData.mapValid and map_age < 10.0 # Speed limit and curvature set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None - if set_speed_limit_active: + if set_speed_limit_active and map_valid: if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit offset = float(self.params.get("SpeedLimitOffset")) @@ -206,24 +209,16 @@ class Planner(object): if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) - model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5 + radar_dead = cur_time - rcv_times['live20'] > 0.5 + + radar_errors = list(live20.live20.radarErrors) + radar_fault = car.RadarState.Error.fault in radar_errors + radar_comm_issue = car.RadarState.Error.commIssue in radar_errors # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') - # TODO: Move all these events to controlsd. This has nothing to do with planning - events = [] - if model_dead: - events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - - radar_errors = list(live20.live20.radarErrors) - if 'commIssue' in radar_errors: - events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if 'fault' in radar_errors: - events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - - plan_send.plan.events = events plan_send.plan.mdMonoTime = md.logMonoTime plan_send.plan.l20MonoTime = live20.logMonoTime @@ -242,6 +237,12 @@ class Planner(object): plan_send.plan.decelForTurn = decel_for_turn plan_send.plan.mapValid = map_valid + radar_valid = not (radar_dead or radar_fault) + plan_send.plan.radarValid = bool(radar_valid) + plan_send.plan.radarCommIssue = bool(radar_comm_issue) + + plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - rcv_times['live20'] + # Send out fcw fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index da7d3f71ba..99f34dfd5c 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -27,7 +27,7 @@ v_ego_stationary = 4. # no stationary object flag below this speed # Lead Kalman Filter params _VLEAD_A = [[1.0, ts], [0.0, 1.0]] -_VLEAD_C = [[1.0, 0.0]] +_VLEAD_C = [1.0, 0.0] #_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]]) #_VLEAD_R = 1e3 #_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]]) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 2a95375408..5946f6192e 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,8 +1,10 @@ #!/usr/bin/env python import zmq +from collections import defaultdict from cereal import car from common.params import Params +from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.services import service_list from selfdrive.controls.lib.planner import Planner @@ -52,22 +54,27 @@ def plannerd_thread(): live_parameters.liveParameters.steerRatio = CP.steerRatio live_parameters.liveParameters.stiffnessFactor = 1.0 + rcv_times = defaultdict(int) + while True: for socket, event in poller.poll(): + msg = messaging.recv_one(socket) + rcv_times[msg.which()] = sec_since_boot() + if socket is live100_sock: - live100 = messaging.recv_one(socket) + live100 = msg elif socket is car_state_sock: - car_state = messaging.recv_one(socket) + car_state = msg elif socket is live_parameters_sock: - live_parameters = messaging.recv_one(socket) + live_parameters = msg elif socket is model_sock: - model = messaging.recv_one(socket) - PP.update(CP, VM, car_state, model, live100, live_parameters) + model = msg + PP.update(rcv_times, CP, VM, car_state, model, live100, live_parameters) elif socket is live_map_data_sock: - live_map_data = messaging.recv_one(socket) + live_map_data = msg elif socket is live20_sock: - live20 = messaging.recv_one(socket) - PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data) + live20 = msg + PL.update(rcv_times, car_state, CP, VM, PP, live20, live100, model, live_map_data) def main(gctx=None): diff --git a/selfdrive/controls/tests/__init__.py b/selfdrive/controls/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py new file mode 100644 index 0000000000..a3cb636012 --- /dev/null +++ b/selfdrive/controls/tests/test_following_distance.py @@ -0,0 +1,90 @@ +import unittest +import numpy as np + +from cereal import log +import selfdrive.messaging as messaging +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.planner import calc_cruise_accel_limits +from selfdrive.controls.lib.speed_smoother import speed_smoother +from selfdrive.controls.lib.long_mpc import LongitudinalMpc + + +def RW(v_ego, v_l): + TR = 1.8 + G = 9.81 + return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G)) + + +class FakeSocket(object): + def send(self, data): + assert data + + +def run_following_distance_simulation(v_lead, t_end=200.0): + dt = 0.2 + t = 0. + + x_lead = 200.0 + + x_ego = 0.0 + v_ego = v_lead + a_ego = 0.0 + + v_cruise_setpoint = v_lead + 10. + + mpc = LongitudinalMpc(1, FakeSocket()) + + first = True + while t < t_end: + # Run cruise control + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)] + jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] + v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint, + accel_limits[1], accel_limits[0], + jerk_limits[1], jerk_limits[0], + dt) + + # Setup CarState + CS = messaging.new_message() + CS.init('carState') + CS.carState.vEgo = v_ego + CS.carState.aEgo = a_ego + + # Setup lead packet + lead = log.Live20Data.LeadData.new_message() + lead.status = True + lead.dRel = x_lead - x_ego + lead.vLead = v_lead + lead.aLeadK = 0.0 + + # Run MPC + mpc.set_cur_state(v_ego, a_ego) + if first: # Make sure MPC is converged on first timestep + for _ in range(20): + mpc.update(CS, lead, v_cruise_setpoint) + mpc.update(CS, lead, v_cruise_setpoint) + + # Choose slowest of two solutions + if v_cruise < mpc.v_mpc: + v_ego, a_ego = v_cruise, a_cruise + else: + v_ego, a_ego = mpc.v_mpc, mpc.a_mpc + + # Update state + x_lead += v_lead * dt + x_ego += v_ego * dt + t += dt + first = False + + return x_lead - x_ego + + +class TestFollowingDistance(unittest.TestCase): + def test_following_distanc(self): + for speed_mph in np.linspace(10, 100, num=10): + v_lead = float(speed_mph * CV.MPH_TO_MS) + + simulation_steady_state = run_following_distance_simulation(v_lead) + correct_steady_state = RW(v_lead, v_lead) + 4.0 + + self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=0.1) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index 48cca99f91..3ba96fd005 100644 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -5,6 +5,7 @@ import sys import numpy as np import argparse import re +from collections import defaultdict ''' System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs. @@ -27,9 +28,11 @@ PRINT_INTERVAL = 5 SLEEP_INTERVAL = 0.2 monitored_proc_names = [ - 'ubloxd', 'thermald', 'uploader', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned', + 'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned', 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'calibrationd', 'locationd', 'visiond', 'sensord', 'updated', 'gpsd', 'athena'] +cpu_time_names = ['user', 'system', 'children_user', 'children_system'] +timer = getattr(time, 'monotonic', time.time) def get_arg_parser(): parser = argparse.ArgumentParser( @@ -38,8 +41,10 @@ def get_arg_parser(): parser.add_argument("proc_names", nargs="?", default='', help="Process names to be monitored, comma seperated") - parser.add_argument("--list_all", nargs="?", type=bool, default=False, + parser.add_argument("--list_all", action='store_true', help="Show all running processes' cmdline") + parser.add_argument("--detailed_times", action='store_true', + help="show cpu time details (split by user, system, child user, child system)") return parser @@ -61,39 +66,54 @@ if __name__ == "__main__": if matched: k = ' '.join(p.cmdline()) print('Add monitored proc:', k) - stats[k] = {'cpu_samples': [], 'avg_cpu': None, 'min': None, 'max': None} + stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None), + 'avg': defaultdict(lambda: 0.0), 'last_cpu_times': None, 'last_sys_time':None} + stats[k]['last_sys_time'] = timer() + stats[k]['last_cpu_times'] = p.cpu_times() monitored_procs.append(p) i = 0 interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL) while True: for p in monitored_procs: k = ' '.join(p.cmdline()) - stats[k]['cpu_samples'].append(p.cpu_percent()) + cur_sys_time = timer() + cur_cpu_times = p.cpu_times() + cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time']) + stats[k]['last_sys_time'] = cur_sys_time + stats[k]['last_cpu_times'] = cur_cpu_times + cpu_percent = 0 + for num, name in enumerate(cpu_time_names): + stats[k]['cpu_samples'][name].append(cpu_times[num]) + cpu_percent += cpu_times[num] + stats[k]['cpu_samples']['total'].append(cpu_percent) time.sleep(SLEEP_INTERVAL) i += 1 if i % interval_int == 0: l = [] - avg_cpus = [] for k, stat in stats.items(): if len(stat['cpu_samples']) <= 0: continue - avg_cpu = np.array(stat['cpu_samples']).mean() - c = len(stat['cpu_samples']) - stat['cpu_samples'] = [] - if not stat['avg_cpu']: - stat['avg_cpu'] = avg_cpu - else: - stat['avg_cpu'] = (stat['avg_cpu'] * (c + i) + avg_cpu * c) / (c + i + c) - if not stat['min'] or avg_cpu < stat['min']: - stat['min'] = avg_cpu - if not stat['max'] or avg_cpu > stat['max']: - stat['max'] = avg_cpu - msg = 'avg: {1:.2f}%, min: {2:.2f}%, max: {3:.2f}% {0}'.format(os.path.basename(k), stat['avg_cpu'], stat['min'], stat['max']) - l.append((os.path.basename(k), avg_cpu, msg)) - avg_cpus.append(avg_cpu) + for name, samples in stat['cpu_samples'].iteritems(): + samples = np.array(samples) + avg = samples.mean() + c = samples.size + min_cpu = np.amin(samples) + max_cpu = np.amax(samples) + if stat['min'][name] is None or min_cpu < stat['min'][name]: + stat['min'][name] = min_cpu + if stat['max'][name] is None or max_cpu > stat['max'][name]: + stat['max'][name] = max_cpu + stat['avg'][name] = (stat['avg'][name] * (i - c) + avg * c) / (i) + stat['cpu_samples'][name] = [] + + msg = 'avg: {1:.2%}, min: {2:.2%}, max: {3:.2%} {0}'.format(os.path.basename(k), stat['avg']['total'], stat['min']['total'], stat['max']['total']) + if args.detailed_times: + for stat_type in ['avg', 'min', 'max']: + msg += '\n {}: {}'.format(stat_type, [name + ':' + str(round(stat[stat_type][name]*100, 2)) for name in cpu_time_names]) + l.append((os.path.basename(k), stat['avg']['total'], msg)) l.sort(key= lambda x: -x[1]) for x in l: print(x[2]) - print('avg sum: {0:.2f}%\n'.format( - sum([stat['avg_cpu'] for k, stat in stats.items()]) + print('avg sum: {0:.2%} over {1} samples {2} seconds\n'.format( + sum([stat['avg']['total'] for k, stat in stats.items()]), i, i * SLEEP_INTERVAL )) diff --git a/selfdrive/debug/tuner.py b/selfdrive/debug/tuner.py new file mode 100755 index 0000000000..066b0841ab --- /dev/null +++ b/selfdrive/debug/tuner.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python +""" +This tool can be used to quickly changes the values in a JSON file used for tuning +Keys like in vim: + - h: decrease by 0.05 + - l: increase by 0.05 + - k: move pointer up + - j: move pointer down +""" + +import tty +import sys +import json +import termios +from collections import OrderedDict + +FILENAME = '/data/tuning.json' + +def read_tuning(): + while True: + try: + return json.loads(open(FILENAME).read()) + except: + pass + +def main(): + dat = json.loads(open(FILENAME, 'r').read()) + dat = OrderedDict(sorted(dat.items(), key=lambda i: i[0])) + + cur = 0 + while True: + sys.stdout.write("\x1Bc") + + for i, (k, v) in enumerate(dat.items()): + prefix = "> " if cur == i else " " + print((prefix + k).ljust(20) + "%.2f" % v) + + key = sys.stdin.read(1)[0] + + write = False + if key == "k": + cur = max(0, cur - 1) + elif key == "j": + cur = min(len(dat.keys()) - 1, cur + 1) + elif key == "l": + dat[dat.keys()[cur]] += 0.05 + write = True + elif key == "h": + dat[dat.keys()[cur]] -= 0.05 + write = True + elif key == "q": + break + + if write: + open(FILENAME, 'w').write(json.dumps(dat)) + + +if __name__ == "__main__": + orig_settings = termios.tcgetattr(sys.stdin) + tty.setcbreak(sys.stdin) + + try: + main() + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings) + except: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings) + raise diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore new file mode 100644 index 0000000000..6c8271a885 --- /dev/null +++ b/selfdrive/locationd/.gitignore @@ -0,0 +1,2 @@ +ubloxd +ubloxd_test diff --git a/selfdrive/locationd/kalman/.gitignore b/selfdrive/locationd/kalman/.gitignore new file mode 100644 index 0000000000..9d357c72cc --- /dev/null +++ b/selfdrive/locationd/kalman/.gitignore @@ -0,0 +1,4 @@ +lane.cpp +gnss.cpp +loc*.cpp +pos_computer*.cpp diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py new file mode 100644 index 0000000000..01a91d7749 --- /dev/null +++ b/selfdrive/loggerd/deleter.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +import os +import shutil +import threading +from selfdrive.swaglog import cloudlog +from selfdrive.loggerd.config import ROOT +from selfdrive.loggerd.uploader import listdir_by_creation_date + + +def deleter_thread(exit_event): + while not exit_event.is_set(): + statvfs = os.statvfs(ROOT) + available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks + + if available_percent < 10.0: + # remove the earliest directory we can + dirs = listdir_by_creation_date(ROOT) + for delete_dir in dirs: + delete_path = os.path.join(ROOT, delete_dir) + + if any(name.endswith(".lock") for name in os.listdir(delete_path)): + continue + + try: + cloudlog.info("deleting %s" % delete_path) + shutil.rmtree(delete_path) + break + except OSError: + cloudlog.exception("issue deleting %s" % delete_path) + + exit_event.wait(30) + + +def main(gctx=None): + deleter_thread(threading.Event()) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 4477fe915a..dbeeaa7728 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:5d2399c0129433a50fa874b36d721d68f8429dff043ab23a487791ccde1a965c -size 1630952 +oid sha256:ac05170d2fbe9f5b65080edbb77cf408a1c90099575c1a3409d0a1ef75cd7a24 +size 1703704 diff --git a/selfdrive/loggerd/tests/Makefile b/selfdrive/loggerd/tests/Makefile new file mode 100644 index 0000000000..d05d549170 --- /dev/null +++ b/selfdrive/loggerd/tests/Makefile @@ -0,0 +1,35 @@ +CC = clang +CXX = clang++ + +PHONELIBS = ../../../phonelibs + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args \ + -Wno-deprecated-declarations + +CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) + +FFMPEG_LIBS = -lavformat \ + -lavcodec \ + -lswscale \ + -lavutil \ + -lz + +OBJS = testraw.o \ + ../RawLogger.o \ + ../../common/visionipc.o + +testraw: $(OBJS) + $(CXX) -fPIC -o '$@' $^ -L/usr/lib $(FFMPEG_LIBS) + +%.o: %.cc + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) \ + -I../ \ + -I../../ \ + -I../../../ \ + -c -o '$@' '$<' diff --git a/selfdrive/loggerd/tests/__init__.py b/selfdrive/loggerd/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/loggerd/tests/fill_eon.py b/selfdrive/loggerd/tests/fill_eon.py new file mode 100755 index 0000000000..2385e3aa5c --- /dev/null +++ b/selfdrive/loggerd/tests/fill_eon.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +"""Script to fill up EON with fake data""" + +import os + +from selfdrive.loggerd.config import ROOT +from selfdrive.loggerd.tests.loggerd_tests_common import create_random_file + + +if __name__ == "__main__": + segment_idx = 0 + while True: + seg_name = "1970-01-01--00-00-00--%d" % segment_idx + seg_path = os.path.join(ROOT, seg_name) + + print(seg_path) + + create_random_file(os.path.join(seg_path, 'fcamera.hevc'), 36) + create_random_file(os.path.join(seg_path, 'rlog.bz2'), 2) + + segment_idx += 1 + + # Fill up to 99 percent + statvfs = os.statvfs(ROOT) + available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks + if available_percent < 1.0: + break diff --git a/selfdrive/loggerd/tests/loggerd_tests_common.py b/selfdrive/loggerd/tests/loggerd_tests_common.py new file mode 100644 index 0000000000..dd48381d8e --- /dev/null +++ b/selfdrive/loggerd/tests/loggerd_tests_common.py @@ -0,0 +1,53 @@ +import os +import errno +import shutil +import random +import tempfile +import unittest + +import selfdrive.loggerd.uploader as uploader + + +def create_random_file(file_path, size_mb, lock=False): + try: + os.mkdir(os.path.dirname(file_path)) + except OSError: + pass + + lock_path = file_path + ".lock" + os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL)) + + chunks = 128 + chunk_bytes = int(size_mb * 1024 * 1024 / chunks) + data = os.urandom(chunk_bytes) + + with open(file_path, 'wb') as f: + for _ in range(chunks): + f.write(data) + + if not lock: + os.remove(lock_path) + + +class UploaderTestCase(unittest.TestCase): + f_type = "UNKNOWN" + + def setUp(self): + self.root = tempfile.mkdtemp() + uploader.ROOT = self.root # Monkey patch root dir + self.seg_num = random.randint(1, 300) + self.seg_format = "2019-04-18--12-52-54--{}" + self.seg_dir = self.seg_format.format(self.seg_num) + + def tearDown(self): + try: + shutil.rmtree(self.root) + except OSError as e: + if e.errno != errno.ENOENT: + raise + + def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False): + file_path = os.path.join(self.root, f_dir, fn) + create_random_file(file_path, size_mb) + + return file_path diff --git a/selfdrive/loggerd/tests/test_deleter.py b/selfdrive/loggerd/tests/test_deleter.py new file mode 100644 index 0000000000..d804b6f6ff --- /dev/null +++ b/selfdrive/loggerd/tests/test_deleter.py @@ -0,0 +1,104 @@ +import os +import time +import threading +from collections import namedtuple + +import selfdrive.loggerd.deleter as deleter +from common.timeout import Timeout, TimeoutException + +from loggerd_tests_common import UploaderTestCase + +Stats = namedtuple("Stats", ['f_bavail', 'f_blocks']) + +fake_stats = Stats(f_bavail=0, f_blocks=10) + + +def fake_statvfs(d): + return fake_stats + + +class TestDeleter(UploaderTestCase): + def setUp(self): + self.f_type = "fcamera.hevc" + super(TestDeleter, self).setUp() + deleter.os.statvfs = fake_statvfs + deleter.ROOT = self.root + + def tearDown(self): + super(TestDeleter, self).tearDown() + + def start_thread(self): + self.end_event = threading.Event() + self.del_thread = threading.Thread(target=deleter.deleter_thread, args=[self.end_event]) + self.del_thread.daemon = True + self.del_thread.start() + + def join_thread(self): + self.end_event.set() + self.del_thread.join() + + def test_delete(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, 1) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.5) + self.join_thread() + + self.assertFalse(os.path.exists(f_path), "File not deleted") + + def test_delete_files_in_create_order(self): + f_path_1 = self.make_file_with_data(self.seg_dir, self.f_type) + time.sleep(1) + self.seg_num += 1 + self.seg_dir = self.seg_format.format(self.seg_num) + f_path_2 = self.make_file_with_data(self.seg_dir, self.f_type) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path_1) and os.path.exists(f_path_2): + time.sleep(0.5) + + self.join_thread() + + self.assertFalse(os.path.exists(f_path_1), "Older file not deleted") + + self.assertTrue(os.path.exists(f_path_2), "Newer file deleted before older file") + + def test_no_delete_when_available_space(self): + global fake_stats + f_path = self.make_file_with_data(self.seg_dir, self.f_type) + + fake_stats = Stats(f_bavail=10, f_blocks=10) + + self.start_thread() + + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.5) + except TimeoutException: + pass + finally: + self.join_thread() + + self.assertTrue(os.path.exists(f_path), "File deleted with available space") + + def test_no_delete_with_lock_file(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True) + + self.start_thread() + + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.5) + except TimeoutException: + pass + finally: + self.join_thread() + + self.assertTrue(os.path.exists(f_path), "File deleted when locked") diff --git a/selfdrive/loggerd/tests/testraw.cc b/selfdrive/loggerd/tests/testraw.cc new file mode 100644 index 0000000000..80858e6f28 --- /dev/null +++ b/selfdrive/loggerd/tests/testraw.cc @@ -0,0 +1,57 @@ +#include +#include +#include + +#include + +#include "common/visionipc.h" +#include "common/timing.h" + +#include "RawLogger.h" + +int main() { + int err; + + VisionStream stream; + + VisionStreamBufs buf_info; + while (true) { + err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info); + if (err != 0) { + printf("visionstream fail\n"); + usleep(100000); + } + break; + } + + RawLogger vidlogger("prcamera", buf_info.width, buf_info.height, 20); + vidlogger.Open("o1"); + + for (int cnt=0; cnt<200; cnt++) { + VIPCBufExtra extra; + VIPSBuf* buf = visionstream_get(&stream, &extra); + if (buf == NULL) { + printf("visionstream get failed\n"); + break; + } + + if (cnt == 100) { + vidlogger.Rotate("o2", 2); + } + + uint8_t *y = (uint8_t*)buf->addr; + uint8_t *u = y + (buf_info.width*buf_info.height); + uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2); + + double t1 = millis_since_boot(); + vidlogger.LogFrame(cnt, y, u, v, NULL); + double t2 = millis_since_boot(); + printf("%d %.2f\n", cnt, (t2-t1)); + } + + vidlogger.Close(); + + visionstream_destroy(&stream); + + return 0; +} diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 670e924ab4..451b0b4120 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -116,7 +116,10 @@ class Uploader(object): return for logname in listdir_by_creation_date(self.root): path = os.path.join(self.root, logname) - names = os.listdir(path) + try: + names = os.listdir(path) + except OSError: + continue if any(name.endswith(".lock") for name in names): continue @@ -194,26 +197,6 @@ class Uploader(object): return self.last_resp - def killable_upload(self, key, fn): - self.last_resp = None - self.last_exc = None - - self.upload_thread = threading.Thread(target=lambda: self.do_upload(key, fn)) - self.upload_thread.start() - self.upload_thread.join() - self.upload_thread = None - - return self.last_resp - - def abort_upload(self): - thread = self.upload_thread - if thread is None: - return - if not thread.is_alive(): - return - raise_on_thread(thread, SystemExit) - thread.join() - def compress(self, key, fn): # write out the bz2 compress if fn.endswith("log"): @@ -246,7 +229,6 @@ class Uploader(object): success = True else: cloudlog.info("uploading %r", fn) - # stat = self.killable_upload(key, fn) stat = self.normal_upload(key, fn) if stat is not None and stat.status_code in (200, 201): cloudlog.event("upload_success", key=key, fn=fn, sz=sz) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 9c6dd646a7..6dcd982f76 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -42,7 +42,7 @@ def unblock_stdout(): if __name__ == "__main__": neos_update_required = os.path.isfile("/init.qcom.rc") \ - and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 8) + and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 9) if neos_update_required: # update continue.sh before updating NEOS if os.path.isfile(os.path.join(BASEDIR, "scripts", "continue.sh")): @@ -88,6 +88,7 @@ from selfdrive.loggerd.config import ROOT managed_processes = { "thermald": "selfdrive.thermald", "uploader": "selfdrive.loggerd.uploader", + "deleter": "selfdrive.loggerd.deleter", "controlsd": "selfdrive.controls.controlsd", "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", @@ -127,6 +128,7 @@ persistent_processes = [ 'logcatd', 'tombstoned', 'uploader', + 'deleter', 'ui', 'gpsd', 'updated', diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py index b03e66d7af..888c9e1df9 100755 --- a/selfdrive/mapd/default_speeds_generator.py +++ b/selfdrive/mapd/default_speeds_generator.py @@ -1,6 +1,5 @@ #!/usr/bin/env python import json -import six DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" @@ -146,6 +145,31 @@ def main(filename = DEFAULT_OUTPUT_FILENAME): DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") DE.add_rule({"bicycle_road": "yes"}, "30") + + + """ + -------------------------------------------------- + EE - Estonia + -------------------------------------------------- + """ + EE = Country("EE") + countries.append(EE) + + """ Default rules """ + EE.add_rule({"highway": "motorway"}, "90") + EE.add_rule({"highway": "trunk"}, "90") + EE.add_rule({"highway": "primary"}, "90") + EE.add_rule({"highway": "secondary"}, "50") + EE.add_rule({"highway": "tertiary"}, "50") + EE.add_rule({"highway": "unclassified"}, "90") + EE.add_rule({"highway": "residential"}, "40") + EE.add_rule({"highway": "service"}, "40") + EE.add_rule({"highway": "motorway_link"}, "90") + EE.add_rule({"highway": "trunk_link"}, "70") + EE.add_rule({"highway": "primary_link"}, "70") + EE.add_rule({"highway": "secondary_link"}, "50") + EE.add_rule({"highway": "tertiary_link"}, "50") + EE.add_rule({"highway": "living_street"}, "20") """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ @@ -206,7 +230,7 @@ class Country(Region): def jsonify(self): ret_dict = {} ret_dict[self.name] = {} - for r_name, region in six.iteritems(self.regions): + for r_name, region in self.regions.items(): ret_dict[self.name].update(region.jsonify()) ret_dict[self.name]['Default'] = self.rules return ret_dict diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index 50dde6ecb2..58a33ee41f 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -1,4 +1,3 @@ -import six import math import json import numpy as np @@ -92,7 +91,7 @@ def geocode_maxspeed(tags, location_info): rule_valid = all( tag_name in tags and tags[tag_name] == value - for tag_name, value in six.iteritems(rule['tags']) + for tag_name, value in rule['tags'].items() ) if rule_valid: max_speed = rule['speed'] @@ -103,7 +102,7 @@ def geocode_maxspeed(tags, location_info): rule_valid = all( tag_name in tags and tags[tag_name] == value - for tag_name, value in six.iteritems(rule['tags']) + for tag_name, value in rule['tags'].items() ) if rule_valid: max_speed = rule['speed'] diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index 433a1a4df6..fbefe764a5 100755 --- a/selfdrive/sensord/gpsd +++ b/selfdrive/sensord/gpsd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:56bf45ba0c5289350d4d504cf4c85bcb67db73ec5874d313f3b3195101298ef5 +oid sha256:125944cbfd9300a663ec5c01c48a67b3bdb0d64d253423656c47f3dcc7886e68 size 1171544 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 5085e979b1..5cb5cd7c0c 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:53e0d9ac75e8e77fb890591db0e09441b150675ee080d5b97ff2ac5555554fd4 +oid sha256:43447776956dfc2b120ee525b489dc8a9090bc02975130ee6bcaa8557356de52 size 1159016 diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index ac641e2948..0b86095c56 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -318,7 +318,7 @@ class Plant(object): msg_data = fix(msg_data, 0xe4) can_msgs.append([0xe4, 0, msg_data, 2]) - Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes()) + Plant.logcan.send(can_list_to_can_capnp(can_msgs)) # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index bacabec299..a2c9b20a8c 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -279,10 +279,12 @@ def thermald_thread(): params.car_start() started_ts = sec_since_boot() started_seen = True + os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor') else: started_ts = None if off_ts is None: off_ts = sec_since_boot() + os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor') # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for # more than a minute but we were running diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 9b90013d10..3d7061c6a6 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -105,7 +105,7 @@ def main(gctx): initial_tombstones = set(get_tombstones()) client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', - install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) + install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}, string_max_length=10000) client.user_context({'id': os.environ.get('DONGLE_ID')}) while True: diff --git a/selfdrive/ui/slplay.c b/selfdrive/ui/slplay.c index 6e5d8b9538..2085057664 100644 --- a/selfdrive/ui/slplay.c +++ b/selfdrive/ui/slplay.c @@ -38,25 +38,25 @@ uri_player* slplay_create_player_for_uri(const char* uri, char **error) { result = (*engineInterface)->CreateAudioPlayer(engineInterface, &player.player, &audioSrc, &audioSnk, 0, NULL, NULL); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to create audio player"); + *error = "Failed to create audio player"; return NULL; } result = (*(player.player))->Realize(player.player, SL_BOOLEAN_FALSE); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to realize audio player"); + *error = "Failed to realize audio player"; return NULL; } result = (*(player.player))->GetInterface(player.player, SL_IID_PLAY, &(player.playInterface)); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to get player interface"); + *error = "Failed to get player interface"; return NULL; } result = (*(player.playInterface))->SetPlayState(player.playInterface, SL_PLAYSTATE_PAUSED); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to initialize playstate to SL_PLAYSTATE_PAUSED"); + *error = "Failed to initialize playstate to SL_PLAYSTATE_PAUSED"; return NULL; } @@ -74,29 +74,29 @@ void slplay_setup(char **error) { SLEngineOption engineOptions[] = {{SL_ENGINEOPTION_THREADSAFE, SL_BOOLEAN_TRUE}}; result = slCreateEngine(&engine, 1, engineOptions, 0, NULL, NULL); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to create OpenSL engine"); + *error = "Failed to create OpenSL engine"; } result = (*engine)->Realize(engine, SL_BOOLEAN_FALSE); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to realize OpenSL engine"); + *error = "Failed to realize OpenSL engine"; } result = (*engine)->GetInterface(engine, SL_IID_ENGINE, &engineInterface); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to realize OpenSL engine"); + *error = "Failed to realize OpenSL engine"; } const SLInterfaceID ids[1] = {SL_IID_VOLUME}; const SLboolean req[1] = {SL_BOOLEAN_FALSE}; result = (*engineInterface)->CreateOutputMix(engineInterface, &outputMix, 1, ids, req); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to create output mix"); + *error = "Failed to create output mix"; } result = (*outputMix)->Realize(outputMix, SL_BOOLEAN_FALSE); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to realize output mix"); + *error = "Failed to realize output mix"; } } @@ -120,7 +120,7 @@ void slplay_stop (uri_player* player, char **error) { result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to set SL_PLAYSTATE_STOPPED"); + *error = "Failed to set SL_PLAYSTATE_STOPPED"; return; } } @@ -164,7 +164,7 @@ void slplay_play (const char *uri, bool loop, char **error) { result = (*playInterface)->SetCallbackEventsMask(playInterface, SL_PLAYEVENT_HEADATEND); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to set callback event mask"); + *error = "Failed to set callback event mask"; return; } } @@ -172,13 +172,13 @@ void slplay_play (const char *uri, bool loop, char **error) { // Reset the audio player result = (*playInterface)->ClearMarkerPosition(playInterface); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to clear marker position"); + *error = "Failed to clear marker position"; return; } result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED); result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_STOPPED); result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PLAYING); if (result != SL_RESULT_SUCCESS) { - *error = strdup("Failed to set SL_PLAYSTATE_PLAYING"); + *error = "Failed to set SL_PLAYSTATE_PLAYING"; } } diff --git a/selfdrive/visiond/camera_qcom.c b/selfdrive/visiond/camera_qcom.c index 08a1dff652..45b782ecf2 100644 --- a/selfdrive/visiond/camera_qcom.c +++ b/selfdrive/visiond/camera_qcom.c @@ -1925,6 +1925,8 @@ static void camera_close(CameraState *s) { LOG("isp release stream: %d", err); } } + + free(s->eeprom); } diff --git a/selfdrive/visiond/model.cc b/selfdrive/visiond/model.cc index ef1a7df9f0..2fbb4f3c92 100644 --- a/selfdrive/visiond/model.cc +++ b/selfdrive/visiond/model.cc @@ -90,6 +90,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, } void model_free(ModelState* s) { + free(s->output); model_input_free(&s->in); delete s->m; }