mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-20 05:22:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

update to 0.6

Browse Source
This commit is contained in:
Rick Lan
2019-07-10 09:51:52 +10:00
parent 979d1e9fb4 99a96a99bc
commit 74cbb6fde3
1372 changed files with 92887 additions and 177092 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -1,3 +1,4 @@
venv/
.DS_Store
.tags
.ipynb_checkpoints
.travis.yml
+1 -17
View File
@@ -3,21 +3,5 @@ sudo: required
services:
- docker
install:
- docker build -t tmppilot -f Dockerfile.openpilot .
script:
- docker run
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
- docker run
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'
- ./run_docker_tests.sh
Dockerfile.openpilot
+24 -11
View File
@@ -4,32 +4,45 @@ ENV PYTHONUNBUFFERED 1
RUN apt-get update && apt-get install -y \
autoconf \
build-essential \
clang \
wget \
bzip2 \
clang \
git \
libglib2.0-0 \
libtool \
python-pip \
libzmq5-dev \
libarchive-dev \
libavcodec-dev \
libavdevice-dev \
libavfilter-dev \
libavresample-dev \
libavutil-dev \
libffi-dev \
libusb-1.0-0 \
libglib2.0-0 \
libssl-dev \
libswscale-dev \
libtool \
libusb-1.0-0 \
libzmq5-dev \
ocl-icd-libopencl1 \
ocl-icd-opencl-dev \
opencl-headers
opencl-headers \
pkg-config \
python-pip \
wget
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
RUN pip install pipenv==2018.11.26
COPY requirements_openpilot.txt /tmp/
RUN pip install -r /tmp/requirements_openpilot.txt
COPY Pipfile /tmp/
COPY Pipfile.lock /tmp/
RUN cd /tmp && pipenv install --deploy --system
ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH
RUN git clone --branch v0.6 https://github.com/commaai/openpilot-tools.git /tmp/openpilot/tools
RUN pip install -r /tmp/openpilot/tools/requirements.txt
RUN pip install fastcluster==1.1.20 scipy==0.19.1
COPY ./.pylintrc /tmp/openpilot/.pylintrc
COPY ./common /tmp/openpilot/common
COPY ./cereal /tmp/openpilot/cereal
Pipfile
+140
View File
@@ -0,0 +1,140 @@
[[source]]
name = "pypi"
url = "https://pypi.org/simple"
verify_ssl = true
[dev-packages]
ipython = "<6.0"
aenum = "*"
azure-batch = "==4.1.3"
azure-common = "==1.1.16"
azure-nspkg = "==3.0.1"
azure-storage-blob = "==1.3.1"
azure-storage-common = "==1.3.0"
azure-storage-nspkg = "==3.0.0"
bincopy = "*"
bleach = "==1.5.0"
boto = "*"
"boto3" = "*"
celery = "*"
control = "*"
datadog = "*"
decorator = "*"
dlib = "*"
dominate = "*"
elasticsearch = "*"
entium = "==0.1.4"
fasteners = "*"
future = "*"
futures = "*"
gevent = "*"
pycocotools = {git = "https://github.com/cocodataset/cocoapi.git",subdirectory = "PythonAPI"}
gunicorn = "*"
"h5py" = "*"
hexdump = "*"
"html5lib" = "==0.9999999"
imageio = "*"
intervaltree = "*"
ipykernel = "<5.0"
joblib = "*"
json-logging-py = "*"
jupyter = "*"
libarchive = "*"
lru-dict = "*"
lxml = "*"
matplotlib = "==2.2.3"
"mpld3" = "*"
msgpack-python = "*"
nbstripout = "*"
nose-parameterized = "*"
numpy = "==1.14.5"
osmium = "==2.15.0"
pbr = "==5.1.3"
percache = "*"
pprofile = "*"
psutil = "*"
pycurl = "*"
git-pylint-commit-hook = "==2.5.1"
pymongo = "*"
"pynmea2" = "*"
pypolyline = "==0.1.17"
pysendfile = "*"
python-logstash = "*"
pyvcd = "*"
redis = "*"
redlock = "*"
"s2sphere" = "*"
scikit-image = "*"
"subprocess32" = "*"
supervisor = "*"
tenacity = "*"
tensorflow-gpu = "==1.13.0rc0"
"transforms3d" = "*"
utm = "*"
"v4l2" = "*"
visdom = "*"
PyJWT = "==1.4.1"
PyMySQL = "==0.9.2"
Theano = "*"
Werkzeug = "*"
"backports.lzma" = "*"
Flask-Cors = "*"
Flask-SocketIO = "*"
"GeoAlchemy2" = "*"
Keras = ">=2.1.6"
keras-maskrcnn = "*"
keras-retinanet = "*"
Pygments = "*"
PyNaCl = "*"
"PySDL2" = "*"
reverse_geocoder = "*"
Shapely = "*"
SQLAlchemy = "==1.2.7"
uWSGI = "*"
scipy = "*"
fastcluster = "==1.1.25"
backports-abc = "*"
pygame = "*"
simplejson = "*"
[packages]
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
atomicwrites = "*"
cffi = "*"
crcmod = "*"
hexdump = "*"
libusb1 = "*"
numpy = "*"
psutil = "*"
pycapnp = "*"
cryptography = "*"
pyserial = "*"
python-dateutil = "*"
pyzmq = "*"
raven = "*"
requests = "*"
setproctitle = "*"
six = "*"
smbus2 = "*"
sympy = "*"
tqdm = "*"
Cython = "*"
PyYAML = "*"
websocket_client = "*"
Logentries = {git = "https://github.com/commaai/le_python.git",ref = "5eef8f5be5929d33973e1b10e686fa0cdcd6792f"}
urllib3 = "*"
chardet = "*"
idna = "*"
gunicorn = "*"
utm = "*"
json-rpc = "*"
Flask = "*"
PyJWT = "*"
"Jinja2" = "*"
nose = "*"
pyflakes = "*"
pylint = "*"
pycryptodome = "*"
[requires]
python_version = "2.7"
Pipfile.lock
Generated
+2810
View File
File diff suppressed because it is too large Load Diff
README.md
+6 -4
View File
@@ -1,4 +1,4 @@
[![](https://i.imgur.com/xY2gdHv.png)](#)
[![](https://i.imgur.com/UetIFyH.jpg)](#)
Welcome to openpilot
======
@@ -77,8 +77,8 @@ Supported Cars
| Honda | Civic Hatchback 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Honda | CR-V 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | CR-V Hybrid 2019 | All | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | Odyssey 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
| Honda | Passport 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
@@ -91,13 +91,15 @@ Supported Cars
| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Camry 2018 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | C-HR 2017-18 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Corolla 2017-19 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Highlander 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Highlander Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
RELEASES.md
+13
View File
@@ -1,3 +1,16 @@
Version 0.6 (2019-07-01)
========================
* New model, with double the pixels and ten times the temporal context!
* Car should not take exits when in the right lane
* openpilot uses only ~65% of the CPU (down from 75%)
* Routes visible in connect/explorer after only 0.2% is uploaded (qlogs)
* loggerd and sensord are open source, every line of openpilot is now open
* Panda safety code is MISRA compliant and ships with a signed version on release2
* New NEOS is 500MB smaller and has a reproducible usr/pipenv
* Lexus ES Hybrid support thanks to wocsor!
* Improve tuning for supported Toyota with TSS2
* Various other stability improvements
Version 0.5.13 (2019-05-31)
==========================
* Reduce panda power consumption by 70%, down to 80mW, when car is off (not for GM)
cereal/.gitignore
+3
View File
@@ -1,3 +1,6 @@
gen
node_modules
package-lock.json
*.pyc
__pycache__
cereal/car.capnp
+8 -6
View File
@@ -22,7 +22,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
enum EventName @0xbaa8c5d505f727de {
# TODO: copy from error list
commIssue @0;
canError @0;
steerUnavailable @1;
brakeUnavailable @2;
gasUnavailable @3;
@@ -37,7 +37,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
buttonEnable @12;
pedalPressed @13;
cruiseDisabled @14;
radarCommIssue @15;
radarCanError @15;
dataNeeded @16;
speedTooLow @17;
outOfSpace @18;
@@ -49,7 +49,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
pcmDisable @24;
noTarget @25;
radarFault @26;
modelCommIssue @27;
modelCommIssueDEPRECATED @27;
brakeHold @28;
parkBrake @29;
manualRestart @30;
@@ -75,6 +75,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
vehicleModelInvalid @50;
controlsFailed @51;
sensorDataInvalid @52;
commIssue @53;
}
}
@@ -123,6 +124,7 @@ struct CarState {
# lock info
doorOpen @24 :Bool;
seatbeltUnlatched @25 :Bool;
canValid @26 :Bool;
# which packets this state came from
canMonoTimes @12: List(UInt64);
@@ -184,7 +186,7 @@ struct RadarData @0x888ad6581cf0aacb {
canMonoTimes @2 :List(UInt64);
enum Error {
commIssue @0;
canError @0;
fault @1;
wrongConfig @2;
}
@@ -293,7 +295,7 @@ struct CarParams {
minEnableSpeed @7 :Float32;
minSteerSpeed @8 :Float32;
safetyModel @9 :Int16;
safetyModel @9 :SafetyModel;
safetyParam @10 :Int16;
steerMaxBP @11 :List(Float32);
@@ -362,7 +364,7 @@ struct CarParams {
}
enum SafetyModels {
enum SafetyModel {
# does NOT match board setting
noOutput @0;
honda @1;
cereal/generate_javascript.sh
Executable
+26
View File
@@ -0,0 +1,26 @@
#!/bin/bash
rm -r gen/ts
rm -r gen/js
mkdir gen/ts
mkdir gen/js
echo "Installing needed npm modules"
npm i capnpc-ts capnp-ts
capnpc -o node_modules/.bin/capnpc-ts:gen/ts log.capnp car.capnp
capnpc -o node_modules/.bin/capnpc-ts:gen/ts car.capnp
cat log.capnp | egrep '\([a-zA-Z]*\.[^\s]+\.[^s]+\)' | sed 's/^.*([a-zA-Z]*\.\([a-zA-Z.]*\)).*/\1/' | while read line
do
TOKEN=`echo $line | sed 's/\./_/g'`
ROOT=`echo $line | sed 's/\..*$//g'`
cat gen/ts/log.capnp.ts | grep '^import.*'${TOKEN}
if [[ "$?" == "1" ]]
then
sed -i 's/^\(import {.*\)'${ROOT}'\(,*\) \(.*\)$/\1'${ROOT}', '${TOKEN}'\2 \3/' ./gen/ts/log.capnp.ts
fi
done
tsc ./gen/ts/* --lib es2015 --outDir ./gen/js
cereal/install_capnp.sh
Executable
+39
View File
@@ -0,0 +1,39 @@
set -e
echo "Installing capnp"
cd /tmp
VERSION=0.6.1
wget https://capnproto.org/capnproto-c++-${VERSION}.tar.gz
tar xvf capnproto-c++-${VERSION}.tar.gz
cd capnproto-c++-${VERSION}
CXXFLAGS="-fPIC" ./configure
make -j4
# manually build binaries statically
g++ -std=gnu++11 -I./src -I./src -DKJ_HEADER_WARNINGS -DCAPNP_HEADER_WARNINGS -DCAPNP_INCLUDE_DIR=\"/usr/local/include\" -pthread -O2 -DNDEBUG -pthread -pthread -o .libs/capnp src/capnp/compiler/module-loader.o src/capnp/compiler/capnp.o ./.libs/libcapnpc.a ./.libs/libcapnp.a ./.libs/libkj.a -lpthread -pthread
g++ -std=gnu++11 -I./src -I./src -DKJ_HEADER_WARNINGS -DCAPNP_HEADER_WARNINGS -DCAPNP_INCLUDE_DIR=\"/usr/local/include\" -pthread -O2 -DNDEBUG -pthread -pthread -o .libs/capnpc-c++ src/capnp/compiler/capnpc-c++.o ./.libs/libcapnp.a ./.libs/libkj.a -lpthread -pthread
g++ -std=gnu++11 -I./src -I./src -DKJ_HEADER_WARNINGS -DCAPNP_HEADER_WARNINGS -DCAPNP_INCLUDE_DIR=\"/usr/local/include\" -pthread -O2 -DNDEBUG -pthread -pthread -o .libs/capnpc-capnp src/capnp/compiler/capnpc-capnp.o ./.libs/libcapnp.a ./.libs/libkj.a -lpthread -pthread
cp .libs/capnp /usr/local/bin/
ln -s /usr/local/bin/capnp /usr/local/bin/capnpc
cp .libs/capnpc-c++ /usr/local/bin/
cp .libs/capnpc-capnp /usr/local/bin/
cp .libs/*.a /usr/local/lib
cd /tmp
echo "Installing c-capnp"
git clone https://github.com/commaai/c-capnproto.git
cd c-capnproto
git submodule update --init --recursive
autoreconf -f -i -s
CXXFLAGS="-fPIC" ./configure
make -j4
# manually build binaries statically
gcc -fPIC -o .libs/capnpc-c compiler/capnpc-c.o compiler/schema.capnp.o compiler/str.o ./.libs/libcapnp_c.a
cp .libs/capnpc-c /usr/local/bin/
cp .libs/*.a /usr/local/lib
cereal/log.capnp
+20 -3
View File
@@ -150,6 +150,12 @@ struct FrameData {
}
}
struct Thumbnail {
frameId @0 :UInt32;
timestampEof @1 :UInt64;
thumbnail @2 :Data;
}
struct GPSNMEAData {
timestamp @0 :Int64;
localWallTime @1 :UInt64;
@@ -508,12 +514,15 @@ struct ModelData {
prob @1 :Float32;
std @2 :Float32;
stds @3 :List(Float32);
poly @4 :List(Float32);
}
struct LeadData {
dist @0 :Float32;
prob @1 :Float32;
std @2 :Float32;
relVel @3 :Float32;
relVelStd @4 :Float32;
}
struct ModelSettings {
@@ -576,6 +585,8 @@ struct LogRotate {
struct Plan {
mdMonoTime @9 :UInt64;
radarStateMonoTime @10 :UInt64;
commIssue @31 :Bool;
eventsDEPRECATED @13 :List(Car.CarEvent);
# lateral, 3rd order polynomial
@@ -614,7 +625,7 @@ struct Plan {
decelForTurn @22 :Bool;
mapValid @25 :Bool;
radarValid @28 :Bool;
radarCommIssue @30 :Bool;
radarCanError @30 :Bool;
processingDelay @29 :Float32;
@@ -645,11 +656,12 @@ struct PathPlan {
angleSteers @8 :Float32; # deg
rateSteers @13 :Float32; # deg/s
valid @9 :Bool;
mpcSolutionValid @9 :Bool;
paramsValid @10 :Bool;
modelValid @12 :Bool;
modelValidDEPRECATED @12 :Bool;
angleOffset @11 :Float32;
sensorValid @14 :Bool;
commIssue @15 :Bool;
}
struct LiveLocationData {
@@ -1646,6 +1658,7 @@ struct LiveParametersData {
stiffnessFactor @4 :Float32;
steerRatio @5 :Float32;
sensorValid @6 :Bool;
yawRate @7 :Float32;
}
struct LiveMapData {
@@ -1685,6 +1698,7 @@ struct KalmanOdometry {
struct Event {
# in nanoseconds?
logMonoTime @0 :UInt64;
valid @67 :Bool = true;
union {
initData @1 :InitData;
@@ -1752,5 +1766,8 @@ struct Event {
cameraOdometry @63 :CameraOdometry;
pathPlan @64 :PathPlan;
kalmanOdometry @65 :KalmanOdometry;
thumbnail @66: Thumbnail;
carEvents @68: List(Car.CarEvent);
carParams @69: Car.CarParams;
}
}
cereal/maptile.capnp
+53
View File
@@ -0,0 +1,53 @@
using Cxx = import "./include/c++.capnp";
$Cxx.namespace("cereal");
using Java = import "./include/java.capnp";
$Java.package("ai.comma.openpilot.cereal");
$Java.outerClassname("Map");
@0xa086df597ef5d7a0;
# Geometry
struct Point {
x @0: Float64;
y @1: Float64;
z @2: Float64;
}
struct PolyLine {
points @0: List(Point);
}
# Map features
struct Lane {
id @0 :Text;
leftBoundary @1 :LaneBoundary;
rightBoundary @2 :LaneBoundary;
leftAdjacentId @3 :Text;
rightAdjacentId @4 :Text;
inboundIds @5 :List(Text);
outboundIds @6 :List(Text);
struct LaneBoundary {
polyLine @0 :PolyLine;
startHeading @1 :Float32; # WRT north
}
}
# Map tiles
struct TileSummary {
version @0 :Text;
updatedAt @1 :UInt64; # Millis since epoch
level @2 :UInt8;
x @3 :UInt16;
y @4 :UInt16;
}
struct MapTile {
summary @0 :TileSummary;
lanes @1 :List(Lane);
}
common/fingerprints.py
+1 -1
View File
@@ -4,7 +4,7 @@ from common.basedir import BASEDIR
def get_fingerprint_list():
# read all the folders in selfdrive/car and return a dict where:
# - keys are all the car models for which we have a fingerprint
# - values are lists dicts of messages that constitute the unique
# - values are lists dicts of messages that constitute the unique
# CAN fingerprint of each car model and all its variants
fingerprints = {}
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
common/kalman/Makefile
+1 -1
View File
@@ -1,7 +1,7 @@
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
python2 simple_kalman_setup.py build_ext --inplace
rm -rf build
rm simple_kalman_impl.c
common/params.py
+1
View File
@@ -65,6 +65,7 @@ keys = {
"IsGeofenceEnabled": [TxType.PERSISTENT],
"IsMetric": [TxType.PERSISTENT],
"IsUpdateAvailable": [TxType.PERSISTENT],
"IsUploadRawEnabled": [TxType.PERSISTENT],
"IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT],
"LimitSetSpeed": [TxType.PERSISTENT],
"LiveParameters": [TxType.PERSISTENT],
common/realtime.py
+7
View File
@@ -15,6 +15,13 @@ assert monotonic_time
assert sec_since_boot
# time step for each process
DT_CTRL = 0.01 # controlsd
DT_PLAN = 0.05 # mpc
DT_MDL = 0.05 # model
DT_DMON = 0.1 # driver monitoring
ffi = FFI()
ffi.cdef("long syscall(long number, ...);")
libc = ffi.dlopen(None)
common/transformations/camera.py
+5 -2
View File
@@ -1,6 +1,5 @@
import numpy as np
import common.transformations.orientation as orient
import cv2
import math
FULL_FRAME_SIZE = (1164, 874)
@@ -126,6 +125,8 @@ def img_from_device(pt_device):
#TODO please use generic img transform below
def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
import cv2
size = img.shape[:2]
rot = orient.rot_from_euler(eulers)
quadrangle = np.array([[0, 0],
@@ -162,6 +163,8 @@ def transform_img(base_img,
output_size=None,
pretransform=None,
top_hacks=True):
import cv2
size = base_img.shape[:2]
if not output_size:
output_size = size[::-1]
@@ -204,10 +207,10 @@ def transform_img(base_img,
def yuv_crop(frame, output_size, center=None):
# output_size in camera coordinates so u,v
# center in array coordinates so row, column
import cv2
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)
installer/updater/Makefile
+88
View File
@@ -0,0 +1,88 @@
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
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
CURL_FLAGS = -I$(PHONELIBS)/curl/include
CURL_LIBS = $(PHONELIBS)/curl/lib/libcurl.a \
$(PHONELIBS)/zlib/lib/libz.a
BORINGSSL_FLAGS = -I$(PHONELIBS)/boringssl/include
BORINGSSL_LIBS = $(PHONELIBS)/boringssl/lib/libssl_static.a \
$(PHONELIBS)/boringssl/lib/libcrypto_static.a \
NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
JSON11_FLAGS = -I$(PHONELIBS)/json11
OPENGL_LIBS = -lGLESv3
FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
.PHONY: all
all: updater
OBJS = courbd.ttf.o \
../../selfdrive/common/touch.o \
../../selfdrive/common/framebuffer.o \
$(PHONELIBS)/json11/json11.o \
$(PHONELIBS)/nanovg/nanovg.o
DEPS := $(OBJS:.o=.d)
updater: updater.o $(OBJS)
@echo "[ LINK ] $@"
$(CXX) $(CPPFLAGS) -fPIC -o 'updater' $^ \
$(FRAMEBUFFER_LIBS) \
$(CURL_LIBS) \
$(BORINGSSL_LIBS) \
-L/system/vendor/lib64 \
$(OPENGL_LIBS) \
-lcutils -lm -llog
strip updater
courbd.ttf.o: ../../selfdrive/assets/courbd.ttf
@echo "[ bin2o ] $@"
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
%.o: %.c
mkdir -p $(@D)
@echo "[ CC ] $@"
$(CC) $(CPPFLAGS) $(CFLAGS) \
-I../.. \
-I$(PHONELIBS)/android_frameworks_native/include \
-I$(PHONELIBS)/android_system_core/include \
-I$(PHONELIBS)/android_hardware_libhardware/include \
$(NANOVG_FLAGS) \
-c -o '$@' '$<'
%.o: %.cc
mkdir -p $(@D)
@echo "[ CXX ] $@"
$(CXX) $(CPPFLAGS) $(CXXFLAGS) \
-I../../selfdrive \
-I../../ \
-I$(PHONELIBS)/android_frameworks_native/include \
-I$(PHONELIBS)/android_system_core/include \
-I$(PHONELIBS)/android_hardware_libhardware/include \
$(NANOVG_FLAGS) \
$(JSON11_FLAGS) \
$(CURL_FLAGS) \
$(BORINGSSL_FLAGS) \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f $(OBJS) $(DEPS)
-include $(DEPS)
installer/updater/update.json
+7
View File
@@ -0,0 +1,7 @@
{
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-c992abb59cbaf6588f51055db52db619061107851773fc8480acb8bb5d77a28f.zip",
"ota_hash": "c992abb59cbaf6588f51055db52db619061107851773fc8480acb8bb5d77a28f",
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-af099a84cfd7b91266090779238ac358278948dcde2dcfa0fbca6e8397366f0a.img",
"recovery_len": 15136044,
"recovery_hash": "af099a84cfd7b91266090779238ac358278948dcde2dcfa0fbca6e8397366f0a"
}
installer/updater/updater
BIN
View File
Binary file not shown.
installer/updater/updater.cc
+675
View File
@@ -0,0 +1,675 @@
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cassert>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/statvfs.h>
#include <string>
#include <sstream>
#include <fstream>
#include <mutex>
#include <thread>
#include <curl/curl.h>
#include <openssl/sha.h>
#include <GLES3/gl3.h>
#include <EGL/egl.h>
#include <EGL/eglext.h>
#include "nanovg.h"
#define NANOVG_GLES3_IMPLEMENTATION
#include "nanovg_gl.h"
#include "nanovg_gl_utils.h"
#include "json11.hpp"
#include "common/framebuffer.h"
#include "common/touch.h"
#include "common/utilpp.h"
#define USER_AGENT "NEOSUpdater-0.2"
#define MANIFEST_URL_EON_STAGING "https://github.com/commaai/eon-neos/raw/master/update.staging.json"
#define MANIFEST_URL_EON_LOCAL "http://192.168.5.1:8000/neosupdate/update.local.json"
#define MANIFEST_URL_EON "https://github.com/commaai/eon-neos/raw/master/update.json"
const char *manifest_url = MANIFEST_URL_EON;
#define RECOVERY_DEV "/dev/block/bootdevice/by-name/recovery"
#define RECOVERY_COMMAND "/cache/recovery/command"
#define UPDATE_DIR "/data/neoupdate"
extern const uint8_t bin_courbd[] asm("_binary_courbd_ttf_start");
extern const uint8_t bin_courbd_end[] asm("_binary_courbd_ttf_end");
namespace {
std::string sha256_file(std::string fn, size_t limit=0) {
SHA256_CTX ctx;
SHA256_Init(&ctx);
FILE *file = fopen(fn.c_str(), "rb");
if (!file) return "";
const size_t buf_size = 8192;
std::unique_ptr<char[]> buffer( new char[ buf_size ] );
bool read_limit = (limit != 0);
while (true) {
size_t read_size = buf_size;
if (read_limit) read_size = std::min(read_size, limit);
size_t bytes_read = fread(buffer.get(), 1, read_size, file);
if (!bytes_read) break;
SHA256_Update(&ctx, buffer.get(), bytes_read);
if (read_limit) {
limit -= bytes_read;
if (limit == 0) break;
}
}
uint8_t hash[SHA256_DIGEST_LENGTH];
SHA256_Final(hash, &ctx);
fclose(file);
return util::tohex(hash, sizeof(hash));
}
size_t download_string_write(void *ptr, size_t size, size_t nmeb, void *up) {
size_t sz = size * nmeb;
((std::string*)up)->append((char*)ptr, sz);
return sz;
}
std::string download_string(CURL *curl, std::string url) {
std::string os;
curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1);
curl_easy_setopt(curl, CURLOPT_NOSIGNAL, 1);
curl_easy_setopt(curl, CURLOPT_USERAGENT, USER_AGENT);
curl_easy_setopt(curl, CURLOPT_FAILONERROR, 1);
curl_easy_setopt(curl, CURLOPT_RESUME_FROM, 0);
curl_easy_setopt(curl, CURLOPT_NOPROGRESS, 1);
curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, download_string_write);
curl_easy_setopt(curl, CURLOPT_WRITEDATA, &os);
CURLcode res = curl_easy_perform(curl);
if (res != CURLE_OK) {
return "";
}
return os;
}
size_t download_file_write(void *ptr, size_t size, size_t nmeb, void *up) {
return fwrite(ptr, size, nmeb, (FILE*)up);
}
bool check_battery() {
std::string bat_cap_s = util::read_file("/sys/class/power_supply/battery/capacity");
int bat_cap = atoi(bat_cap_s.c_str());
std::string current_now_s = util::read_file("/sys/class/power_supply/battery/current_now");
int current_now = atoi(current_now_s.c_str());
return bat_cap > 35 || (current_now < 0 && bat_cap > 10);
}
bool check_space() {
struct statvfs stat;
if (statvfs("/data/", &stat) != 0) {
return false;
}
size_t space = stat.f_bsize * stat.f_bavail;
return space > 2000000000ULL; // 2GB
}
static void start_settings_activity(const char* name) {
char launch_cmd[1024];
snprintf(launch_cmd, sizeof(launch_cmd),
"am start -W --ez :settings:show_fragment_as_subsetting true -n 'com.android.settings/.%s'", name);
system(launch_cmd);
}
struct Updater {
bool do_exit = false;
TouchState touch;
int fb_w, fb_h;
EGLDisplay display;
EGLSurface surface;
FramebufferState *fb = NULL;
NVGcontext *vg = NULL;
int font;
std::thread update_thread_handle;
std::mutex lock;
// i hate state machines give me coroutines already
enum UpdateState {
CONFIRMATION,
RUNNING,
ERROR,
};
UpdateState state;
std::string progress_text;
float progress_frac;
std::string error_text;
// button
int b_x, b_w, b_y, b_h;
int balt_x;
CURL *curl = NULL;
Updater() {
touch_init(&touch);
fb = framebuffer_init("updater", 0x00001000, false,
&display, &surface, &fb_w, &fb_h);
assert(fb);
vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
assert(vg);
font = nvgCreateFontMem(vg, "courbd", (unsigned char*)bin_courbd, (bin_courbd_end - bin_courbd), 0);
assert(font >= 0);
b_w = 600;
balt_x = 200;
b_x = fb_w-b_w-200;
b_y = 700;
b_h = 250;
state = CONFIRMATION;
}
int download_file_xferinfo(curl_off_t dltotal, curl_off_t dlno,
curl_off_t ultotal, curl_off_t ulnow) {
{
std::lock_guard<std::mutex> guard(lock);
if (dltotal != 0) {
progress_frac = (float) dlno / dltotal;
}
}
// printf("info: %ld %ld %f\n", dltotal, dlno, progress_frac);
return 0;
}
bool download_file(std::string url, std::string out_fn) {
FILE *of = fopen(out_fn.c_str(), "ab");
assert(of);
CURLcode res;
long last_resume_from = 0;
fseek(of, 0, SEEK_END);
int tries = 4;
bool ret = false;
while (true) {
long resume_from = ftell(of);
curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1);
curl_easy_setopt(curl, CURLOPT_NOSIGNAL, 1);
curl_easy_setopt(curl, CURLOPT_USERAGENT, USER_AGENT);
curl_easy_setopt(curl, CURLOPT_FAILONERROR, 1);
curl_easy_setopt(curl, CURLOPT_RESUME_FROM, resume_from);
curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, download_file_write);
curl_easy_setopt(curl, CURLOPT_WRITEDATA, of);
curl_easy_setopt(curl, CURLOPT_NOPROGRESS, 0);
curl_easy_setopt(curl, CURLOPT_XFERINFODATA, this);
curl_easy_setopt(curl, CURLOPT_XFERINFOFUNCTION, &Updater::download_file_xferinfo);
CURLcode res = curl_easy_perform(curl);
long response_code = 0;
curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &response_code);
// double content_length = 0.0;
// curl_easy_getinfo(curl, CURLINFO_CONTENT_LENGTH_DOWNLOAD, &content_length);
printf("download %s res %d, code %ld, resume from %ld\n", url.c_str(), res, response_code, resume_from);
if (res == CURLE_OK) {
ret = true;
break;
} else if (res == CURLE_HTTP_RETURNED_ERROR && response_code == 416) {
// failed because the file is already complete?
ret = true;
break;
} else if (resume_from == last_resume_from) {
// failed and dind't make make forward progress. only retry a couple times
tries--;
if (tries <= 0) {
break;
}
}
last_resume_from = resume_from;
}
// printf("res %d\n", res);
// printf("- %ld %f\n", response_code, content_length);
fclose(of);
return ret;
}
void set_progress(std::string text) {
std::lock_guard<std::mutex> guard(lock);
progress_text = text;
}
void set_error(std::string text) {
std::lock_guard<std::mutex> guard(lock);
error_text = text;
state = ERROR;
}
std::string stage_download(std::string url, std::string hash, std::string name) {
std::string out_fn = UPDATE_DIR "/" + util::base_name(url);
set_progress("downloading " + name + "...");
bool r = download_file(url, out_fn);
if (!r) {
set_error("failed to download " + name);
return "";
}
set_progress("verifying " + name + "...");
std::string fn_hash = sha256_file(out_fn);
printf("got %s hash: %s\n", name.c_str(), hash.c_str());
if (fn_hash != hash) {
set_error(name + " was corrupt");
unlink(out_fn.c_str());
return "";
}
return out_fn;
}
void run_stages() {
curl = curl_easy_init();
assert(curl);
if (!check_battery()) {
set_error("Please plug power in to your EON and wait for charge");
return;
}
if (!check_space()) {
set_error("2GB of free space required to update");
return;
}
mkdir(UPDATE_DIR, 0777);
const int EON = (access("/EON", F_OK) != -1);
set_progress("finding latest version...");
std::string manifest_s;
if (EON) {
manifest_s = download_string(curl, manifest_url);
} else {
// don't update NEO
exit(0);
}
printf("manifest: %s\n", manifest_s.c_str());
std::string err;
auto manifest = json11::Json::parse(manifest_s, err);
if (manifest.is_null() || !err.empty()) {
set_error("failed to load update manifest");
return;
}
std::string ota_url = manifest["ota_url"].string_value();
std::string ota_hash = manifest["ota_hash"].string_value();
std::string recovery_url = manifest["recovery_url"].string_value();
std::string recovery_hash = manifest["recovery_hash"].string_value();
int recovery_len = manifest["recovery_len"].int_value();
// std::string installer_url = manifest["installer_url"].string_value();
// std::string installer_hash = manifest["installer_hash"].string_value();
if (ota_url.empty() || ota_hash.empty()) {
set_error("invalid update manifest");
return;
}
// std::string installer_fn = stage_download(installer_url, installer_hash, "installer");
// if (installer_fn.empty()) {
// //error'd
// return;
// }
std::string recovery_fn;
if (recovery_url.empty() || recovery_hash.empty() || recovery_len == 0) {
set_progress("skipping recovery flash...");
} else {
// only download the recovery if it differs from what's flashed
set_progress("checking recovery...");
std::string existing_recovery_hash = sha256_file(RECOVERY_DEV, recovery_len);
printf("existing recovery hash: %s\n", existing_recovery_hash.c_str());
if (existing_recovery_hash != recovery_hash) {
recovery_fn = stage_download(recovery_url, recovery_hash, "recovery");
if (recovery_fn.empty()) {
// error'd
return;
}
}
}
std::string ota_fn = stage_download(ota_url, ota_hash, "update");
if (ota_fn.empty()) {
//error'd
return;
}
if (!check_battery()) {
set_error("must have at least 35% battery to update");
return;
}
if (!recovery_fn.empty()) {
// flash recovery
set_progress("flashing recovery...");
FILE *flash_file = fopen(recovery_fn.c_str(), "rb");
if (!flash_file) {
set_error("failed to flash recovery");
return;
}
FILE *recovery_dev = fopen(RECOVERY_DEV, "w+b");
if (!recovery_dev) {
fclose(flash_file);
set_error("failed to flash recovery");
return;
}
const size_t buf_size = 4096;
std::unique_ptr<char[]> buffer( new char[ buf_size ] );
while (true) {
size_t bytes_read = fread(buffer.get(), 1, buf_size, flash_file);
if (!bytes_read) break;
size_t bytes_written = fwrite(buffer.get(), 1, bytes_read, recovery_dev);
if (bytes_read != bytes_written) {
fclose(recovery_dev);
fclose(flash_file);
set_error("failed to flash recovery: write failed");
return;
}
}
fclose(recovery_dev);
fclose(flash_file);
set_progress("verifying flash...");
std::string new_recovery_hash = sha256_file(RECOVERY_DEV, recovery_len);
printf("new recovery hash: %s\n", new_recovery_hash.c_str());
if (new_recovery_hash != recovery_hash) {
set_error("recovery flash corrupted");
return;
}
}
// write arguments to recovery
FILE *cmd_file = fopen(RECOVERY_COMMAND, "wb");
if (!cmd_file) {
set_error("failed to reboot into recovery");
return;
}
fprintf(cmd_file, "--update_package=%s\n", ota_fn.c_str());
fclose(cmd_file);
set_progress("rebooting");
// remove the continue.sh so we come back into the setup.
// maybe we should go directly into the installer, but what if we don't come back with internet? :/
//unlink("/data/data/com.termux/files/continue.sh");
// TODO: this should be generic between android versions
// IPowerManager.reboot(confirm=false, reason="recovery", wait=true)
system("service call power 16 i32 0 s16 recovery i32 1");
while(1) pause();
// execl("/system/bin/reboot", "recovery");
// set_error("failed to reboot into recovery");
}
void draw_ack_screen(const char *message, const char *button, const char *altbutton) {
nvgFontSize(vg, 96.0f);
nvgFillColor(vg, nvgRGBA(255,255,255,255));
nvgTextAlign(vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
nvgTextBox(vg, 50, 100, fb_w-100, message, NULL);
// draw button
if (button) {
nvgBeginPath(vg);
nvgFillColor(vg, nvgRGBA(0, 0, 0, 255));
nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
nvgFill(vg);
nvgFillColor(vg, nvgRGBA(255, 255, 255, 255));
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
nvgText(vg, b_x+b_w/2, b_y+b_h/2, button, NULL);
nvgBeginPath(vg);
nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 255));
nvgStrokeWidth(vg, 5);
nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
nvgStroke(vg);
}
// draw button
if (altbutton) {
nvgBeginPath(vg);
nvgFillColor(vg, nvgRGBA(0, 0, 0, 255));
nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20);
nvgFill(vg);
nvgFillColor(vg, nvgRGBA(255, 255, 255, 255));
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
nvgText(vg, balt_x+b_w/2, b_y+b_h/2, altbutton, NULL);
nvgBeginPath(vg);
nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 255));
nvgStrokeWidth(vg, 5);
nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20);
nvgStroke(vg);
}
}
void draw_progress_screen() {
// draw progress message
nvgFontSize(vg, 64.0f);
nvgFillColor(vg, nvgRGBA(255,255,255,255));
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
nvgTextBox(vg, 0, 700, fb_w, progress_text.c_str(), NULL);
// draw progress bar
{
int progress_width = 800;
int progress_x = fb_w/2-progress_width/2;
int progress_y = 768;
int progress_height = 15;
int powerprompt_y = 512;
nvgText(vg, fb_w/2, powerprompt_y, "Ensure EON is connected to power", NULL);
NVGpaint paint = nvgBoxGradient(
vg, progress_x + 1, progress_y + 1,
progress_width - 2, progress_height, 3, 4, nvgRGB(0, 32, 0), nvgRGB(0, 92, 0));
nvgBeginPath(vg);
nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 3);
nvgFillPaint(vg, paint);
nvgFill(vg);
float value = std::min(std::max(0.0f, progress_frac), 1.0f);
int bar_pos = ((progress_width - 2) * value);
paint = nvgBoxGradient(
vg, progress_x, progress_y,
bar_pos+1.5f, progress_height-1, 3, 4,
nvgRGB(220, 100, 0), nvgRGB(128, 100, 0));
nvgBeginPath(vg);
nvgRoundedRect(
vg, progress_x+1, progress_y+1,
bar_pos, progress_height-2, 3);
nvgFillPaint(vg, paint);
nvgFill(vg);
}
}
void ui_draw() {
std::lock_guard<std::mutex> guard(lock);
nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
switch (state) {
case CONFIRMATION:
draw_ack_screen("An upgrade to NEOS is required.\n\n"
"Your device will now be reset and upgraded. You may want to connect to wifi as download is around 1 GB\nData on device shouldn't be lost.",
"continue",
"wifi");
break;
case RUNNING:
draw_progress_screen();
break;
case ERROR:
draw_ack_screen(("ERROR: " + error_text + "\n\nYou will need to retry").c_str(), NULL, "exit");
break;
}
nvgEndFrame(vg);
}
void ui_update() {
std::lock_guard<std::mutex> guard(lock);
switch (state) {
case ERROR:
case CONFIRMATION: {
int touch_x = -1, touch_y = -1;
int res = touch_poll(&touch, &touch_x, &touch_y, 0);
if (res == 1 && !is_settings_active()) {
if (touch_x >= b_x && touch_x < b_x+b_w && touch_y >= b_y && touch_y < b_y+b_h) {
if (state == CONFIRMATION) {
state = RUNNING;
update_thread_handle = std::thread(&Updater::run_stages, this);
}
}
if (touch_x >= balt_x && touch_x < balt_x+b_w && touch_y >= b_y && touch_y < b_y+b_h) {
if (state == CONFIRMATION) {
start_settings_activity("Settings$WifiSettingsActivity");
} else if (state == ERROR) {
do_exit = 1;
}
}
}
}
default:
break;
}
}
void go() {
while (!do_exit) {
ui_update();
glClearColor(0.19, 0.09, 0.2, 1.0);
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
ui_draw();
glDisable(GL_BLEND);
eglSwapBuffers(display, surface);
assert(glGetError() == GL_NO_ERROR);
// no simple way to do 30fps vsync with surfaceflinger...
usleep(30000);
}
if (update_thread_handle.joinable()) {
update_thread_handle.join();
}
system("service call power 16 i32 0 i32 0 i32 1");
}
bool is_settings_active() {
FILE *fp;
char sys_output[4096];
fp = popen("/bin/dumpsys window windows", "r");
if (fp == NULL) {
return false;
}
bool active = false;
while (fgets(sys_output, sizeof(sys_output), fp) != NULL) {
if (strstr(sys_output, "mCurrentFocus=null") != NULL) {
break;
}
if (strstr(sys_output, "mCurrentFocus=Window") != NULL) {
active = true;
break;
}
}
pclose(fp);
return active;
}
};
}
int main(int argc, char *argv[]) {
if (argc > 1) {
if (strcmp(argv[1], "local") == 0) {
manifest_url = MANIFEST_URL_EON_LOCAL;
} else if (strcmp(argv[1], "staging") == 0) {
manifest_url = MANIFEST_URL_EON_STAGING;
} else {
manifest_url = argv[1];
}
}
printf("updating from %s\n", manifest_url);
Updater updater;
updater.go();
return 0;
}
launch_chffrplus.sh
+16 -3
View File
@@ -1,5 +1,11 @@
#!/usr/bin/bash
export OMP_NUM_THREADS=1
export MKL_NUM_THREADS=1
export NUMEXPR_NUM_THREADS=1
export OPENBLAS_NUM_THREADS=1
export VECLIB_MAXIMUM_THREADS=1
if [ -z "$PASSIVE" ]; then
export PASSIVE="1"
fi
@@ -7,9 +13,16 @@ fi
function launch {
# apply update
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
git reset --hard @{u} &&
git clean -xdf &&
exec "${BASH_SOURCE[0]}"
git reset --hard @{u} &&
git clean -xdf &&
# Touch all files on release2 after checkout to prevent rebuild
BRANCH=$(git rev-parse --abbrev-ref HEAD)
if [[ "$BRANCH" == "release2" ]]; then
touch **
fi
exec "${BASH_SOURCE[0]}"
fi
# no cpu rationing for now
models/driving_model.dlc
BIN
View File
Binary file not shown.
opendbc/generator/toyota/toyota_camry_hybrid_2018_pt.dbc
+1 -1
View File
@@ -16,7 +16,7 @@ BO_ 581 GAS_PEDAL: 8 XXX
SG_ GAS_PEDAL : 23|8@0+ (0.005,0) [0|1] "" XXX
BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_EPS : 47|16@0- (0.66,0) [-20000|20000] "" XXX
SG_ STEER_TORQUE_EPS : 47|16@0- (0.73,0) [-20000|20000] "" XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
opendbc/generator/toyota/toyota_chr_hybrid_2018_pt.dbc → opendbc/generator/toyota/toyota_nodsu_hybrid_pt.dbc
+1
View File
@@ -20,6 +20,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.056,0) [-500|500] "" XXX
BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
opendbc/generator/toyota/toyota_chr_2018_pt.dbc → opendbc/generator/toyota/toyota_nodsu_pt.dbc
+12
View File
@@ -1,6 +1,17 @@
CM_ "IMPORT _toyota_2017.dbc"
CM_ "IMPORT _comma.dbc"
BO_ 401 STEERING_LTA: 8 XXX
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX
SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX
SG_ STEER_ANGLE_CMD : 15|16@0- (0.056,0) [-540|540] "" XXX
SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX
SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 550 BRAKE_MODULE: 8 XXX
SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX
@@ -15,6 +26,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.056,0) [-500|500] "" XXX
BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc
+3 -2
View File
@@ -150,14 +150,15 @@ BO_ 780 ACC_HUD: 8 ADAS
SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY
SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY
SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY
SG_ SET_ME_X03 : 47|2@0+ (1,0) [0|3] "" BDY
SG_ HUD_DISTANCE : 47|2@0+ (1,0) [0|3] "" BDY
SG_ HUD_LEAD : 45|2@0+ (1,0) [0|3] "" BDY
SG_ BOH_3 : 43|1@0+ (1,0) [0|3] "" BDY
SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY
SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY
SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY
SG_ HUD_DISTANCE_3 : 52|1@0+ (1,0) [0|1] "" BDY
SG_ SET_ME_X03_2 : 55|2@0+ (1,0) [0|3] "" BDY
SG_ IMPERIAL_UNIT : 54|1@0+ (1,0) [0|1] "" BDY
SG_ SET_ME_X01_2 : 55|1@0+ (1,0) [0|1] "" BDY
SG_ SET_ME_X01 : 48|1@0+ (1,0) [0|1] "" BDY
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" BDY
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" BDY
opendbc/toyota_prius_2017_adas.dbc → opendbc/toyota_adas.dbc
View File
opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
+1 -1
View File
@@ -314,7 +314,7 @@ BO_ 581 GAS_PEDAL: 8 XXX
SG_ GAS_PEDAL : 23|8@0+ (0.005,0) [0|1] "" XXX
BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_EPS : 47|16@0- (0.66,0) [-20000|20000] "" XXX
SG_ STEER_TORQUE_EPS : 47|16@0- (0.73,0) [-20000|20000] "" XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
opendbc/toyota_chr_hybrid_2018_pt_generated.dbc → opendbc/toyota_nodsu_hybrid_pt_generated.dbc
+2 -1
View File
@@ -295,7 +295,7 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "toyota_chr_hybrid_2018_pt.dbc starts here"
CM_ "toyota_nodsu_hybrid_pt.dbc starts here"
@@ -318,6 +318,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.056,0) [-500|500] "" XXX
BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
opendbc/toyota_chr_2018_pt_generated.dbc → opendbc/toyota_nodsu_pt_generated.dbc
+13 -1
View File
@@ -295,10 +295,21 @@ VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "toyota_chr_2018_pt.dbc starts here"
CM_ "toyota_nodsu_pt.dbc starts here"
BO_ 401 STEERING_LTA: 8 XXX
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX
SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX
SG_ STEER_ANGLE_CMD : 15|16@0- (0.056,0) [-540|540] "" XXX
SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX
SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 550 BRAKE_MODULE: 8 XXX
SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX
@@ -313,6 +324,7 @@ BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ STEER_ANGLE : 31|16@0- (0.056,0) [-500|500] "" XXX
BO_ 610 EPS_STATUS: 8 EPS
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
opendbc/toyota_rav4_2019_adas.dbc → opendbc/toyota_tss2_adas.dbc
View File
panda/.circleci/config.yml
+26 -2
View File
@@ -26,9 +26,19 @@ jobs:
command: |
mkdir /tmp/misra
docker run -v /tmp/misra:/tmp/misra panda_misra /bin/bash -c "cd /panda/tests/misra; ./test_misra.sh"
- store_artifacts:
name: Store cppcheck test output
path: /tmp/misra/cppcheck_output.txt
- store_artifacts:
name: Store misra test output
path: /tmp/misra/output.txt
path: /tmp/misra/misra_output.txt
- store_artifacts:
name: Store cppcheck safety test output
path: /tmp/misra/cppcheck_safety_output.txt
- store_artifacts:
name: Store misra safety test output
path: /tmp/misra/misra_safety_output.txt
strict-compiler:
machine:
@@ -41,7 +51,7 @@ jobs:
- run:
name: Build Panda with strict compiler rules
command: |
docker run panda_strict_compiler /bin/bash -c "cd /panda/board; make -f Makefile.strict clean; make -f Makefile.strict bin"
docker run panda_strict_compiler /bin/bash -c "cd /panda/tests/build_strict; ./test_build_strict.sh"
build:
machine:
@@ -76,6 +86,19 @@ jobs:
command: |
docker run panda_build /bin/bash -c "cd /panda/boardesp; make user1.bin"
safety_replay:
machine:
docker_layer_caching: true
steps:
- checkout
- run:
name: Build image
command: "docker build -t panda_safety_replay -f tests/safety_replay/Dockerfile ."
- run:
name: Replay drives
command: |
docker run panda_safety_replay /bin/bash -c "cd /openpilot/panda/tests/safety_replay; PYTHONPATH=/openpilot ./test_safety_replay.py"
workflows:
version: 2
main:
@@ -84,3 +107,4 @@ workflows:
- misra-c2012
- strict-compiler
- build
- safety_replay
panda/VERSION
+1 -1
View File
@@ -1 +1 @@
v1.3.2
v1.4.0
panda/board/Makefile.strict
+1 -1
View File
@@ -1,5 +1,5 @@
PROJ_NAME = panda
CFLAGS = -g -Wall -Wextra -pedantic -Wstrict-prototypes
CFLAGS = -g -Wall -Wextra -Wstrict-prototypes
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4
CFLAGS += -mhard-float -DSTM32F4 -DSTM32F413xx -mfpu=fpv4-sp-d16 -fsingle-precision-constant
panda/board/bootstub.c
+1 -1
View File
@@ -13,7 +13,7 @@
#endif
// default since there's no serial
int puts(const char *a) { return 0; }
void puts(const char *a) {}
void puth(unsigned int i) {}
#include "libc.h"
panda/board/build.mk
+3 -1
View File
@@ -2,12 +2,15 @@ CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -Os
CFLAGS += -Tstm32_flash.ld
DFU_UTIL = "dfu-util"
# Compile fast charge (DCP) only not on EON
ifeq (,$(wildcard /EON))
BUILDER = DEV
else
CFLAGS += "-DEON"
BUILDER = EON
DFU_UTIL = "tools/dfu-util-aarch64"
endif
CC = arm-none-eabi-gcc
@@ -22,7 +25,6 @@ else
CFLAGS += "-DALLOW_DEBUG"
endif
DFU_UTIL = "dfu-util"
DEPDIR = generated_dependencies
$(shell mkdir -p -m 777 $(DEPDIR) >/dev/null)
panda/board/config.h
+5 -5
View File
@@ -22,17 +22,17 @@
#include <stdbool.h>
#define NULL ((void*)0)
#define COMPILE_TIME_ASSERT(pred) switch(0){case 0:case pred:;}
#define COMPILE_TIME_ASSERT(pred) ((void)sizeof(char[1 - (2 * (!(pred)))]))
#define min(a,b) \
#define MIN(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
(_a < _b) ? _a : _b; })
#define max(a,b) \
#define MAX(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
(_a > _b) ? _a : _b; })
#define MAX_RESP_LEN 0x40
panda/board/drivers/adc.h
+1 -1
View File
@@ -8,7 +8,7 @@
#define ADCCHAN_VOLTAGE 12
#define ADCCHAN_CURRENT 13
void adc_init() {
void adc_init(void) {
// global setup
ADC->CCR = ADC_CCR_TSVREFE | ADC_CCR_VBATE;
//ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_EOCS | ADC_CR2_DDS;
panda/board/drivers/can.h
+91 -95
View File
@@ -3,8 +3,8 @@
// CAN3_TX, CAN3_RX0, CAN3_SCE
typedef struct {
uint32_t w_ptr;
uint32_t r_ptr;
volatile uint32_t w_ptr;
volatile uint32_t r_ptr;
uint32_t fifo_size;
CAN_FIFOMailBox_TypeDef *elems;
} can_ring;
@@ -18,14 +18,14 @@ extern int can_live, pending_can_live;
// must reinit after changing these
extern int can_loopback, can_silent;
extern uint32_t can_speed[];
extern uint32_t can_speed[4];
void can_set_forwarding(int from, int to);
void can_init(uint8_t can_number);
void can_init_all();
void can_init_all(void);
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number);
int can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
// end API
@@ -48,10 +48,17 @@ can_buffer(tx3_q, 0x100)
can_buffer(txgmlan_q, 0x100)
can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q};
// global CAN stats
int can_rx_cnt = 0;
int can_tx_cnt = 0;
int can_txd_cnt = 0;
int can_err_cnt = 0;
int can_overflow_cnt = 0;
// ********************* interrupt safe queue *********************
int can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
int ret = 0;
bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
bool ret = 0;
enter_critical_section();
if (q->w_ptr != q->r_ptr) {
@@ -78,7 +85,12 @@ int can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
ret = 1;
}
exit_critical_section();
if (ret == 0) puts("can_push failed!\n");
if (ret == 0) {
can_overflow_cnt++;
#ifdef DEBUG
puts("can_push failed!\n");
#endif
}
return ret;
}
@@ -98,11 +110,6 @@ void can_clear(can_ring *q) {
// can_num_lookup: Translates from 'bus number' to 'can number'.
// can_forwarding: Given a bus num, lookup bus num to forward to. -1 means no forward.
int can_rx_cnt = 0;
int can_tx_cnt = 0;
int can_txd_cnt = 0;
int can_err_cnt = 0;
// Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3
CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3};
uint8_t bus_lookup[] = {0,1,2};
@@ -112,8 +119,8 @@ uint32_t can_speed[] = {5000, 5000, 5000, 333};
#define CAN_MAX 3
#define CANIF_FROM_CAN_NUM(num) (cans[num])
#define CAN_NUM_FROM_CANIF(CAN) (CAN==CAN1 ? 0 : (CAN==CAN2 ? 1 : 2))
#define CAN_NAME_FROM_CANIF(CAN) (CAN==CAN1 ? "CAN1" : (CAN==CAN2 ? "CAN2" : "CAN3"))
#define CAN_NUM_FROM_CANIF(CAN) ((CAN)==CAN1 ? 0 : ((CAN) == CAN2 ? 1 : 2))
#define CAN_NAME_FROM_CANIF(CAN) ((CAN)==CAN1 ? "CAN1" : ((CAN) == CAN2 ? "CAN2" : "CAN3"))
#define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num])
#define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num])
@@ -123,39 +130,34 @@ void can_set_speed(uint8_t can_number) {
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
while (true) {
if (llcan_set_speed(CAN, can_speed[bus_number], can_loopback, can_silent & (1 << can_number))) {
return;
}
if (!llcan_set_speed(CAN, can_speed[bus_number], can_loopback, can_silent & (1 << can_number))) {
puts("CAN init FAILED!!!!!\n");
puth(can_number); puts(" ");
puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n");
return;
}
}
void can_init(uint8_t can_number) {
if (can_number == 0xff) return;
if (can_number != 0xff) {
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
set_can_enable(CAN, 1);
can_set_speed(can_number);
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
set_can_enable(CAN, 1);
can_set_speed(can_number);
llcan_init(CAN);
llcan_init(CAN);
// in case there are queued up messages
process_can(can_number);
// in case there are queued up messages
process_can(can_number);
}
}
void can_init_all() {
void can_init_all(void) {
for (int i=0; i < CAN_MAX; i++) {
can_init(i);
}
}
void can_set_gmlan(int bus) {
if (bus == -1 || bus != can_num_lookup[3]) {
if ((bus == -1) || (bus != can_num_lookup[3])) {
// GMLAN OFF
switch (can_num_lookup[3]) {
case 1:
@@ -174,6 +176,9 @@ void can_set_gmlan(int bus) {
can_num_lookup[3] = -1;
can_init(2);
break;
default:
puts("GMLAN bus value invalid\n");
break;
}
}
@@ -227,67 +232,58 @@ void can_sce(CAN_TypeDef *CAN) {
// ***************************** CAN *****************************
void process_can(uint8_t can_number) {
if (can_number == 0xff) return;
if (can_number != 0xff) {
enter_critical_section();
enter_critical_section();
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
#ifdef DEBUG
puts("process CAN TX\n");
#endif
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
// check for empty mailbox
CAN_FIFOMailBox_TypeDef to_send;
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
// add successfully transmitted message to my fifo
if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
can_txd_cnt += 1;
// check for empty mailbox
CAN_FIFOMailBox_TypeDef to_send;
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
// add successfully transmitted message to my fifo
if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
can_txd_cnt += 1;
if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
CAN_FIFOMailBox_TypeDef to_push;
to_push.RIR = CAN->sTxMailBox[0].TIR;
to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
to_push.RDLR = CAN->sTxMailBox[0].TDLR;
to_push.RDHR = CAN->sTxMailBox[0].TDHR;
can_push(&can_rx_q, &to_push);
if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
CAN_FIFOMailBox_TypeDef to_push;
to_push.RIR = CAN->sTxMailBox[0].TIR;
to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
to_push.RDLR = CAN->sTxMailBox[0].TDLR;
to_push.RDHR = CAN->sTxMailBox[0].TDHR;
can_push(&can_rx_q, &to_push);
}
if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) {
#ifdef DEBUG
puts("CAN TX ERROR!\n");
#endif
}
if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) {
#ifdef DEBUG
puts("CAN TX ARBITRATION LOST!\n");
#endif
}
// clear interrupt
// careful, this can also be cleared by requesting a transmission
CAN->TSR |= CAN_TSR_RQCP0;
}
if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) {
#ifdef DEBUG
puts("CAN TX ERROR!\n");
#endif
if (can_pop(can_queues[bus_number], &to_send)) {
can_tx_cnt += 1;
// only send if we have received a packet
CAN->sTxMailBox[0].TDLR = to_send.RDLR;
CAN->sTxMailBox[0].TDHR = to_send.RDHR;
CAN->sTxMailBox[0].TDTR = to_send.RDTR;
CAN->sTxMailBox[0].TIR = to_send.RIR;
}
if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) {
#ifdef DEBUG
puts("CAN TX ARBITRATION LOST!\n");
#endif
}
// clear interrupt
// careful, this can also be cleared by requesting a transmission
CAN->TSR |= CAN_TSR_RQCP0;
}
if (can_pop(can_queues[bus_number], &to_send)) {
if (CAN->MCR & CAN_MCR_SLEEP) {
set_can_enable(CAN, 1);
CAN->MCR &= ~(CAN_MCR_SLEEP);
CAN->MCR |= CAN_MCR_INRQ;
while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
CAN->MCR &= ~(CAN_MCR_INRQ);
}
can_tx_cnt += 1;
// only send if we have received a packet
CAN->sTxMailBox[0].TDLR = to_send.RDLR;
CAN->sTxMailBox[0].TDHR = to_send.RDHR;
CAN->sTxMailBox[0].TDTR = to_send.RDTR;
CAN->sTxMailBox[0].TIR = to_send.RIR;
}
exit_critical_section();
}
exit_critical_section();
}
// CAN receive handlers
@@ -295,7 +291,7 @@ void process_can(uint8_t can_number) {
void can_rx(uint8_t can_number) {
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
while (CAN->RF0R & CAN_RF0R_FMP0) {
while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
can_rx_cnt += 1;
// can is live
@@ -312,7 +308,7 @@ void can_rx(uint8_t can_number) {
to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
// forwarding (panda only)
int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
int bus_fwd_num = (can_forwarding[bus_number] != -1) ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
if (bus_fwd_num != -1) {
CAN_FIFOMailBox_TypeDef to_send;
to_send.RIR = to_push.RIR | 1; // TXRQ
@@ -332,25 +328,25 @@ void can_rx(uint8_t can_number) {
}
}
void CAN1_TX_IRQHandler() { process_can(0); }
void CAN1_RX0_IRQHandler() { can_rx(0); }
void CAN1_SCE_IRQHandler() { can_sce(CAN1); }
void CAN1_TX_IRQHandler(void) { process_can(0); }
void CAN1_RX0_IRQHandler(void) { can_rx(0); }
void CAN1_SCE_IRQHandler(void) { can_sce(CAN1); }
void CAN2_TX_IRQHandler() { process_can(1); }
void CAN2_RX0_IRQHandler() { can_rx(1); }
void CAN2_SCE_IRQHandler() { can_sce(CAN2); }
void CAN2_TX_IRQHandler(void) { process_can(1); }
void CAN2_RX0_IRQHandler(void) { can_rx(1); }
void CAN2_SCE_IRQHandler(void) { can_sce(CAN2); }
void CAN3_TX_IRQHandler() { process_can(2); }
void CAN3_RX0_IRQHandler() { can_rx(2); }
void CAN3_SCE_IRQHandler() { can_sce(CAN3); }
void CAN3_TX_IRQHandler(void) { process_can(2); }
void CAN3_RX0_IRQHandler(void) { can_rx(2); }
void CAN3_SCE_IRQHandler(void) { can_sce(CAN3); }
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) {
if (safety_tx_hook(to_push)) {
if (safety_tx_hook(to_push) != 0) {
if (bus_number < BUS_MAX) {
// add CAN packet to send queue
// bus number isn't passed through
to_push->RDTR &= 0xF;
if (bus_number == 3 && can_num_lookup[3] == 0xFF) {
if ((bus_number == 3) && (can_num_lookup[3] == 0xFF)) {
// TODO: why uint8 bro? only int8?
bitbang_gmlan(to_push);
} else {
panda/board/drivers/clock.h
+3 -3
View File
@@ -1,4 +1,4 @@
void clock_init() {
void clock_init(void) {
// enable external oscillator
RCC->CR |= RCC_CR_HSEON;
while ((RCC->CR & RCC_CR_HSERDY) == 0);
@@ -25,7 +25,7 @@ void clock_init() {
// *** running on PLL ***
}
void watchdog_init() {
void watchdog_init(void) {
// setup watchdog
IWDG->KR = 0x5555;
IWDG->PR = 0; // divider /4
@@ -34,7 +34,7 @@ void watchdog_init() {
IWDG->KR = 0xCCCC;
}
void watchdog_feed() {
void watchdog_feed(void) {
IWDG->KR = 0xAAAA;
}
panda/board/drivers/gmlan_alt.h
+39 -33
View File
@@ -9,7 +9,7 @@
#define MAX_BITS_CAN_PACKET (200)
int gmlan_alt_mode = DISABLED;
int gmlan_alt_mode = DISABLED;
// returns out_len
int do_bitstuff(char *out, char *in, int in_len) {
@@ -18,7 +18,8 @@ int do_bitstuff(char *out, char *in, int in_len) {
int j = 0;
for (int i = 0; i < in_len; i++) {
char bit = in[i];
out[j++] = bit;
out[j] = bit;
j++;
// do the stuffing
if (bit == last_bit) {
@@ -26,7 +27,8 @@ int do_bitstuff(char *out, char *in, int in_len) {
if (bit_cnt == 5) {
// 5 in a row the same, do stuff
last_bit = !bit;
out[j++] = last_bit;
out[j] = last_bit;
j++;
bit_cnt = 1;
}
} else {
@@ -42,27 +44,30 @@ int append_crc(char *in, int in_len) {
int crc = 0;
for (int i = 0; i < in_len; i++) {
crc <<= 1;
if (in[i] ^ ((crc>>15)&1)) {
if ((in[i] ^ ((crc >> 15) & 1)) != 0) {
crc = crc ^ 0x4599;
}
crc &= 0x7fff;
}
for (int i = 14; i >= 0; i--) {
in[in_len++] = (crc>>i)&1;
in[in_len] = (crc>>i)&1;
in_len++;
}
return in_len;
}
int append_bits(char *in, int in_len, char *app, int app_len) {
for (int i = 0; i < app_len; i++) {
in[in_len++] = app[i];
in[in_len] = app[i];
in_len++;
}
return in_len;
}
int append_int(char *in, int in_len, int val, int val_len) {
for (int i = val_len-1; i >= 0; i--) {
in[in_len++] = (val&(1<<i)) != 0;
in[in_len] = (val&(1<<i)) != 0;
in_len++;
}
return in_len;
}
@@ -82,8 +87,8 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
// test packet
int dlc_len = to_bang->RDTR & 0xF;
len = append_int(pkt, len, 0, 1); // Start-of-frame
if (to_bang->RIR & 4) {
if ((to_bang->RIR & 4) != 0) {
// extended identifier
len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
len = append_int(pkt, len, 3, 2); // SRR+IDE
@@ -114,7 +119,7 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
return len;
}
void setup_timer4() {
void setup_timer4(void) {
// setup
TIM4->PSC = 48-1; // tick on 1 us
TIM4->CR1 = TIM_CR1_CEN; // enable
@@ -131,7 +136,7 @@ void setup_timer4() {
int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms
int can_timeout_counter = GMLAN_TICKS_PER_SECOND; //1 second
int inverted_bit_to_send = GMLAN_HIGH;
int inverted_bit_to_send = GMLAN_HIGH;
int gmlan_switch_below_timeout = -1;
int gmlan_switch_timeout_enable = 0;
@@ -140,9 +145,9 @@ void gmlan_switch_init(int timeout_enable) {
gmlan_alt_mode = GPIO_SWITCH;
gmlan_switch_below_timeout = 1;
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
setup_timer4();
inverted_bit_to_send = GMLAN_LOW; //We got initialized, set the output low
}
@@ -162,7 +167,7 @@ void reset_gmlan_switch_timeout(void) {
}
void set_bitbanged_gmlan(int val) {
if (val) {
if (val != 0) {
GPIOB->ODR |= (1 << 13);
} else {
GPIOB->ODR &= ~(1 << 13);
@@ -180,7 +185,7 @@ int gmlan_fail_count = 0;
void TIM4_IRQHandler(void) {
if (gmlan_alt_mode == BITBANG) {
if (TIM4->SR & TIM_SR_UIF && gmlan_sendmax != -1) {
if ((TIM4->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) {
int read = get_gpio_input(GPIOB, 12);
if (gmlan_silent_count < REQUIRED_SILENT_TIME) {
if (read == 0) {
@@ -189,16 +194,16 @@ void TIM4_IRQHandler(void) {
gmlan_silent_count++;
}
} else if (gmlan_silent_count == REQUIRED_SILENT_TIME) {
int retry = 0;
bool retry = 0;
// in send loop
if (gmlan_sending > 0 && // not first bit
(read == 0 && pkt_stuffed[gmlan_sending-1] == 1) && // bus wrongly dominant
gmlan_sending != (gmlan_sendmax-11)) { //not ack bit
if ((gmlan_sending > 0) && // not first bit
((read == 0) && (pkt_stuffed[gmlan_sending-1] == 1)) && // bus wrongly dominant
(gmlan_sending != (gmlan_sendmax - 11))) { //not ack bit
puts("GMLAN ERR: bus driven at ");
puth(gmlan_sending);
puts("\n");
retry = 1;
} else if (read == 1 && gmlan_sending == (gmlan_sendmax-11)) { // recessive during ACK
} else if ((read == 1) && (gmlan_sending == (gmlan_sendmax - 11))) { // recessive during ACK
puts("GMLAN ERR: didn't recv ACK\n");
retry = 1;
}
@@ -216,7 +221,7 @@ void TIM4_IRQHandler(void) {
gmlan_sending++;
}
}
if (gmlan_sending == gmlan_sendmax || gmlan_fail_count == MAX_FAIL_COUNT) {
if ((gmlan_sending == gmlan_sendmax) || (gmlan_fail_count == MAX_FAIL_COUNT)) {
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_INPUT);
TIM4->DIER = 0; // no update interrupt
@@ -228,8 +233,8 @@ void TIM4_IRQHandler(void) {
} //bit bang mode
else if (gmlan_alt_mode == GPIO_SWITCH) {
if (TIM4->SR & TIM_SR_UIF && gmlan_switch_below_timeout != -1) {
if (can_timeout_counter == 0 && gmlan_switch_timeout_enable) {
if ((TIM4->SR & TIM_SR_UIF) && (gmlan_switch_below_timeout != -1)) {
if ((can_timeout_counter == 0) && gmlan_switch_timeout_enable) {
//it has been more than 1 second since timeout was reset; disable timer and restore the GMLAN output
set_gpio_output(GPIOB, 13, GMLAN_LOW);
gmlan_switch_below_timeout = -1;
@@ -256,18 +261,19 @@ void TIM4_IRQHandler(void) {
void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
gmlan_alt_mode = BITBANG;
// TODO: make failure less silent
if (gmlan_sendmax != -1) return;
if (gmlan_sendmax == -1) {
int len = get_bit_message(pkt_stuffed, to_bang);
gmlan_fail_count = 0;
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_sendmax = len;
int len = get_bit_message(pkt_stuffed, to_bang);
gmlan_fail_count = 0;
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_sendmax = len;
// setup for bitbang loop
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
// setup for bitbang loop
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
setup_timer4();
setup_timer4();
}
}
panda/board/drivers/llcan.h
+5 -4
View File
@@ -31,12 +31,13 @@ bool llcan_set_speed(CAN_TypeDef *CAN, uint32_t speed, bool loopback, bool silen
#define CAN_TIMEOUT 1000000
int tmp = 0;
while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK && tmp < CAN_TIMEOUT) tmp++;
bool ret = false;
while(((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK) && (tmp < CAN_TIMEOUT)) tmp++;
if (tmp < CAN_TIMEOUT) {
return true;
ret = true;
}
return false;
return ret;
}
void llcan_init(CAN_TypeDef *CAN) {
@@ -48,7 +49,7 @@ void llcan_init(CAN_TypeDef *CAN) {
CAN->sFilterRegister[0].FR2 = 0;
CAN->sFilterRegister[14].FR1 = 0;
CAN->sFilterRegister[14].FR2 = 0;
CAN->FA1R |= 1 | (1 << 14);
CAN->FA1R |= 1 | (1U << 14);
CAN->FMR &= ~(CAN_FMR_FINIT);
panda/board/drivers/llgpio.h
+3 -3
View File
@@ -14,8 +14,8 @@ void set_gpio_mode(GPIO_TypeDef *GPIO, int pin, int mode) {
GPIO->MODER = tmp;
}
void set_gpio_output(GPIO_TypeDef *GPIO, int pin, int val) {
if (val) {
void set_gpio_output(GPIO_TypeDef *GPIO, int pin, bool enabled) {
if (enabled) {
GPIO->ODR |= (1 << pin);
} else {
GPIO->ODR &= ~(1 << pin);
@@ -39,6 +39,6 @@ void set_gpio_pullup(GPIO_TypeDef *GPIO, int pin, int mode) {
}
int get_gpio_input(GPIO_TypeDef *GPIO, int pin) {
return (GPIO->IDR & (1 << pin)) == (1 << pin);
return (GPIO->IDR & (1U << pin)) == (1U << pin);
}
panda/board/drivers/spi.h
+3 -3
View File
@@ -1,6 +1,6 @@
// IRQs: DMA2_Stream2, DMA2_Stream3, EXTI4
void spi_init();
void spi_init(void);
int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out);
// end API
@@ -10,7 +10,7 @@ uint8_t spi_buf[SPI_BUF_SIZE];
int spi_buf_count = 0;
int spi_total_count = 0;
void spi_init() {
void spi_init(void) {
//puts("SPI init\n");
SPI1->CR1 = SPI_CR1_SPE;
@@ -118,7 +118,7 @@ void EXTI4_IRQHandler(void) {
puts("exti4\n");
#endif
// SPI CS falling
if (pr & (1 << 4)) {
if ((pr & (1 << 4)) != 0) {
spi_total_count = 0;
spi_rx_dma(spi_buf, 0x14);
}
panda/board/drivers/uart.h
+71 -44
View File
@@ -2,11 +2,11 @@
#define FIFO_SIZE 0x400
typedef struct uart_ring {
uint16_t w_ptr_tx;
uint16_t r_ptr_tx;
volatile uint16_t w_ptr_tx;
volatile uint16_t r_ptr_tx;
uint8_t elems_tx[FIFO_SIZE];
uint16_t w_ptr_rx;
uint16_t r_ptr_rx;
volatile uint16_t w_ptr_rx;
volatile uint16_t r_ptr_rx;
uint8_t elems_rx[FIFO_SIZE];
USART_TypeDef *uart;
void (*callback)(struct uart_ring*);
@@ -14,10 +14,10 @@ typedef struct uart_ring {
void uart_init(USART_TypeDef *u, int baud);
int getc(uart_ring *q, char *elem);
int putc(uart_ring *q, char elem);
bool getc(uart_ring *q, char *elem);
bool putc(uart_ring *q, char elem);
int puts(const char *a);
void puts(const char *a);
void puth(unsigned int i);
void hexdump(const void *a, int l);
@@ -50,18 +50,25 @@ uart_ring debug_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
uart_ring *get_ring_by_number(int a) {
uart_ring *ring = NULL;
switch(a) {
case 0:
return &debug_ring;
ring = &debug_ring;
break;
case 1:
return &esp_ring;
ring = &esp_ring;
break;
case 2:
return &lin1_ring;
ring = &lin1_ring;
break;
case 3:
return &lin2_ring;
ring = &lin2_ring;
break;
default:
return NULL;
ring = NULL;
break;
}
return ring;
}
// ***************************** serial port *****************************
@@ -72,31 +79,32 @@ void uart_ring_process(uart_ring *q) {
int sr = q->uart->SR;
if (q->w_ptr_tx != q->r_ptr_tx) {
if (sr & USART_SR_TXE) {
if ((sr & USART_SR_TXE) != 0) {
q->uart->DR = q->elems_tx[q->r_ptr_tx];
q->r_ptr_tx = (q->r_ptr_tx + 1) % FIFO_SIZE;
} else {
// push on interrupt later
q->uart->CR1 |= USART_CR1_TXEIE;
}
// there could be more to send
q->uart->CR1 |= USART_CR1_TXEIE;
} else {
// nothing to send
q->uart->CR1 &= ~USART_CR1_TXEIE;
}
if (sr & USART_SR_RXNE || sr & USART_SR_ORE) {
if ((sr & USART_SR_RXNE) || (sr & USART_SR_ORE)) {
uint8_t c = q->uart->DR; // TODO: can drop packets
if (q != &esp_ring) {
uint16_t next_w_ptr = (q->w_ptr_rx + 1) % FIFO_SIZE;
if (next_w_ptr != q->r_ptr_rx) {
q->elems_rx[q->w_ptr_rx] = c;
q->w_ptr_rx = next_w_ptr;
if (q->callback) q->callback(q);
if (q->callback != NULL) {
q->callback(q);
}
}
}
}
if (sr & USART_SR_ORE) {
if ((sr & USART_SR_ORE) != 0) {
// set dropped packet flag?
}
@@ -110,22 +118,22 @@ void USART2_IRQHandler(void) { uart_ring_process(&debug_ring); }
void USART3_IRQHandler(void) { uart_ring_process(&lin2_ring); }
void UART5_IRQHandler(void) { uart_ring_process(&lin1_ring); }
int getc(uart_ring *q, char *elem) {
int ret = 0;
bool getc(uart_ring *q, char *elem) {
bool ret = false;
enter_critical_section();
if (q->w_ptr_rx != q->r_ptr_rx) {
*elem = q->elems_rx[q->r_ptr_rx];
if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx];
q->r_ptr_rx = (q->r_ptr_rx + 1) % FIFO_SIZE;
ret = 1;
ret = true;
}
exit_critical_section();
return ret;
}
int injectc(uart_ring *q, char elem) {
int ret = 0;
bool injectc(uart_ring *q, char elem) {
int ret = false;
uint16_t next_w_ptr;
enter_critical_section();
@@ -133,15 +141,15 @@ int injectc(uart_ring *q, char elem) {
if (next_w_ptr != q->r_ptr_rx) {
q->elems_rx[q->w_ptr_rx] = elem;
q->w_ptr_rx = next_w_ptr;
ret = 1;
ret = true;
}
exit_critical_section();
return ret;
}
int putc(uart_ring *q, char elem) {
int ret = 0;
bool putc(uart_ring *q, char elem) {
bool ret = false;
uint16_t next_w_ptr;
enter_critical_section();
@@ -149,7 +157,7 @@ int putc(uart_ring *q, char elem) {
if (next_w_ptr != q->r_ptr_tx) {
q->elems_tx[q->w_ptr_tx] = elem;
q->w_ptr_tx = next_w_ptr;
ret = 1;
ret = true;
}
exit_critical_section();
@@ -158,6 +166,24 @@ int putc(uart_ring *q, char elem) {
return ret;
}
void uart_flush(uart_ring *q) {
while (q->w_ptr_tx != q->r_ptr_tx) {
__WFI();
}
}
void uart_flush_sync(uart_ring *q) {
// empty the TX buffer
while (q->w_ptr_tx != q->r_ptr_tx) {
uart_ring_process(q);
}
}
void uart_send_break(uart_ring *u) {
while ((u->uart->CR1 & USART_CR1_SBK) != 0);
u->uart->CR1 |= USART_CR1_SBK;
}
void clear_uart_buff(uart_ring *q) {
enter_critical_section();
q->w_ptr_tx = 0;
@@ -169,10 +195,10 @@ void clear_uart_buff(uart_ring *q) {
// ***************************** start UART code *****************************
#define __DIV(_PCLK_, _BAUD_) (((_PCLK_)*25)/(4*(_BAUD_)))
#define __DIVMANT(_PCLK_, _BAUD_) (__DIV((_PCLK_), (_BAUD_))/100)
#define __DIVFRAQ(_PCLK_, _BAUD_) (((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100)) * 16 + 50) / 100)
#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4)|(__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0F))
#define __DIV(_PCLK_, _BAUD_) (((_PCLK_) * 25) / (4 * (_BAUD_)))
#define __DIVMANT(_PCLK_, _BAUD_) (__DIV((_PCLK_), (_BAUD_)) / 100)
#define __DIVFRAQ(_PCLK_, _BAUD_) ((((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100)) * 16) + 50) / 100)
#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4) | (__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0F))
void uart_set_baud(USART_TypeDef *u, int baud) {
if (u == USART1) {
@@ -186,19 +212,19 @@ void uart_set_baud(USART_TypeDef *u, int baud) {
#define USART1_DMA_LEN 0x20
char usart1_dma[USART1_DMA_LEN];
void uart_dma_drain() {
void uart_dma_drain(void) {
uart_ring *q = &esp_ring;
enter_critical_section();
if (DMA2->HISR & DMA_HISR_TCIF5 || DMA2->HISR & DMA_HISR_HTIF5 || DMA2_Stream5->NDTR != USART1_DMA_LEN) {
if ((DMA2->HISR & DMA_HISR_TCIF5) || (DMA2->HISR & DMA_HISR_HTIF5) || (DMA2_Stream5->NDTR != USART1_DMA_LEN)) {
// disable DMA
q->uart->CR3 &= ~USART_CR3_DMAR;
DMA2_Stream5->CR &= ~DMA_SxCR_EN;
while (DMA2_Stream5->CR & DMA_SxCR_EN);
while ((DMA2_Stream5->CR & DMA_SxCR_EN) != 0);
int i;
for (i = 0; i < USART1_DMA_LEN - DMA2_Stream5->NDTR; i++) {
unsigned int i;
for (i = 0; i < (USART1_DMA_LEN - DMA2_Stream5->NDTR); i++) {
char c = usart1_dma[i];
uint16_t next_w_ptr = (q->w_ptr_rx + 1) % FIFO_SIZE;
if (next_w_ptr != q->r_ptr_rx) {
@@ -280,22 +306,23 @@ void putch(const char a) {
}
}
int puts(const char *a) {
void puts(const char *a) {
for (;*a;a++) {
if (*a == '\n') putch('\r');
putch(*a);
}
return 0;
}
void putui(uint32_t i) {
char str[11];
uint8_t idx = 10;
str[idx--] = '\0';
str[idx] = '\0';
idx--;
do {
str[idx--] = (i % 10) + 0x30;
str[idx] = (i % 10) + 0x30;
idx--;
i /= 10;
} while (i);
} while (i != 0);
puts(str + idx + 1);
}
@@ -318,7 +345,7 @@ void puth2(unsigned int i) {
void hexdump(const void *a, int l) {
int i;
for (i=0;i<l;i++) {
if (i != 0 && (i&0xf) == 0) puts("\n");
if ((i != 0) && ((i & 0xf) == 0)) puts("\n");
puth2(((const unsigned char*)a)[i]);
puts(" ");
}
panda/board/drivers/usb.h
+64 -57
View File
@@ -23,12 +23,12 @@ typedef union _USB_Setup {
}
USB_Setup_TypeDef;
void usb_init();
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired);
int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired);
void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired);
void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired);
void usb_cb_enumeration_complete();
void usb_init(void);
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired);
int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired);
void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired);
void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired);
void usb_cb_enumeration_complete(void);
// **** supporting defines ****
@@ -43,9 +43,9 @@ USB_OTG_GlobalTypeDef *USBx = USB_OTG_FS;
#define USBx_HOST ((USB_OTG_HostTypeDef *)((uint32_t)USBx + USB_OTG_HOST_BASE))
#define USBx_HOST_PORT ((USB_OTG_HostPortTypeDef *)((uint32_t)USBx + USB_OTG_HOST_PORT_BASE))
#define USBx_DEVICE ((USB_OTG_DeviceTypeDef *)((uint32_t)USBx + USB_OTG_DEVICE_BASE))
#define USBx_INEP(i) ((USB_OTG_INEndpointTypeDef *)((uint32_t)USBx + USB_OTG_IN_ENDPOINT_BASE + (i)*USB_OTG_EP_REG_SIZE))
#define USBx_OUTEP(i) ((USB_OTG_OUTEndpointTypeDef *)((uint32_t)USBx + USB_OTG_OUT_ENDPOINT_BASE + (i)*USB_OTG_EP_REG_SIZE))
#define USBx_DFIFO(i) *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_FIFO_BASE + (i) * USB_OTG_FIFO_SIZE)
#define USBx_INEP(i) ((USB_OTG_INEndpointTypeDef *)((uint32_t)USBx + USB_OTG_IN_ENDPOINT_BASE + ((i) * USB_OTG_EP_REG_SIZE)))
#define USBx_OUTEP(i) ((USB_OTG_OUTEndpointTypeDef *)((uint32_t)USBx + USB_OTG_OUT_ENDPOINT_BASE + ((i) * USB_OTG_EP_REG_SIZE)))
#define USBx_DFIFO(i) *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_FIFO_BASE + ((i) * USB_OTG_FIFO_SIZE))
#define USBx_PCGCCTL *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_PCGCCTL_BASE)
#define USB_REQ_GET_STATUS 0x00
@@ -125,11 +125,11 @@ uint8_t resp[MAX_RESP_LEN];
// Convert machine byte order to USB byte order
#define TOUSBORDER(num)\
(num&0xFF), ((num>>8)&0xFF)
((num) & 0xFF), (((num) >> 8) & 0xFF)
// take in string length and return the first 2 bytes of a string descriptor
#define STRING_DESCRIPTOR_HEADER(size)\
(((size * 2 + 2)&0xFF) | 0x0300)
(((((size) * 2) + 2) & 0xFF) | 0x0300)
uint8_t device_desc[] = {
DSCR_DEVICE_LEN, USB_DESC_TYPE_DEVICE, //Length, Type
@@ -397,9 +397,10 @@ void *USB_ReadPacket(void *dest, uint16_t len) {
uint32_t i=0;
uint32_t count32b = (len + 3) / 4;
for ( i = 0; i < count32b; i++, dest += 4 ) {
for ( i = 0; i < count32b; i++) {
// packed?
*(__attribute__((__packed__)) uint32_t *)dest = USBx_DFIFO(0);
dest += 4;
}
return ((void *)dest);
}
@@ -420,8 +421,9 @@ void USB_WritePacket(const uint8_t *src, uint16_t len, uint32_t ep) {
USBx_INEP(ep)->DIEPCTL |= (USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA);
// load the FIFO
for (i = 0; i < count32b; i++, src += 4) {
for (i = 0; i < count32b; i++) {
USBx_DFIFO(ep) = *((__attribute__((__packed__)) uint32_t *)src);
src += 4;
}
}
@@ -433,7 +435,7 @@ void USB_WritePacket_EP0(uint8_t *src, uint16_t len) {
hexdump(src, len);
#endif
uint16_t wplen = min(len, 0x40);
uint16_t wplen = MIN(len, 0x40);
USB_WritePacket(src, wplen, 0);
if (wplen < len) {
@@ -445,7 +447,7 @@ void USB_WritePacket_EP0(uint8_t *src, uint16_t len) {
}
}
void usb_reset() {
void usb_reset(void) {
// unmask endpoint interrupts, so many sets
USBx_DEVICE->DAINT = 0xFFFFFFFF;
USBx_DEVICE->DAINTMSK = 0xFFFFFFFF;
@@ -489,14 +491,16 @@ void usb_reset() {
}
char to_hex_char(int a) {
char ret;
if (a < 10) {
return '0' + a;
ret = '0' + a;
} else {
return 'a' + (a-10);
ret = 'a' + (a - 10);
}
return ret;
}
void usb_setup() {
void usb_setup(void) {
int resp_len;
// setup packet is ready
switch (setup.b.bRequest) {
@@ -545,29 +549,29 @@ void usb_setup() {
//puts(" writing device descriptor\n");
// setup transfer
USB_WritePacket(device_desc, min(sizeof(device_desc), setup.b.wLength.w), 0);
USB_WritePacket(device_desc, MIN(sizeof(device_desc), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
//puts("D");
break;
case USB_DESC_TYPE_CONFIGURATION:
USB_WritePacket(configuration_desc, min(sizeof(configuration_desc), setup.b.wLength.w), 0);
USB_WritePacket(configuration_desc, MIN(sizeof(configuration_desc), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case USB_DESC_TYPE_DEVICE_QUALIFIER:
USB_WritePacket(device_qualifier, min(sizeof(device_qualifier), setup.b.wLength.w), 0);
USB_WritePacket(device_qualifier, MIN(sizeof(device_qualifier), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case USB_DESC_TYPE_STRING:
switch (setup.b.wValue.bw.msb) {
case STRING_OFFSET_LANGID:
USB_WritePacket((uint8_t*)string_language_desc, min(sizeof(string_language_desc), setup.b.wLength.w), 0);
USB_WritePacket((uint8_t*)string_language_desc, MIN(sizeof(string_language_desc), setup.b.wLength.w), 0);
break;
case STRING_OFFSET_IMANUFACTURER:
USB_WritePacket((uint8_t*)string_manufacturer_desc, min(sizeof(string_manufacturer_desc), setup.b.wLength.w), 0);
USB_WritePacket((uint8_t*)string_manufacturer_desc, MIN(sizeof(string_manufacturer_desc), setup.b.wLength.w), 0);
break;
case STRING_OFFSET_IPRODUCT:
USB_WritePacket((uint8_t*)string_product_desc, min(sizeof(string_product_desc), setup.b.wLength.w), 0);
USB_WritePacket((uint8_t*)string_product_desc, MIN(sizeof(string_product_desc), setup.b.wLength.w), 0);
break;
case STRING_OFFSET_ISERIAL:
#ifdef UID_BASE
@@ -583,16 +587,16 @@ void usb_setup() {
resp[2 + i*4 + 3] = '\0';
}
USB_WritePacket(resp, min(resp[0], setup.b.wLength.w), 0);
USB_WritePacket(resp, MIN(resp[0], setup.b.wLength.w), 0);
#else
USB_WritePacket((const uint8_t *)string_serial_desc, min(sizeof(string_serial_desc), setup.b.wLength.w), 0);
USB_WritePacket((const uint8_t *)string_serial_desc, MIN(sizeof(string_serial_desc), setup.b.wLength.w), 0);
#endif
break;
case STRING_OFFSET_ICONFIGURATION:
USB_WritePacket((uint8_t*)string_configuration_desc, min(sizeof(string_configuration_desc), setup.b.wLength.w), 0);
USB_WritePacket((uint8_t*)string_configuration_desc, MIN(sizeof(string_configuration_desc), setup.b.wLength.w), 0);
break;
case 238:
USB_WritePacket((uint8_t*)string_238_desc, min(sizeof(string_238_desc), setup.b.wLength.w), 0);
USB_WritePacket((uint8_t*)string_238_desc, MIN(sizeof(string_238_desc), setup.b.wLength.w), 0);
break;
default:
// nothing
@@ -602,7 +606,7 @@ void usb_setup() {
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case USB_DESC_TYPE_BINARY_OBJECT_STORE:
USB_WritePacket(binary_object_store_desc, min(sizeof(binary_object_store_desc), setup.b.wLength.w), 0);
USB_WritePacket(binary_object_store_desc, MIN(sizeof(binary_object_store_desc), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
default:
@@ -628,7 +632,7 @@ void usb_setup() {
case WEBUSB_VENDOR_CODE:
switch (setup.b.wIndex.w) {
case WEBUSB_REQ_GET_URL:
USB_WritePacket(webusb_url_descriptor, min(sizeof(webusb_url_descriptor), setup.b.wLength.w), 0);
USB_WritePacket(webusb_url_descriptor, MIN(sizeof(webusb_url_descriptor), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
default:
@@ -642,15 +646,15 @@ void usb_setup() {
switch (setup.b.wIndex.w) {
// winusb 2.0 descriptor from BOS
case WINUSB_REQ_GET_DESCRIPTOR:
USB_WritePacket_EP0((uint8_t*)winusb_20_desc, min(sizeof(winusb_20_desc), setup.b.wLength.w));
USB_WritePacket_EP0((uint8_t*)winusb_20_desc, MIN(sizeof(winusb_20_desc), setup.b.wLength.w));
break;
// Extended Compat ID OS Descriptor
case WINUSB_REQ_GET_COMPATID_DESCRIPTOR:
USB_WritePacket_EP0((uint8_t*)winusb_ext_compatid_os_desc, min(sizeof(winusb_ext_compatid_os_desc), setup.b.wLength.w));
USB_WritePacket_EP0((uint8_t*)winusb_ext_compatid_os_desc, MIN(sizeof(winusb_ext_compatid_os_desc), setup.b.wLength.w));
break;
// Extended Properties OS Descriptor
case WINUSB_REQ_GET_EXT_PROPS_OS:
USB_WritePacket_EP0((uint8_t*)winusb_ext_prop_os_desc, min(sizeof(winusb_ext_prop_os_desc), setup.b.wLength.w));
USB_WritePacket_EP0((uint8_t*)winusb_ext_prop_os_desc, MIN(sizeof(winusb_ext_prop_os_desc), setup.b.wLength.w));
break;
default:
USB_WritePacket_EP0(0, 0);
@@ -658,12 +662,12 @@ void usb_setup() {
break;
default:
resp_len = usb_cb_control_msg(&setup, resp, 1);
USB_WritePacket(resp, min(resp_len, setup.b.wLength.w), 0);
USB_WritePacket(resp, MIN(resp_len, setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
}
}
void usb_init() {
void usb_init(void) {
// full speed PHY, do reset and remove power down
/*puth(USBx->GRSTCTL);
puts(" resetting PHY\n");*/
@@ -756,27 +760,27 @@ void usb_irqhandler(void) {
puts(" USB interrupt!\n");
#endif
if (gintsts & USB_OTG_GINTSTS_CIDSCHG) {
if ((gintsts & USB_OTG_GINTSTS_CIDSCHG) != 0) {
puts("connector ID status change\n");
}
if (gintsts & USB_OTG_GINTSTS_ESUSP) {
if ((gintsts & USB_OTG_GINTSTS_ESUSP) != 0) {
puts("ESUSP detected\n");
}
if (gintsts & USB_OTG_GINTSTS_USBRST) {
if ((gintsts & USB_OTG_GINTSTS_USBRST) != 0) {
puts("USB reset\n");
usb_reset();
}
if (gintsts & USB_OTG_GINTSTS_ENUMDNE) {
if ((gintsts & USB_OTG_GINTSTS_ENUMDNE) != 0) {
puts("enumeration done");
// Full speed, ENUMSPD
//puth(USBx_DEVICE->DSTS);
puts("\n");
}
if (gintsts & USB_OTG_GINTSTS_OTGINT) {
if ((gintsts & USB_OTG_GINTSTS_OTGINT) != 0) {
puts("OTG int:");
puth(USBx->GOTGINT);
puts("\n");
@@ -786,7 +790,7 @@ void usb_irqhandler(void) {
}
// RX FIFO first
if (gintsts & USB_OTG_GINTSTS_RXFLVL) {
if ((gintsts & USB_OTG_GINTSTS_RXFLVL) != 0) {
// 1. Read the Receive status pop register
volatile unsigned int rxst = USBx->GRXSTSP;
@@ -848,7 +852,7 @@ void usb_irqhandler(void) {
USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGONAK | USB_OTG_DCTL_CGINAK;
}
if (gintsts & USB_OTG_GINTSTS_SRQINT) {
if ((gintsts & USB_OTG_GINTSTS_SRQINT) != 0) {
// we want to do "A-device host negotiation protocol" since we are the A-device
/*puts("start request\n");
puth(USBx->GOTGCTL);
@@ -859,7 +863,7 @@ void usb_irqhandler(void) {
}
// out endpoint hit
if (gintsts & USB_OTG_GINTSTS_OEPINT) {
if ((gintsts & USB_OTG_GINTSTS_OEPINT) != 0) {
#ifdef DEBUG_USB
puts(" 0:");
puth(USBx_OUTEP(0)->DOEPINT);
@@ -874,7 +878,7 @@ void usb_irqhandler(void) {
puts(" OUT ENDPOINT\n");
#endif
if (USBx_OUTEP(2)->DOEPINT & USB_OTG_DOEPINT_XFRC) {
if ((USBx_OUTEP(2)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0) {
#ifdef DEBUG_USB
puts(" OUT2 PACKET XFRC\n");
#endif
@@ -882,32 +886,32 @@ void usb_irqhandler(void) {
USBx_OUTEP(2)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
}
if (USBx_OUTEP(3)->DOEPINT & USB_OTG_DOEPINT_XFRC) {
if ((USBx_OUTEP(3)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0) {
#ifdef DEBUG_USB
puts(" OUT3 PACKET XFRC\n");
#endif
USBx_OUTEP(3)->DOEPTSIZ = (1 << 19) | 0x40;
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
} else if (USBx_OUTEP(3)->DOEPINT & 0x2000) {
} else if ((USBx_OUTEP(3)->DOEPINT & 0x2000) != 0) {
#ifdef DEBUG_USB
puts(" OUT3 PACKET WTF\n");
#endif
// if NAK was set trigger this, unknown interrupt
USBx_OUTEP(3)->DOEPTSIZ = (1 << 19) | 0x40;
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
} else if (USBx_OUTEP(3)->DOEPINT) {
} else if ((USBx_OUTEP(3)->DOEPINT) != 0) {
puts("OUTEP3 error ");
puth(USBx_OUTEP(3)->DOEPINT);
puts("\n");
}
if (USBx_OUTEP(0)->DOEPINT & USB_OTG_DIEPINT_XFRC) {
if ((USBx_OUTEP(0)->DOEPINT & USB_OTG_DIEPINT_XFRC) != 0) {
// ready for next packet
USBx_OUTEP(0)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) | (1 * 8);
}
// respond to setup packets
if (USBx_OUTEP(0)->DOEPINT & USB_OTG_DOEPINT_STUP) {
if ((USBx_OUTEP(0)->DOEPINT & USB_OTG_DOEPINT_STUP) != 0) {
usb_setup();
}
@@ -917,7 +921,7 @@ void usb_irqhandler(void) {
}
// interrupt endpoint hit (Page 1221)
if (gintsts & USB_OTG_GINTSTS_IEPINT) {
if ((gintsts & USB_OTG_GINTSTS_IEPINT) != 0) {
#ifdef DEBUG_USB
puts(" ");
puth(USBx_INEP(0)->DIEPINT);
@@ -932,8 +936,8 @@ void usb_irqhandler(void) {
// No need to set NAK in OTG_DIEPCTL0 when nothing to send,
// Appears USB core automatically sets NAK. WritePacket clears it.
// Handle the two interface alternate settings. Setting 0 is has
// EP1 as bulk. Setting 1 has EP1 as interrupt. The code to handle
// Handle the two interface alternate settings. Setting 0 has EP1
// as bulk. Setting 1 has EP1 as interrupt. The code to handle
// these two EP variations are very similar and can be
// restructured for smaller code footprint. Keeping split out for
// now for clarity.
@@ -942,7 +946,7 @@ void usb_irqhandler(void) {
switch (current_int0_alt_setting) {
case 0: ////// Bulk config
// *** IN token received when TxFIFO is empty
if (USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) {
if ((USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) {
#ifdef DEBUG_USB
puts(" IN PACKET QUEUE\n");
#endif
@@ -953,7 +957,7 @@ void usb_irqhandler(void) {
case 1: ////// Interrupt config
// *** IN token received when TxFIFO is empty
if (USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) {
if ((USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) {
#ifdef DEBUG_USB
puts(" IN PACKET QUEUE\n");
#endif
@@ -964,15 +968,18 @@ void usb_irqhandler(void) {
}
}
break;
default:
puts("current_int0_alt_setting value invalid\n");
break;
}
if (USBx_INEP(0)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) {
if ((USBx_INEP(0)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) {
#ifdef DEBUG_USB
puts(" IN PACKET QUEUE\n");
#endif
if (ep0_txlen != 0 && (USBx_INEP(0)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40) {
uint16_t len = min(ep0_txlen, 0x40);
if ((ep0_txlen != 0) && ((USBx_INEP(0)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40)) {
uint16_t len = MIN(ep0_txlen, 0x40);
USB_WritePacket(ep0_txdata, len, 0);
ep0_txdata += len;
ep0_txlen -= len;
panda/board/gpio.h
+33 -20
View File
@@ -13,23 +13,25 @@
#define PULL_EFFECTIVE_DELAY 10
int has_external_debug_serial = 0;
int is_giant_panda = 0;
int is_entering_bootmode = 0;
int revision = PANDA_REV_AB;
int is_grey_panda = 0;
void puts(const char *a);
int detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
bool has_external_debug_serial = 0;
bool is_giant_panda = 0;
bool is_entering_bootmode = 0;
int revision = PANDA_REV_AB;
bool is_grey_panda = 0;
bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
set_gpio_mode(GPIO, pin, MODE_INPUT);
set_gpio_pullup(GPIO, pin, mode);
for (volatile int i=0; i<PULL_EFFECTIVE_DELAY; i++);
int ret = get_gpio_input(GPIO, pin);
bool ret = get_gpio_input(GPIO, pin);
set_gpio_pullup(GPIO, pin, PULL_NONE);
return ret;
}
// must call again from main because BSS is zeroed
void detect() {
void detect(void) {
// detect has_external_debug_serial
has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN);
@@ -63,7 +65,7 @@ void detect() {
// ********************* bringup *********************
void periph_init() {
void periph_init(void) {
// enable GPIOB, UART2, CAN, USB clock
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
@@ -97,7 +99,7 @@ void periph_init() {
// ********************* setters *********************
void set_can_enable(CAN_TypeDef *CAN, int enabled) {
void set_can_enable(CAN_TypeDef *CAN, bool enabled) {
// enable CAN busses
if (CAN == CAN1) {
#ifdef PANDA
@@ -139,16 +141,16 @@ void set_can_enable(CAN_TypeDef *CAN, int enabled) {
#endif
void set_led(int led_num, int on) {
if (led_num == -1) return;
if (led_num != -1) {
#ifdef PANDA
set_gpio_output(GPIOC, led_num, !on);
#else
set_gpio_output(GPIOB, led_num, !on);
#endif
}
}
void set_can_mode(int can, int use_gmlan) {
void set_can_mode(int can, bool use_gmlan) {
// connects to CAN2 xcvr or GMLAN xcvr
if (use_gmlan) {
if (can == 1) {
@@ -200,6 +202,7 @@ void set_can_mode(int can, int use_gmlan) {
int usb_power_mode = USB_POWER_NONE;
void set_usb_power_mode(int mode) {
bool valid_mode = true;
switch (mode) {
case USB_POWER_CLIENT:
// B2,A13: set client mode
@@ -216,8 +219,15 @@ void set_usb_power_mode(int mode) {
set_gpio_output(GPIOB, 2, 0);
set_gpio_output(GPIOA, 13, 0);
break;
default:
valid_mode = false;
puts("Invalid usb power mode\n");
break;
}
if (valid_mode) {
usb_power_mode = mode;
}
usb_power_mode = mode;
}
#define ESP_DISABLED 0
@@ -240,13 +250,16 @@ void set_esp_mode(int mode) {
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 0);
break;
default:
puts("Invalid esp mode\n");
break;
}
}
// ********************* big init function *********************
// board specific
void gpio_init() {
void gpio_init(void) {
// pull low to hold ESP in reset??
// enable OTG out tied to ground
GPIOA->ODR = 0;
@@ -366,7 +379,7 @@ void gpio_init() {
extern void *g_pfnVectors;
extern uint32_t enter_bootloader_mode;
void jump_to_bootloader() {
void jump_to_bootloader(void) {
// do enter bootloader
enter_bootloader_mode = 0;
void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)0x1fff0004));
@@ -379,11 +392,11 @@ void jump_to_bootloader() {
NVIC_SystemReset();
}
void early() {
void early(void) {
// after it's been in the bootloader, things are initted differently, so we reset
if (enter_bootloader_mode != BOOT_NORMAL &&
enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC &&
enter_bootloader_mode != ENTER_SOFTLOADER_MAGIC) {
if ((enter_bootloader_mode != BOOT_NORMAL) &&
(enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC) &&
(enter_bootloader_mode != ENTER_SOFTLOADER_MAGIC)) {
enter_bootloader_mode = BOOT_NORMAL;
NVIC_SystemReset();
}
panda/board/libc.h
+20 -10
View File
@@ -1,12 +1,12 @@
// **** shitty libc ****
// **** libc ****
void delay(int a) {
volatile int i;
for (i=0;i<a;i++);
for (i = 0; i < a; i++);
}
void *memset(void *str, int c, unsigned int n) {
int i;
unsigned int i;
for (i = 0; i < n; i++) {
*((uint8_t*)str) = c;
++str;
@@ -15,7 +15,7 @@ void *memset(void *str, int c, unsigned int n) {
}
void *memcpy(void *dest, const void *src, unsigned int n) {
int i;
unsigned int i;
// TODO: make not slow
for (i = 0; i < n; i++) {
((uint8_t*)dest)[i] = *(uint8_t*)src;
@@ -25,26 +25,36 @@ void *memcpy(void *dest, const void *src, unsigned int n) {
}
int memcmp(const void * ptr1, const void * ptr2, unsigned int num) {
int i;
unsigned int i;
int ret = 0;
for (i = 0; i < num; i++) {
if ( ((uint8_t*)ptr1)[i] != ((uint8_t*)ptr2)[i] ) return -1;
if ( ((uint8_t*)ptr1)[i] != ((uint8_t*)ptr2)[i] ) {
ret = -1;
break;
}
}
return 0;
return ret;
}
// ********************* IRQ helpers *********************
int interrupts_enabled = 0;
void enable_interrupts(void) {
interrupts_enabled = 1;
__enable_irq();
}
int critical_depth = 0;
void enter_critical_section() {
void enter_critical_section(void) {
__disable_irq();
// this is safe because interrupts are disabled
critical_depth += 1;
}
void exit_critical_section() {
void exit_critical_section(void) {
// this is safe because interrupts are disabled
critical_depth -= 1;
if (critical_depth == 0) {
if ((critical_depth == 0) && interrupts_enabled) {
__enable_irq();
}
}
panda/board/main.c
+88 -55
View File
@@ -17,10 +17,13 @@
#include "drivers/adc.h"
#include "drivers/usb.h"
#include "drivers/gmlan_alt.h"
#include "drivers/spi.h"
#include "drivers/timer.h"
#include "drivers/clock.h"
#ifndef EON
#include "drivers/spi.h"
#endif
#include "power_saving.h"
#include "safety.h"
#include "drivers/can.h"
@@ -61,14 +64,14 @@ void debug_ring_callback(uart_ring *ring) {
// ***************************** started logic *****************************
int is_gpio_started() {
bool is_gpio_started(void) {
// ignition is on PA1
return (GPIOA->IDR & (1 << 1)) == 0;
return (GPIOA->IDR & (1U << 1)) == 0;
}
void EXTI1_IRQHandler() {
volatile int pr = EXTI->PR & (1 << 1);
if (pr & (1 << 1)) {
void EXTI1_IRQHandler(void) {
volatile int pr = EXTI->PR & (1U << 1);
if ((pr & (1U << 1)) != 0) {
#ifdef DEBUG
puts("got started interrupt\n");
#endif
@@ -77,20 +80,17 @@ void EXTI1_IRQHandler() {
delay(100000);
// set power savings mode here
if (is_gpio_started() == 1) {
power_save_disable();
} else {
power_save_enable();
}
EXTI->PR = (1 << 1);
int power_save_state = is_gpio_started() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
set_power_save_state(power_save_state);
EXTI->PR = (1U << 1);
}
}
void started_interrupt_init() {
void started_interrupt_init(void) {
SYSCFG->EXTICR[1] = SYSCFG_EXTICR1_EXTI1_PA;
EXTI->IMR |= (1 << 1);
EXTI->RTSR |= (1 << 1);
EXTI->FTSR |= (1 << 1);
EXTI->IMR |= (1U << 1);
EXTI->RTSR |= (1U << 1);
EXTI->FTSR |= (1U << 1);
NVIC_EnableIRQ(EXTI1_IRQn);
}
@@ -139,25 +139,34 @@ int get_health_pkt(void *dat) {
return sizeof(*health);
}
int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) {
int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata;
int ilen = 0;
while (ilen < min(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) ilen++;
while (ilen < MIN(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) {
ilen++;
}
return ilen*0x10;
}
// send on serial, first byte to select the ring
void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {
if (len == 0) return;
void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
uart_ring *ur = get_ring_by_number(usbdata[0]);
if (!ur) return;
if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) {
for (int i = 1; i < len; i++) while (!putc(ur, usbdata[i]));
if ((len != 0) && (ur != NULL)) {
if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) {
for (int i = 1; i < len; i++) {
while (!putc(ur, usbdata[i])) {
// wait
}
}
}
}
}
// send on CAN
void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {
void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
int dpkt = 0;
for (dpkt = 0; dpkt < len; dpkt += 0x10) {
uint32_t *tf = (uint32_t*)(&usbdata[dpkt]);
@@ -174,14 +183,14 @@ void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {
}
}
int is_enumerated = 0;
bool is_enumerated = 0;
void usb_cb_enumeration_complete() {
puts("USB enumeration complete\n");
is_enumerated = 1;
}
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {
int resp_len = 0;
uart_ring *ur = NULL;
int i;
@@ -227,6 +236,9 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
NVIC_SystemReset();
break;
default:
puts("Bootloader mode invalid\n");
break;
}
break;
// **** 0xd2: get health packet
@@ -235,7 +247,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
break;
// **** 0xd6: get version
case 0xd6:
COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN)
COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN);
memcpy(resp, gitversion, sizeof(gitversion));
resp_len = sizeof(gitversion)-1;
break;
@@ -288,7 +300,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
if (safety_ignition_hook() != -1) {
// if the ignition hook depends on something other than the started GPIO
// we have to disable power savings (fix for GM and Tesla)
power_save_disable();
set_power_save_state(POWER_SAVE_STATUS_DISABLED);
}
#ifndef EON
// always LIVE on EON
@@ -334,10 +346,14 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
// **** 0xe0: uart read
case 0xe0:
ur = get_ring_by_number(setup->b.wValue.w);
if (!ur) break;
if (ur == &esp_ring) uart_dma_drain();
if (!ur) {
break;
}
if (ur == &esp_ring) {
uart_dma_drain();
}
// read
while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) &&
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
getc(ur, (char*)&resp[resp_len])) {
++resp_len;
}
@@ -345,13 +361,17 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
// **** 0xe1: uart set baud rate
case 0xe1:
ur = get_ring_by_number(setup->b.wValue.w);
if (!ur) break;
if (!ur) {
break;
}
uart_set_baud(ur->uart, setup->b.wIndex.w);
break;
// **** 0xe2: uart set parity
case 0xe2:
ur = get_ring_by_number(setup->b.wValue.w);
if (!ur) break;
if (!ur) {
break;
}
switch (setup->b.wIndex.w) {
case 0:
// disable parity, 8-bit
@@ -374,7 +394,9 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
// **** 0xe4: uart set baud rate extended
case 0xe4:
ur = get_ring_by_number(setup->b.wValue.w);
if (!ur) break;
if (!ur) {
break;
}
uart_set_baud(ur->uart, (int)setup->b.wIndex.w*300);
break;
// **** 0xe5: set CAN loopback (for testing)
@@ -398,11 +420,11 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
// **** 0xf0: do k-line wValue pulse on uart2 for Acura
case 0xf0:
if (setup->b.wValue.w == 1) {
GPIOC->ODR &= ~(1 << 10);
GPIOC->ODR &= ~(1U << 10);
GPIOC->MODER &= ~GPIO_MODER_MODER10_1;
GPIOC->MODER |= GPIO_MODER_MODER10_0;
} else {
GPIOC->ODR &= ~(1 << 12);
GPIOC->ODR &= ~(1U << 12);
GPIOC->MODER &= ~GPIO_MODER_MODER12_1;
GPIOC->MODER |= GPIO_MODER_MODER12_0;
}
@@ -410,11 +432,11 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
for (i = 0; i < 80; i++) {
delay(8000);
if (setup->b.wValue.w == 1) {
GPIOC->ODR |= (1 << 10);
GPIOC->ODR &= ~(1 << 10);
GPIOC->ODR |= (1U << 10);
GPIOC->ODR &= ~(1U << 10);
} else {
GPIOC->ODR |= (1 << 12);
GPIOC->ODR &= ~(1 << 12);
GPIOC->ODR |= (1U << 12);
GPIOC->ODR &= ~(1U << 12);
}
}
@@ -442,7 +464,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
case 0xf2:
{
uart_ring * rb = get_ring_by_number(setup->b.wValue.w);
if (rb) {
if (rb != NULL) {
puts("Clearing UART queue.\n");
clear_uart_buff(rb);
}
@@ -461,7 +483,7 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
// data[0] = endpoint
// data[2] = length
// data[4:] = data
UNUSED(len);
int resp_len = 0;
switch (data[0]) {
case 0:
@@ -480,6 +502,9 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
// ep 3, send CAN
usb_cb_ep3_out(data+4, data[2], 0);
break;
default:
puts("SPI data invalid");
break;
}
return resp_len;
}
@@ -487,20 +512,20 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
// ***************************** main code *****************************
void __initialize_hardware_early() {
void __initialize_hardware_early(void) {
early();
}
void __attribute__ ((noinline)) enable_fpu() {
void __attribute__ ((noinline)) enable_fpu(void) {
// enable the FPU
SCB->CPACR |= ((3UL << (10 * 2)) | (3UL << (11 * 2)));
SCB->CPACR |= ((3UL << (10U * 2)) | (3UL << (11U * 2)));
}
uint64_t tcnt = 0;
uint64_t marker = 0;
// called once per second
void TIM3_IRQHandler() {
void TIM3_IRQHandler(void) {
#define CURRENT_THRESHOLD 0xF00
#define CLICKS 5 // 5 seconds to switch modes
@@ -559,6 +584,9 @@ void TIM3_IRQHandler() {
marker = tcnt;
}
break;
default:
puts("USB power mode invalid\n"); // set_usb_power_mode prevents assigning invalid values
break;
}
// ~0x9a = 500 ma
@@ -566,8 +594,9 @@ void TIM3_IRQHandler() {
puts("\n");*/
// reset this every 16th pass
if ((tcnt&0xF) == 0) pending_can_live = 0;
if ((tcnt&0xF) == 0) {
pending_can_live = 0;
}
#ifdef DEBUG
puts("** blink ");
puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" ");
@@ -580,7 +609,7 @@ void TIM3_IRQHandler() {
// turn off the blue LED, turned on by CAN
// unless we are in power saving mode
set_led(LED_BLUE, (tcnt&1) && power_save_status == POWER_SAVE_STATUS_ENABLED);
set_led(LED_BLUE, (tcnt & 1) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
// on to the next one
tcnt += 1;
@@ -588,7 +617,7 @@ void TIM3_IRQHandler() {
TIM3->SR = 0;
}
int main() {
int main(void) {
// shouldn't have interrupts here, but just in case
__disable_irq();
@@ -609,7 +638,9 @@ int main() {
puts(is_entering_bootmode ? " ESP wants bootmode\n" : " no bootmode\n");
// non rev c panda are no longer supported
while (revision != PANDA_REV_C);
while (revision != PANDA_REV_C) {
// hang
}
gpio_init();
@@ -659,7 +690,10 @@ int main() {
can_init_all();
adc_init();
#ifndef EON
spi_init();
#endif
#ifdef EON
// have to save power
@@ -667,8 +701,8 @@ int main() {
set_esp_mode(ESP_DISABLED);
}
// only enter power save after the first cycle
/*if (is_gpio_started() == 0) {
power_save_enable();
/*if (is_gpio_started()) {
set_power_save_state(POWER_SAVE_STATUS_ENABLED);
}*/
// interrupt on started line
started_interrupt_init();
@@ -683,8 +717,7 @@ int main() {
#endif
puts("**** INTERRUPTS ON ****\n");
__enable_irq();
enable_interrupts();
// LED should keep on blinking all the time
uint64_t cnt = 0;
panda/board/pedal/main.c
+8 -8
View File
@@ -19,7 +19,7 @@
#include "drivers/usb.h"
#else
// no serial either
int puts(const char *a) { return 0; }
void puts(const char *a) {}
void puth(unsigned int i) {}
#endif
@@ -41,12 +41,12 @@ void debug_ring_callback(uart_ring *ring) {
}
}
int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; }
void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {}
void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {}
int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) { return 0; }
void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {}
void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) {}
void usb_cb_enumeration_complete() {}
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {
int resp_len = 0;
uart_ring *ur = NULL;
switch (setup->b.bRequest) {
@@ -56,7 +56,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
if (!ur) break;
if (ur == &esp_ring) uart_dma_drain();
// read
while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) &&
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
getc(ur, (char*)&resp[resp_len])) {
++resp_len;
}
@@ -246,8 +246,8 @@ void pedal() {
// write the pedal to the DAC
if (state == NO_FAULT) {
dac_set(0, max(gas_set_0, pdl0));
dac_set(1, max(gas_set_1, pdl1));
dac_set(0, MAX(gas_set_0, pdl0));
dac_set(1, MAX(gas_set_1, pdl1));
} else {
dac_set(0, pdl0);
dac_set(1, pdl1);
panda/board/power_saving.h
+32 -46
View File
@@ -3,55 +3,41 @@
int power_save_status = POWER_SAVE_STATUS_DISABLED;
void power_save_enable(void) {
if (power_save_status == POWER_SAVE_STATUS_ENABLED) return;
puts("enable power savings\n");
void set_power_save_state(int state) {
// turn off can
set_can_enable(CAN1, 0);
set_can_enable(CAN2, 0);
set_can_enable(CAN3, 0);
bool is_valid_state = (state == POWER_SAVE_STATUS_ENABLED) || (state == POWER_SAVE_STATUS_DISABLED);
if (is_valid_state && (state != power_save_status)) {
bool enable = false;
if (state == POWER_SAVE_STATUS_ENABLED) {
puts("enable power savings\n");
if (is_grey_panda) {
char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
uart_ring *ur = get_ring_by_number(1);
for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG) - 1; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
}
} else {
puts("disable power savings\n");
if (is_grey_panda) {
char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
uart_ring *ur = get_ring_by_number(1);
for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG) - 1; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
}
enable = true;
}
// turn off GMLAN
set_gpio_output(GPIOB, 14, 0);
set_gpio_output(GPIOB, 15, 0);
// turn on can
set_can_enable(CAN1, enable);
set_can_enable(CAN2, enable);
set_can_enable(CAN3, enable);
// turn off LIN
set_gpio_output(GPIOB, 7, 0);
set_gpio_output(GPIOA, 14, 0);
// turn on GMLAN
set_gpio_output(GPIOB, 14, enable);
set_gpio_output(GPIOB, 15, enable);
if (is_grey_panda) {
char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
uart_ring *ur = get_ring_by_number(1);
for (int i = 0; i < sizeof(UBLOX_SLEEP_MSG)-1; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
// turn on LIN
set_gpio_output(GPIOB, 7, enable);
set_gpio_output(GPIOA, 14, enable);
power_save_status = state;
}
power_save_status = POWER_SAVE_STATUS_ENABLED;
}
void power_save_disable(void) {
if (power_save_status == POWER_SAVE_STATUS_DISABLED) return;
puts("disable power savings\n");
// turn on can
set_can_enable(CAN1, 1);
set_can_enable(CAN2, 1);
set_can_enable(CAN3, 1);
// turn on GMLAN
set_gpio_output(GPIOB, 14, 1);
set_gpio_output(GPIOB, 15, 1);
// turn on LIN
set_gpio_output(GPIOB, 7, 1);
set_gpio_output(GPIOA, 14, 1);
if (is_grey_panda) {
char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
uart_ring *ur = get_ring_by_number(1);
for (int i = 0; i < sizeof(UBLOX_WAKE_MSG)-1; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
}
power_save_status = POWER_SAVE_STATUS_DISABLED;
}
panda/board/safety.h
+60 -95
View File
@@ -1,56 +1,5 @@
// sample struct that keeps 3 samples in memory
struct sample_t {
int values[6];
int min;
int max;
} sample_t_default = {{0}, 0, 0};
// safety code requires floats
struct lookup_t {
float x[3];
float y[3];
};
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
int safety_ignition_hook();
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
int to_signed(int d, int bits);
void update_sample(struct sample_t *sample, int sample_new);
int max_limit_check(int val, const int MAX, const int MIN);
int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
float interpolate(struct lookup_t xy, float x);
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send);
typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len);
typedef int (*ign_hook)();
typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
typedef struct {
safety_hook_init init;
ign_hook ignition;
rx_hook rx;
tx_hook tx;
tx_lin_hook tx_lin;
fwd_hook fwd;
} safety_hooks;
// This can be set by the safety hooks.
int controls_allowed = 0;
int gas_interceptor_detected = 0;
int gas_interceptor_prev = 0;
// This is set by USB command 0xdf
int long_controls_allowed = 1;
// include first, needed by safety policies
#include "safety_declarations.h"
// Include the actual safety policies.
#include "safety/safety_defaults.h"
#include "safety/safety_honda.h"
@@ -129,83 +78,94 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_ELM327, &elm327_hooks},
};
#define HOOK_CONFIG_COUNT (sizeof(safety_hook_registry)/sizeof(safety_hook_config))
int safety_set_mode(uint16_t mode, int16_t param) {
for (int i = 0; i < HOOK_CONFIG_COUNT; i++) {
int set_status = -1; // not set
int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
for (int i = 0; i < hook_config_count; i++) {
if (safety_hook_registry[i].id == mode) {
current_hooks = safety_hook_registry[i].hooks;
if (current_hooks->init) current_hooks->init(param);
return 0;
set_status = 0; // set
break;
}
}
return -1;
if ((set_status == 0) && (current_hooks->init != NULL)) {
current_hooks->init(param);
}
return set_status;
}
// compute the time elapsed (in microseconds) from 2 counter samples
// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts;
return ts - ts_last;
}
// convert a trimmed integer to signed 32 bit int
int to_signed(int d, int bits) {
if (d >= (1 << (bits - 1))) {
d -= (1 << bits);
int d_signed = d;
if (d >= (1 << MAX((bits - 1), 0))) {
d_signed = d - (1 << MAX(bits, 0));
}
return d;
return d_signed;
}
// given a new sample, update the smaple_t struct
void update_sample(struct sample_t *sample, int sample_new) {
for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) {
int sample_size = sizeof(sample->values) / sizeof(sample->values[0]);
for (int i = sample_size - 1; i > 0; i--) {
sample->values[i] = sample->values[i-1];
}
sample->values[0] = sample_new;
// get the minimum and maximum measured samples
sample->min = sample->max = sample->values[0];
for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) {
if (sample->values[i] < sample->min) sample->min = sample->values[i];
if (sample->values[i] > sample->max) sample->max = sample->values[i];
sample->min = sample->values[0];
sample->max = sample->values[0];
for (int i = 1; i < sample_size; i++) {
if (sample->values[i] < sample->min) {
sample->min = sample->values[i];
}
if (sample->values[i] > sample->max) {
sample->max = sample->values[i];
}
}
}
int max_limit_check(int val, const int MAX, const int MIN) {
return (val > MAX) || (val < MIN);
bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
return (val > MAX_VAL) || (val < MIN_VAL);
}
// check that commanded value isn't too far from measured
int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
// *** val rate limit check ***
int highest_allowed_val = max(val_last, 0) + MAX_RATE_UP;
int lowest_allowed_val = min(val_last, 0) - MAX_RATE_UP;
int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
// if we've exceeded the meas val, we must start moving toward 0
highest_allowed_val = min(highest_allowed_val, max(val_last - MAX_RATE_DOWN, max(val_meas->max, 0) + MAX_ERROR));
lowest_allowed_val = max(lowest_allowed_val, min(val_last + MAX_RATE_DOWN, min(val_meas->min, 0) - MAX_ERROR));
int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, MAX(val_meas->max, 0) + MAX_ERROR));
int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, MIN(val_meas->min, 0) - MAX_ERROR));
// check for violation
return (val < lowest_allowed_val) || (val > highest_allowed_val);
return (val < lowest_allowed) || (val > highest_allowed);
}
// check that commanded value isn't fighting against driver
int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX_VAL, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {
int highest_allowed = max(val_last, 0) + MAX_RATE_UP;
int lowest_allowed = min(val_last, 0) - MAX_RATE_UP;
int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
int driver_max_limit = MAX + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
int driver_min_limit = -MAX + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
int driver_min_limit = -MAX_VAL + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
// if we've exceeded the applied torque, we must start moving toward 0
highest_allowed = min(highest_allowed, max(val_last - MAX_RATE_DOWN,
max(driver_max_limit, 0)));
lowest_allowed = max(lowest_allowed, min(val_last + MAX_RATE_DOWN,
min(driver_min_limit, 0)));
int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN,
MAX(driver_max_limit, 0)));
int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN,
MIN(driver_min_limit, 0)));
// check for violation
return (val < lowest_allowed) || (val > highest_allowed);
@@ -213,11 +173,11 @@ int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
// real time check, mainly used for steer torque rate limiter
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
// *** torque real time rate limit check ***
int highest_val = max(val_last, 0) + MAX_RT_DELTA;
int lowest_val = min(val_last, 0) - MAX_RT_DELTA;
int highest_val = MAX(val_last, 0) + MAX_RT_DELTA;
int lowest_val = MIN(val_last, 0) - MAX_RT_DELTA;
// check for violation
return (val < lowest_val) || (val > highest_val);
@@ -226,25 +186,30 @@ int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
// interp function that holds extreme values
float interpolate(struct lookup_t xy, float x) {
int size = sizeof(xy.x) / sizeof(xy.x[0]);
float ret = xy.y[size - 1]; // default output is last point
// x is lower than the first point in the x array. Return the first point
if (x <= xy.x[0]) {
return xy.y[0];
ret = xy.y[0];
} else {
// find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
for (int i=0; i < size-1; i++) {
for (int i=0; i < (size - 1); i++) {
if (x < xy.x[i+1]) {
float x0 = xy.x[i];
float y0 = xy.y[i];
float dx = xy.x[i+1] - x0;
float dy = xy.y[i+1] - y0;
// dx should not be zero as xy.x is supposed ot be monotonic
if (dx <= 0.) dx = 0.0001;
return dy * (x - x0) / dx + y0;
if (dx <= 0.) {
dx = 0.0001;
}
ret = (dy * (x - x0) / dx) + y0;
break;
}
}
// if no such point is found, then x > xy.x[size-1]. Return last point
return xy.y[size - 1];
}
return ret;
}
panda/board/safety/safety_cadillac.h
+22 -19
View File
@@ -1,8 +1,10 @@
#define CADILLAC_TORQUE_MSG_N 4 // 4 torque messages: 0x151, 0x152, 0x153, 0x154
const int CADILLAC_MAX_STEER = 150; // 1s
// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int CADILLAC_MAX_RT_DELTA = 75; // max delta torque allowed for real time checks
const int32_t CADILLAC_RT_INTERVAL = 250000; // 250ms between real time checks
const uint32_t CADILLAC_RT_INTERVAL = 250000; // 250ms between real time checks
const int CADILLAC_MAX_RATE_UP = 2;
const int CADILLAC_MAX_RATE_DOWN = 5;
const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50;
@@ -11,21 +13,19 @@ const int CADILLAC_DRIVER_TORQUE_FACTOR = 4;
int cadillac_ign = 0;
int cadillac_cruise_engaged_last = 0;
int cadillac_rt_torque_last = 0;
int cadillac_desired_torque_last[4] = {0}; // 4 torque messages
const int cadillac_torque_msgs_n = 4;
int cadillac_desired_torque_last[CADILLAC_TORQUE_MSG_N] = {0};
uint32_t cadillac_ts_last = 0;
int cadillac_supercruise_on = 0;
struct sample_t cadillac_torque_driver; // last few driver torques measured
int cadillac_get_torque_idx(uint32_t addr) {
if (addr==0x151) return 0;
else if (addr==0x152) return 1;
else if (addr==0x153) return 2;
else return 3;
int cadillac_get_torque_idx(int addr, int array_size) {
return MIN(MAX(addr - 0x151, 0), array_size); // 0x151 is id 0, 0x152 is id 1 and so on...
}
static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr = to_push->RIR >> 21;
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (addr == 356) {
int torque_driver_new = ((to_push->RDLR & 0x7) << 8) | ((to_push->RDLR >> 8) & 0xFF);
@@ -35,16 +35,17 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// this message isn't all zeros when ignition is on
if (addr == 0x160 && bus_number == 0) {
if ((addr == 0x160) && (bus == 0)) {
cadillac_ign = to_push->RDLR > 0;
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x370) && (bus_number == 0)) {
if ((addr == 0x370) && (bus == 0)) {
int cruise_engaged = to_push->RDLR & 0x800000; // bit 23
if (cruise_engaged && !cadillac_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
}
if (!cruise_engaged) {
controls_allowed = 0;
}
cadillac_cruise_engaged_last = cruise_engaged;
@@ -57,14 +58,15 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t addr = to_send->RIR >> 21;
int tx = 1;
int addr = GET_ADDR(to_send);
// steer cmd checks
if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) {
int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
int violation = 0;
uint32_t ts = TIM2->CNT;
int idx = cadillac_get_torque_idx(addr);
int idx = cadillac_get_torque_idx(addr, CADILLAC_TORQUE_MSG_N);
desired_torque = to_signed(desired_torque, 14);
if (controls_allowed) {
@@ -105,19 +107,20 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation || cadillac_supercruise_on) {
return false;
tx = 0;
}
}
return true;
return tx;
}
static void cadillac_init(int16_t param) {
UNUSED(param);
controls_allowed = 0;
cadillac_ign = 0;
}
static int cadillac_ign_hook() {
static int cadillac_ign_hook(void) {
return cadillac_ign;
}
@@ -127,5 +130,5 @@ const safety_hooks cadillac_hooks = {
.tx = cadillac_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.ignition = cadillac_ign_hook,
.fwd = alloutput_fwd_hook,
.fwd = default_fwd_hook,
};
panda/board/safety/safety_chrysler.h
+30 -40
View File
@@ -1,11 +1,11 @@
const int CHRYSLER_MAX_STEER = 261;
const int CHRYSLER_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
const int32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks
const uint32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks
const int CHRYSLER_MAX_RATE_UP = 3;
const int CHRYSLER_MAX_RATE_DOWN = 3;
const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor
int chrysler_camera_detected = 0; // is giraffe switch 2 high?
bool chrysler_camera_detected = 0; // is giraffe switch 2 high?
int chrysler_rt_torque_last = 0;
int chrysler_desired_torque_last = 0;
int chrysler_cruise_engaged_last = 0;
@@ -13,40 +13,32 @@ uint32_t chrysler_ts_last = 0;
struct sample_t chrysler_torque_meas; // last few torques measured
static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if (to_push->RIR & 4) {
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
} else {
// Normal
addr = to_push->RIR >> 21;
}
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
// Measured eps torque
if (addr == 544) {
int rdhr = to_push->RDHR;
int torque_meas_new = ((rdhr & 0x7) << 8) + ((rdhr & 0xFF00) >> 8) - 1024;
uint32_t rdhr = to_push->RDHR;
int torque_meas_new = ((rdhr & 0x7U) << 8) + ((rdhr & 0xFF00U) >> 8) - 1024U;
// update array of samples
update_sample(&chrysler_torque_meas, torque_meas_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x1f4) {
if (addr == 0x1F4) {
int cruise_engaged = ((to_push->RDLR & 0x380000) >> 19) == 7;
if (cruise_engaged && !chrysler_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
}
if (!cruise_engaged) {
controls_allowed = 0;
}
chrysler_cruise_engaged_last = cruise_engaged;
}
// check if stock camera ECU is still online
if (bus == 0 && addr == 0x292) {
if ((bus == 0) && (addr == 0x292)) {
chrysler_camera_detected = 1;
controls_allowed = 0;
}
@@ -54,27 +46,21 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// There can be only one! (camera)
int tx = 1;
// If camera is on bus 0, then nothing can be sent
if (chrysler_camera_detected) {
return 0;
}
uint32_t addr;
if (to_send->RIR & 4) {
// Extended
addr = to_send->RIR >> 3;
} else {
// Normal
addr = to_send->RIR >> 21;
tx = 0;
}
int addr = GET_ADDR(to_send);
// LKA STEER
if (addr == 0x292) {
int rdlr = to_send->RDLR;
int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8) - 1024;
uint32_t rdlr = to_send->RDLR;
int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8) - 1024U;
uint32_t ts = TIM2->CNT;
int violation = 0;
bool violation = 0;
if (controls_allowed) {
@@ -112,7 +98,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
@@ -122,24 +108,28 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// TODO: fix bug preventing the button msg to be fwd'd on bus 2
// 1 allows the message through
return true;
return tx;
}
static void chrysler_init(int16_t param) {
UNUSED(param);
controls_allowed = 0;
chrysler_camera_detected = 0;
}
static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int32_t addr = to_fwd->RIR >> 21;
int bus_fwd = -1;
int addr = GET_ADDR(to_fwd);
// forward CAN 0 -> 2 so stock LKAS camera sees messages
if (bus_num == 0 && !chrysler_camera_detected) {
return 2;
if ((bus_num == 0) && !chrysler_camera_detected) {
bus_fwd = 2;
}
// forward all messages from camera except LKAS_COMMAND and LKAS_HUD
if (bus_num == 2 && !chrysler_camera_detected && addr != 658 && addr != 678) {
return 0;
if ((bus_num == 2) && !chrysler_camera_detected && (addr != 658) && (addr != 678)) {
bus_fwd = 0;
}
return -1; // do not forward
return bus_fwd;
}
panda/board/safety/safety_defaults.h
+19 -9
View File
@@ -1,24 +1,33 @@
void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {}
void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
UNUSED(to_push);
}
int default_ign_hook() {
int default_ign_hook(void) {
return -1; // use GPIO to determine ignition
}
// *** no output safety mode ***
static void nooutput_init(int16_t param) {
UNUSED(param);
controls_allowed = 0;
}
static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
UNUSED(to_send);
return false;
}
static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
UNUSED(lin_num);
UNUSED(data);
UNUSED(len);
return false;
}
static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
static int default_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
UNUSED(bus_num);
UNUSED(to_fwd);
return -1;
}
@@ -28,32 +37,33 @@ const safety_hooks nooutput_hooks = {
.tx = nooutput_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.ignition = default_ign_hook,
.fwd = nooutput_fwd_hook,
.fwd = default_fwd_hook,
};
// *** all output safety mode ***
static void alloutput_init(int16_t param) {
UNUSED(param);
controls_allowed = 1;
}
static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
UNUSED(to_send);
return true;
}
static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
UNUSED(lin_num);
UNUSED(data);
UNUSED(len);
return true;
}
static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return -1;
}
const safety_hooks alloutput_hooks = {
.init = alloutput_init,
.rx = default_rx_hook,
.tx = alloutput_tx_hook,
.tx_lin = alloutput_tx_lin_hook,
.ignition = default_ign_hook,
.fwd = alloutput_fwd_hook,
.fwd = default_fwd_hook,
};
panda/board/safety/safety_elm327.h
+33 -19
View File
@@ -1,28 +1,42 @@
static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
//All ELM traffic must appear on CAN0
if(((to_send->RDTR >> 4) & 0xf) != 0) return 0;
//All ISO 15765-4 messages must be 8 bytes long
if((to_send->RDTR & 0xf) != 8) return 0;
if(to_send->RIR & 4){
uint32_t addr = to_send->RIR >> 3;
//Check valid 29 bit send addresses for ISO 15765-4
if(!(addr == 0x18DB33F1 || (addr & 0x1FFF00FF) == 0x18DA00F1)) return 0;
} else {
uint32_t addr = to_send->RIR >> 21;
//Check valid 11 bit send addresses for ISO 15765-4
if(!(addr == 0x7DF || (addr & 0x7F8) == 0x7E0)) return 0;
int tx = 1;
int bus = GET_BUS(to_send);
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
//All ELM traffic must appear on CAN0
if (bus != 0) {
tx = 0;
}
return true;
//All ISO 15765-4 messages must be 8 bytes long
if (len != 8) {
tx = 0;
}
//Check valid 29 bit send addresses for ISO 15765-4
//Check valid 11 bit send addresses for ISO 15765-4
if ((addr != 0x18DB33F1) && ((addr & 0x1FFF00FF) != 0x18DA00F1) &&
((addr != 0x7DF) && ((addr & 0x7F8) != 0x7E0))) {
tx = 0;
}
return tx;
}
static int elm327_tx_lin_hook(int lin_num, uint8_t *data, int len) {
if(lin_num != 0) return false; //Only operate on LIN 0, aka serial 2
if(len < 5 || len > 11) return false; //Valid KWP size
if(!((data[0] & 0xF8) == 0xC0 && (data[0] & 0x07) > 0 &&
data[1] == 0x33 && data[2] == 0xF1)) return false; //Bad msg
return true;
int tx = 1;
if (lin_num != 0) {
tx = 0; //Only operate on LIN 0, aka serial 2
}
if ((len < 5) || (len > 11)) {
tx = 0; //Valid KWP size
}
if (!(((data[0] & 0xF8U) == 0xC0U) && ((data[0] & 0x07U) != 0U) &&
(data[1] == 0x33U) && (data[2] == 0xF1U))) {
tx = 0; //Bad msg
}
return tx;
}
const safety_hooks elm327_hooks = {
@@ -31,5 +45,5 @@ const safety_hooks elm327_hooks = {
.tx = elm327_tx_hook,
.tx_lin = elm327_tx_lin_hook,
.ignition = default_ign_hook,
.fwd = nooutput_fwd_hook,
.fwd = default_fwd_hook,
};
panda/board/safety/safety_ford.h
+24 -17
View File
@@ -13,26 +13,29 @@ int ford_is_moving = 0;
static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if ((to_push->RIR>>21) == 0x217) {
int addr = GET_ADDR(to_push);
if (addr == 0x217) {
// wheel speeds are 14 bits every 16
ford_is_moving = 0xFCFF & (to_push->RDLR | (to_push->RDLR >> 16) |
to_push->RDHR | (to_push->RDHR >> 16));
}
// state machine to enter and exit controls
if ((to_push->RIR>>21) == 0x83) {
int cancel = ((to_push->RDLR >> 8) & 0x1);
int set_or_resume = (to_push->RDLR >> 28) & 0x3;
if (addr == 0x83) {
bool cancel = (to_push->RDLR >> 8) & 0x1;
bool set_or_resume = (to_push->RDLR >> 28) & 0x3;
if (cancel) {
controls_allowed = 0;
} else if (set_or_resume) {
}
if (set_or_resume) {
controls_allowed = 1;
}
}
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if ((to_push->RIR>>21) == 0x165) {
if (addr == 0x165) {
int brake = to_push->RDLR & 0x20;
if (brake && (!(ford_brake_prev) || ford_is_moving)) {
controls_allowed = 0;
@@ -41,7 +44,7 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press
if ((to_push->RIR>>21) == 0x204) {
if (addr == 0x204) {
int gas = to_push->RDLR & 0xFF03;
if (gas && !(ford_gas_prev)) {
controls_allowed = 0;
@@ -58,29 +61,33 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_is_moving);
int current_controls_allowed = controls_allowed && !(pedal_pressed);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
int addr = GET_ADDR(to_send);
// STEER: safety check
if ((to_send->RIR>>21) == 0x3CA) {
if (current_controls_allowed) {
// all messages are fine here
} else {
if (addr == 0x3CA) {
if (!current_controls_allowed) {
// bits 7-4 need to be 0xF to disallow lkas commands
if (((to_send->RDLR >> 4) & 0xF) != 0xF) return 0;
if (((to_send->RDLR >> 4) & 0xF) != 0xF) {
tx = 0;
}
}
}
// FORCE CANCEL: safety check only relevant when spamming the cancel button
// ensuring that set and resume aren't sent
if ((to_send->RIR>>21) == 0x83) {
if ((to_send->RDLR >> 28) & 0x3) return 0;
if (addr == 0x83) {
if (((to_send->RDLR >> 28) & 0x3) != 0) {
tx = 0;
}
}
// 1 allows the message through
return true;
return tx;
}
const safety_hooks ford_hooks = {
@@ -89,5 +96,5 @@ const safety_hooks ford_hooks = {
.tx = ford_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.ignition = default_ign_hook,
.fwd = nooutput_fwd_hook,
.fwd = default_fwd_hook,
};
panda/board/safety/safety_gm.h
+58 -58
View File
@@ -10,7 +10,7 @@
const int GM_MAX_STEER = 300;
const int GM_MAX_RT_DELTA = 128; // max delta torque allowed for real time checks
const int32_t GM_RT_INTERVAL = 250000; // 250ms between real time checks
const uint32_t GM_RT_INTERVAL = 250000; // 250ms between real time checks
const int GM_MAX_RATE_UP = 7;
const int GM_MAX_RATE_DOWN = 17;
const int GM_DRIVER_TORQUE_ALLOWANCE = 50;
@@ -23,25 +23,16 @@ int gm_brake_prev = 0;
int gm_gas_prev = 0;
int gm_speed = 0;
// silence everything if stock car control ECUs are still online
int gm_ascm_detected = 0;
int gm_ignition_started = 0;
bool gm_ascm_detected = 0;
bool gm_ignition_started = 0;
int gm_rt_torque_last = 0;
int gm_desired_torque_last = 0;
uint32_t gm_ts_last = 0;
struct sample_t gm_torque_driver; // last few driver torques measured
static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if (to_push->RIR & 4) {
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
} else {
// Normal
addr = to_push->RIR >> 21;
}
int bus_number = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (addr == 388) {
int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF);
@@ -50,11 +41,11 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
update_sample(&gm_torque_driver, torque_driver_new);
}
if (addr == 0x1f1 && bus_number == 0) {
if ((addr == 0x1F1) && (bus_number == 0)) {
//Bit 5 should be ignition "on"
//Backup plan is Bit 2 (accessory power)
uint32_t ign = (to_push->RDLR) & 0x20;
gm_ignition_started = ign > 0;
bool ign = ((to_push->RDLR) & 0x20) != 0;
gm_ignition_started = ign;
}
// sample speed, really only care if car is moving or not
@@ -67,19 +58,24 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// on powertrain bus.
// 384 = ASCMLKASteeringCmd
// 715 = ASCMGasRegenCmd
if (bus_number == 0 && (addr == 384 || addr == 715)) {
if ((bus_number == 0) && ((addr == 384) || (addr == 715))) {
gm_ascm_detected = 1;
controls_allowed = 0;
}
// ACC steering wheel buttons
if (addr == 481) {
int buttons = (to_push->RDHR >> 12) & 0x7;
// res/set - enable, cancel button - disable
if (buttons == 2 || buttons == 3) {
controls_allowed = 1;
} else if (buttons == 6) {
controls_allowed = 0;
int button = (to_push->RDHR >> 12) & 0x7;
switch (button) {
case 2: // resume
case 3: // set
controls_allowed = 1;
break;
case 6: // cancel
controls_allowed = 0;
break;
default:
break; // any other button is irrelevant
}
}
@@ -109,7 +105,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on regen paddle
if (addr == 189) {
int regen = to_push->RDLR & 0x20;
bool regen = to_push->RDLR & 0x20;
if (regen) {
controls_allowed = 0;
}
@@ -124,43 +120,41 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
// There can be only one! (ASCM)
if (gm_ascm_detected) {
return 0;
tx = 0;
}
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_speed);
int current_controls_allowed = controls_allowed && !pedal_pressed;
bool current_controls_allowed = controls_allowed && !pedal_pressed;
uint32_t addr;
if (to_send->RIR & 4) {
// Extended
addr = to_send->RIR >> 3;
} else {
// Normal
addr = to_send->RIR >> 21;
}
int addr = GET_ADDR(to_send);
// BRAKE: safety check
if (addr == 789) {
int rdlr = to_send->RDLR;
int brake = ((rdlr & 0xF) << 8) + ((rdlr & 0xFF00) >> 8);
uint32_t rdlr = to_send->RDLR;
int brake = ((rdlr & 0xFU) << 8) + ((rdlr & 0xFF00U) >> 8);
brake = (0x1000 - brake) & 0xFFF;
if (current_controls_allowed && long_controls_allowed) {
if (brake > GM_MAX_BRAKE) return 0;
} else {
if (brake != 0) return 0;
if (!current_controls_allowed || !long_controls_allowed) {
if (brake != 0) {
tx = 0;
}
}
if (brake > GM_MAX_BRAKE) {
tx = 0;
}
}
// LKA STEER: safety check
if (addr == 384) {
int rdlr = to_send->RDLR;
int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8);
uint32_t rdlr = to_send->RDLR;
int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8);
uint32_t ts = TIM2->CNT;
int violation = 0;
bool violation = 0;
desired_torque = to_signed(desired_torque, 11);
if (current_controls_allowed) {
@@ -200,37 +194,43 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
// PARK ASSIST STEER: unlimited torque, no thanks
if (addr == 823) return 0;
if (addr == 823) {
tx = 0;
}
// GAS/REGEN: safety check
if (addr == 715) {
int rdlr = to_send->RDLR;
int gas_regen = ((rdlr & 0x7F0000) >> 11) + ((rdlr & 0xF8000000) >> 27);
int apply = rdlr & 1;
if (current_controls_allowed && long_controls_allowed) {
if (gas_regen > GM_MAX_GAS) return 0;
} else {
// Disabled message is !engaed with gas
// value that corresponds to max regen.
if (apply || gas_regen != GM_MAX_REGEN) return 0;
uint32_t rdlr = to_send->RDLR;
int gas_regen = ((rdlr & 0x7F0000U) >> 11) + ((rdlr & 0xF8000000U) >> 27);
// Disabled message is !engaed with gas
// value that corresponds to max regen.
if (!current_controls_allowed || !long_controls_allowed) {
bool apply = (rdlr & 1U) != 0U;
if (apply || (gas_regen != GM_MAX_REGEN)) {
tx = 0;
}
}
if (gas_regen > GM_MAX_GAS) {
tx = 0;
}
}
// 1 allows the message through
return true;
return tx;
}
static void gm_init(int16_t param) {
UNUSED(param);
controls_allowed = 0;
gm_ignition_started = 0;
}
static int gm_ign_hook() {
static int gm_ign_hook(void) {
return gm_ignition_started;
}
@@ -240,5 +240,5 @@ const safety_hooks gm_hooks = {
.tx = gm_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.ignition = gm_ign_hook,
.fwd = nooutput_fwd_hook,
.fwd = default_fwd_hook,
};
panda/board/safety/safety_gm_ascm.h
+16 -23
View File
@@ -3,42 +3,35 @@
static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
uint32_t addr = to_fwd->RIR>>21;
int bus_fwd = -1;
if (bus_num == 0) {
// do not propagate lkas messages from ascm to actuators
int addr = GET_ADDR(to_fwd);
bus_fwd = 2;
// do not propagate lkas messages from ascm to actuators, unless supercruise is on
// block 0x152 and 0x154, which are the lkas command from ASCM1 and ASCM2
// block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1
//if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) {
if ((addr == 0x152) || (addr == 0x154)) {
int supercruise_on = (to_fwd->RDHR>>4) & 0x1; // bit 36
if (!supercruise_on) return -1;
int supercruise_on = (to_fwd->RDHR >> 4) & 0x1; // bit 36
if (!supercruise_on) {
bus_fwd = -1;
}
}
// on the chassis bus, the OBDII port is on the module side, so we need to read
// the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to
// the actuator as 0x152 and 0x154
if (addr == 0x151) {
to_fwd->RIR = (0x152 << 21) | (to_fwd->RIR & 0x1fffff);
if ((addr == 0x151) || (addr == 0x153) || (addr == 0x314)) {
// on the chassis bus, the OBDII port is on the module side, so we need to read
// the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to
// the actuator as 0x152 and 0x154
uint32_t fwd_addr = addr + 1;
to_fwd->RIR = (fwd_addr << 21) | (to_fwd->RIR & 0x1fffff);
}
if (addr == 0x153) {
to_fwd->RIR = (0x154 << 21) | (to_fwd->RIR & 0x1fffff);
}
// brake
if (addr == 0x314) {
to_fwd->RIR = (0x315 << 21) | (to_fwd->RIR & 0x1fffff);
}
return 2;
}
if (bus_num == 2) {
return 0;
bus_fwd = 0;
}
return -1;
return bus_fwd;
}
const safety_hooks gm_ascm_hooks = {
panda/board/safety/safety_honda.h
+79 -48
View File
@@ -16,20 +16,29 @@ bool honda_alt_brake_msg = false;
static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
// sample speed
if ((to_push->RIR>>21) == 0x158) {
if (addr == 0x158) {
// first 2 bytes
honda_ego_speed = to_push->RDLR & 0xFFFF;
}
// state machine to enter and exit controls
// 0x1A6 for the ILX, 0x296 for the Civic Touring
if ((to_push->RIR>>21) == 0x1A6 || (to_push->RIR>>21) == 0x296) {
int buttons = (to_push->RDLR & 0xE0) >> 5;
if (buttons == 4 || buttons == 3) {
controls_allowed = 1;
} else if (buttons == 2) {
controls_allowed = 0;
if ((addr == 0x1A6) || (addr == 0x296)) {
int button = (to_push->RDLR & 0xE0) >> 5;
switch (button) {
case 2: // cancel
controls_allowed = 0;
break;
case 3: // set
case 4: // resume
controls_allowed = 1;
break;
default:
break; // any other button is irrelevant
}
}
@@ -39,11 +48,12 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// in these cases, this is used instead.
// most hondas: 0x17C bit 53
// accord, crv: 0x1BE bit 4
#define IS_USER_BRAKE_MSG(to_push) (!honda_alt_brake_msg ? to_push->RIR>>21 == 0x17C : to_push->RIR>>21 == 0x1BE)
#define USER_BRAKE_VALUE(to_push) (!honda_alt_brake_msg ? to_push->RDHR & 0x200000 : to_push->RDLR & 0x10)
#define IS_USER_BRAKE_MSG(addr) (!honda_alt_brake_msg ? ((addr) == 0x17C) : ((addr) == 0x1BE))
#define USER_BRAKE_VALUE(to_push) (!honda_alt_brake_msg ? ((to_push)->RDHR & 0x200000) : ((to_push)->RDLR & 0x10))
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (IS_USER_BRAKE_MSG(to_push)) {
bool is_user_brake_msg = IS_USER_BRAKE_MSG(addr); // needed to enforce type
if (is_user_brake_msg) {
int brake = USER_BRAKE_VALUE(to_push);
if (brake && (!(honda_brake_prev) || honda_ego_speed)) {
controls_allowed = 0;
@@ -53,7 +63,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
if ((to_push->RIR>>21) == 0x201 && (to_push->RDTR & 0xf) == 6) {
if ((addr == 0x201) && (len == 6)) {
gas_interceptor_detected = 1;
int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
@@ -66,7 +76,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press if no interceptor
if (!gas_interceptor_detected) {
if ((to_push->RIR>>21) == 0x17C) {
if (addr == 0x17C) {
int gas = to_push->RDLR & 0xFF;
if (gas && !(honda_gas_prev) && long_controls_allowed) {
controls_allowed = 0;
@@ -84,52 +94,62 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
(honda_brake_prev && honda_ego_speed);
int current_controls_allowed = controls_allowed && !(pedal_pressed);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
// BRAKE: safety check
if ((to_send->RIR>>21) == 0x1FA) {
if (current_controls_allowed && long_controls_allowed) {
if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) return 0;
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if (addr == 0x1FA) {
if (!current_controls_allowed || !long_controls_allowed) {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) {
tx = 0;
}
}
// STEER: safety check
if ((to_send->RIR>>21) == 0xE4 || (to_send->RIR>>21) == 0x194) {
if (current_controls_allowed) {
// all messages are fine here
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if ((addr == 0xE4) || (addr == 0x194)) {
if (!current_controls_allowed) {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
}
// GAS: safety check
if ((to_send->RIR>>21) == 0x200) {
if (current_controls_allowed && long_controls_allowed) {
// all messages are fine here
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if (addr == 0x200) {
if (!current_controls_allowed || !long_controls_allowed) {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
}
// FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
// This avoids unintended engagements while still allowing resume spam
if (((to_send->RIR>>21) == 0x296) && honda_bosch_hardware &&
!current_controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
if (((to_send->RDLR >> 5) & 0x7) != 2) return 0;
if ((addr == 0x296) && honda_bosch_hardware &&
!current_controls_allowed && (bus == 0)) {
if (((to_send->RDLR >> 5) & 0x7) != 2) {
tx = 0;
}
}
// 1 allows the message through
return true;
return tx;
}
static void honda_init(int16_t param) {
UNUSED(param);
controls_allowed = 0;
honda_bosch_hardware = false;
honda_alt_brake_msg = false;
@@ -139,7 +159,7 @@ static void honda_bosch_init(int16_t param) {
controls_allowed = 0;
honda_bosch_hardware = true;
// Checking for alternate brake override from safety parameter
honda_alt_brake_msg = param == 1 ? true : false;
honda_alt_brake_msg = (param == 1) ? true : false;
}
static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
@@ -147,27 +167,38 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX,
// 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud,
// 0x39f is radar hud
int addr = to_fwd->RIR>>21;
int bus_fwd = -1;
if (bus_num == 0) {
return 2;
} else if (bus_num == 2) {
// block stock lkas messages and stock acc messages (if OP is doing ACC)
int is_lkas_msg = (addr == 0xE4 || addr == 0x194 || addr == 0x33D);
int is_acc_msg = (addr == 0x1FA || addr == 0x30C || addr == 0x39F);
if (is_lkas_msg || (is_acc_msg && long_controls_allowed)) {
return -1;
}
return 0;
bus_fwd = 2;
}
return -1;
if (bus_num == 2) {
// block stock lkas messages and stock acc messages (if OP is doing ACC)
int addr = GET_ADDR(to_fwd);
int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F);
int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed);
if (!block_fwd) {
bus_fwd = 0;
}
}
return bus_fwd;
}
static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
if (bus_num == 1 || bus_num == 2) {
int addr = to_fwd->RIR>>21;
return addr != 0xE4 && addr != 0x33D ? (uint8_t)(~bus_num & 0x3) : -1;
int bus_fwd = -1;
if (bus_num == 2) {
bus_fwd = 1;
}
return -1;
if (bus_num == 1) {
int addr = GET_ADDR(to_fwd);
int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
if (!is_lkas_msg) {
bus_fwd = 2;
}
}
return bus_fwd;
}
const safety_hooks honda_hooks = {
panda/board/safety/safety_hyundai.h
+35 -39
View File
@@ -1,14 +1,14 @@
const int HYUNDAI_MAX_STEER = 255; // like stock
const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
const int32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
const int HYUNDAI_MAX_RATE_UP = 3;
const int HYUNDAI_MAX_RATE_DOWN = 7;
const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
int hyundai_camera_detected = 0;
bool hyundai_camera_detected = 0;
bool hyundai_giraffe_switch_2 = 0; // is giraffe switch 2 high?
int hyundai_camera_bus = 0;
int hyundai_giraffe_switch_2 = 0; // is giraffe switch 2 high?
int hyundai_rt_torque_last = 0;
int hyundai_desired_torque_last = 0;
int hyundai_cruise_engaged_last = 0;
@@ -16,17 +16,8 @@ uint32_t hyundai_ts_last = 0;
struct sample_t hyundai_torque_driver; // last few driver torques measured
static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if (to_push->RIR & 4) {
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
} else {
// Normal
addr = to_push->RIR >> 21;
}
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (addr == 897) {
int torque_driver_new = ((to_push->RDLR >> 11) & 0xfff) - 2048;
@@ -35,7 +26,7 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// check if stock camera ECU is still online
if (bus == 0 && addr == 832) {
if ((bus == 0) && (addr == 832)) {
hyundai_camera_detected = 1;
controls_allowed = 0;
}
@@ -46,44 +37,39 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((to_push->RIR>>21) == 1057) {
if (addr == 1057) {
// 2 bits: 13-14
int cruise_engaged = (to_push->RDLR >> 13) & 0x3;
if (cruise_engaged && !hyundai_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
}
if (!cruise_engaged) {
controls_allowed = 0;
}
hyundai_cruise_engaged_last = cruise_engaged;
}
// 832 is lkas cmd. If it is on camera bus, then giraffe switch 2 is high
if ((to_push->RIR>>21) == 832 && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
if ((addr == 832) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
hyundai_giraffe_switch_2 = 1;
}
}
static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
// There can be only one! (camera)
if (hyundai_camera_detected) {
return 0;
}
uint32_t addr;
if (to_send->RIR & 4) {
// Extended
addr = to_send->RIR >> 3;
} else {
// Normal
addr = to_send->RIR >> 21;
tx = 0;
}
// LKA STEER: safety check
if (addr == 832) {
int desired_torque = ((to_send->RDLR >> 16) & 0x7ff) - 1024;
uint32_t ts = TIM2->CNT;
int violation = 0;
bool violation = 0;
if (controls_allowed) {
@@ -122,7 +108,7 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
@@ -130,26 +116,36 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// ensuring that only the cancel button press is sent (VAL 4) when controls are off.
// This avoids unintended engagements while still allowing resume spam
// TODO: fix bug preventing the button msg to be fwd'd on bus 2
//if (((to_send->RIR>>21) == 1265) && !controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
// if ((to_send->RDLR & 0x7) != 4) return 0;
//if ((addr == 1265) && !controls_allowed && (bus == 0) {
// if ((to_send->RDLR & 0x7) != 4) {
// tx = 0;
// }
//}
// 1 allows the message through
return true;
return tx;
}
static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// forward cam to ccan and viceversa, except lkas cmd
if ((bus_num == 0 || bus_num == hyundai_camera_bus) && hyundai_giraffe_switch_2) {
if ((to_fwd->RIR>>21) == 832 && bus_num == hyundai_camera_bus) return -1;
if (bus_num == 0) return hyundai_camera_bus;
if (bus_num == hyundai_camera_bus) return 0;
int bus_fwd = -1;
// forward cam to ccan and viceversa, except lkas cmd
if (hyundai_giraffe_switch_2) {
if (bus_num == 0) {
bus_fwd = hyundai_camera_bus;
}
if (bus_num == hyundai_camera_bus) {
int addr = GET_ADDR(to_fwd);
if (addr != 832) {
bus_fwd = 0;
}
}
}
return -1;
return bus_fwd;
}
static void hyundai_init(int16_t param) {
UNUSED(param);
controls_allowed = 0;
hyundai_giraffe_switch_2 = 0;
}
panda/board/safety/safety_subaru.h
+25 -40
View File
@@ -2,7 +2,7 @@ const int SUBARU_MAX_STEER = 2047; // 1s
// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int SUBARU_MAX_RT_DELTA = 940; // max delta torque allowed for real time checks
const int32_t SUBARU_RT_INTERVAL = 250000; // 250ms between real time checks
const uint32_t SUBARU_RT_INTERVAL = 250000; // 250ms between real time checks
const int SUBARU_MAX_RATE_UP = 50;
const int SUBARU_MAX_RATE_DOWN = 70;
const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60;
@@ -14,14 +14,12 @@ int subaru_desired_torque_last = 0;
uint32_t subaru_ts_last = 0;
struct sample_t subaru_torque_driver; // last few driver torques measured
static void subaru_init(int16_t param) {
}
static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr = to_push->RIR >> 21;
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if ((addr == 0x119) && (bus_number == 0)){
if ((addr == 0x119) && (bus == 0)){
int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
@@ -29,11 +27,12 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x240) && (bus_number == 0)) {
if ((addr == 0x240) && (bus == 0)) {
int cruise_engaged = (to_push->RDHR >> 9) & 1;
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
}
if (!cruise_engaged) {
controls_allowed = 0;
}
subaru_cruise_engaged_last = cruise_engaged;
@@ -41,12 +40,13 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t addr = to_send->RIR >> 21;
int tx = 1;
int addr = GET_ADDR(to_send);
// steer cmd checks
if (addr == 0x122) {
int desired_torque = ((to_send->RDLR >> 16) & 0x1FFF);
int violation = 0;
bool violation = 0;
uint32_t ts = TIM2->CNT;
desired_torque = to_signed(desired_torque, 13);
@@ -88,52 +88,37 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
return true;
return tx;
}
static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// shifts bits 29 > 11
int32_t addr = to_fwd->RIR >> 21;
// forward CAN 0 > 1
int bus_fwd = -1;
if (bus_num == 0) {
return 2; // ES CAN
bus_fwd = 2; // Camera CAN
}
// forward CAN 1 > 0, except ES_LKAS
else if (bus_num == 2) {
// outback 2015
if (addr == 0x164) {
return -1;
if (bus_num == 2) {
// 356 is LKAS for outback 2015
// 356 is LKAS for Global Platform
// 545 is ES_Distance
// 802 is ES_LKAS
int addr = GET_ADDR(to_fwd);
int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
if (!block_msg) {
bus_fwd = 0; // Main CAN
}
// global platform
if (addr == 0x122) {
return -1;
}
// ES Distance
if (addr == 545) {
return -1;
}
// ES LKAS
if (addr == 802) {
return -1;
}
return 0; // Main CAN
}
// fallback to do not forward
return -1;
return bus_fwd;
}
const safety_hooks subaru_hooks = {
.init = subaru_init,
.init = nooutput_init,
.rx = subaru_rx_hook,
.tx = subaru_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
panda/board/safety/safety_tesla.h
+58 -110
View File
@@ -8,8 +8,8 @@
// brake rising edge
// brake > 0mph
//
int fmax_limit_check(float val, const float MAX, const float MIN) {
return (val > MAX) || (val < MIN);
bool fmax_limit_check(float val, const float MAX_VAL, const float MIN_VAL) {
return (val > MAX_VAL) || (val < MIN_VAL);
}
// 2m/s are added to be less restrictive
@@ -25,7 +25,7 @@ const struct lookup_t TESLA_LOOKUP_MAX_ANGLE = {
{2., 29., 38.},
{410., 92., 36.}};
const int TESLA_RT_INTERVAL = 250000; // 250ms between real time checks
const uint32_t TESLA_RT_INTERVAL = 250000; // 250ms between real time checks
// state of angle limits
float tesla_desired_angle_last = 0; // last desired steer angle
@@ -47,46 +47,27 @@ void reset_gmlan_switch_timeout(void);
void gmlan_switch_init(int timeout_enable);
static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push)
{
static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
set_gmlan_digital_output(0); // #define GMLAN_HIGH 0
reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled
//int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if (to_push->RIR & 4)
{
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
}
else
{
// Normal
addr = to_push->RIR >> 21;
}
int addr = GET_ADDR(to_push);
if (addr == 0x45)
{
if (addr == 0x45) {
// 6 bits starting at position 0
int lever_position = (to_push->RDLR & 0x3F);
if (lever_position == 2)
{ // pull forward
if (lever_position == 2) { // pull forward
// activate openpilot
controls_allowed = 1;
//}
}
else if (lever_position == 1)
{ // push towards the back
if (lever_position == 1) { // push towards the back
// deactivate openpilot
controls_allowed = 0;
}
}
// Detect drive rail on (ignition) (start recording)
if (addr == 0x348)
{
if (addr == 0x348) {
// GTW_status
int drive_rail_on = (to_push->RDLR & 0x0001);
tesla_ignition_started = drive_rail_on == 1;
@@ -94,66 +75,56 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push)
// exit controls on brake press
// DI_torque2::DI_brakePedal 0x118
if (addr == 0x118)
{
if (addr == 0x118) {
// 1 bit at position 16
if (((to_push->RDLR & 0x8000)) >> 15 == 1)
{
//disable break cancel by commenting line below
if ((((to_push->RDLR & 0x8000)) >> 15) == 1) {
// disable break cancel by commenting line below
controls_allowed = 0;
}
//get vehicle speed in m/s. Tesla gives MPH
tesla_speed = ((((((to_push->RDLR >> 24) & 0x0F) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05 - 25) * 1.609 / 3.6);
if (tesla_speed < 0)
{
tesla_speed = ((((((to_push->RDLR >> 24) & 0xF) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05) - 25) * 1.609 / 3.6;
if (tesla_speed < 0) {
tesla_speed = 0;
}
}
// exit controls on EPAS error
// EPAS_sysStatus::EPAS_eacStatus 0x370
if (addr == 0x370)
{
if (addr == 0x370) {
// if EPAS_eacStatus is not 1 or 2, disable control
eac_status = ((to_push->RDHR >> 21)) & 0x7;
// For human steering override we must not disable controls when eac_status == 0
// Additional safety: we could only allow eac_status == 0 when we have human steering allowed
if ((controls_allowed == 1) && (eac_status != 0) && (eac_status != 1) && (eac_status != 2))
{
if (controls_allowed && (eac_status != 0) && (eac_status != 1) && (eac_status != 2)) {
controls_allowed = 0;
//puts("EPAS error! \n");
}
}
//get latest steering wheel angle
if (addr == 0x00E)
{
float angle_meas_now = (int)((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1 - 819.2);
if (addr == 0x00E) {
float angle_meas_now = (int)(((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1) - 819.2);
uint32_t ts = TIM2->CNT;
uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last);
// *** angle real time check
// add 1 to not false trigger the violation and multiply by 25 since the check is done every 250 ms and steer angle is updated at 100Hz
float rt_delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.;
float rt_delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.;
float highest_rt_angle = tesla_rt_angle_last + (tesla_rt_angle_last > 0 ? rt_delta_angle_up : rt_delta_angle_down);
float lowest_rt_angle = tesla_rt_angle_last - (tesla_rt_angle_last > 0 ? rt_delta_angle_down : rt_delta_angle_up);
float rt_delta_angle_up = (interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25.) + 1.;
float rt_delta_angle_down = (interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25.) + 1.;
float highest_rt_angle = tesla_rt_angle_last + ((tesla_rt_angle_last > 0.) ? rt_delta_angle_up : rt_delta_angle_down);
float lowest_rt_angle = tesla_rt_angle_last - ((tesla_rt_angle_last > 0.) ? rt_delta_angle_down : rt_delta_angle_up);
if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last))
{
if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) {
tesla_rt_angle_last = angle_meas_now;
tesla_ts_angle_last = ts;
}
// check for violation;
if (fmax_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle))
{
if (fmax_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) {
// We should not be able to STEER under these conditions
// Other sending is fine (to allow human override)
controls_allowed = 0;
//puts("WARN: RT Angle - No steer allowed! \n");
}
else
{
} else {
controls_allowed = 1;
}
@@ -167,37 +138,28 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push)
// else
// block all commands that produce actuation
static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
{
static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t addr;
float angle_raw;
float desired_angle;
addr = to_send->RIR >> 21;
int tx = 1;
int addr = GET_ADDR(to_send);
// do not transmit CAN message if steering angle too high
// DAS_steeringControl::DAS_steeringAngleRequest
if (addr == 0x488)
{
angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8);
desired_angle = angle_raw * 0.1 - 1638.35;
int16_t violation = 0;
if (addr == 0x488) {
float angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8);
float desired_angle = (angle_raw * 0.1) - 1638.35;
bool violation = 0;
int st_enabled = (to_send->RDLR & 0x400000) >> 22;
if (st_enabled == 0) {
//steering is not enabled, do not check angles and do send
tesla_desired_angle_last = desired_angle;
return true;
}
if (controls_allowed)
{
} else if (controls_allowed) {
// add 1 to not false trigger the violation
float delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) + 1.;
float delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) + 1.;
float highest_desired_angle = tesla_desired_angle_last + (tesla_desired_angle_last > 0 ? delta_angle_up : delta_angle_down);
float lowest_desired_angle = tesla_desired_angle_last - (tesla_desired_angle_last > 0 ? delta_angle_down : delta_angle_up);
float highest_desired_angle = tesla_desired_angle_last + ((tesla_desired_angle_last > 0.) ? delta_angle_up : delta_angle_down);
float lowest_desired_angle = tesla_desired_angle_last - ((tesla_desired_angle_last > 0.) ? delta_angle_down : delta_angle_up);
float TESLA_MAX_ANGLE = interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.;
//check for max angles
@@ -206,69 +168,55 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
//check for angle delta changes
violation |= fmax_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
if (violation)
{
if (violation) {
controls_allowed = 0;
return false;
tx = 0;
}
tesla_desired_angle_last = desired_angle;
return true;
} else {
tx = 0;
}
return false;
}
return true;
return tx;
}
static void tesla_init(int16_t param)
{
static void tesla_init(int16_t param) {
UNUSED(param);
controls_allowed = 0;
tesla_ignition_started = 0;
gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled
}
static int tesla_ign_hook()
{
static int tesla_ign_hook(void) {
return tesla_ignition_started;
}
static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd)
{
static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int32_t addr = to_fwd->RIR >> 21;
if (bus_num == 0)
{
int bus_fwd = -1;
int addr = GET_ADDR(to_fwd);
if (bus_num == 0) {
// change inhibit of GTW_epasControl
if (addr == 0x101)
{
if (addr != 0x214) {
// remove EPB_epasControl
bus_fwd = 2; // Custom EPAS bus
}
if (addr == 0x101) {
to_fwd->RDLR = to_fwd->RDLR | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque)
int checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF;
uint32_t checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF;
to_fwd->RDLR = to_fwd->RDLR & 0xFFFF;
to_fwd->RDLR = to_fwd->RDLR + (checksum << 16);
return 2;
}
// remove EPB_epasControl
if (addr == 0x214)
{
return -1;
}
return 2; // Custom EPAS bus
}
if (bus_num == 2)
{
if (bus_num == 2) {
// remove GTW_epasControl in forwards
if (addr == 0x101)
{
return -1;
if (addr != 0x101) {
bus_fwd = 0; // Chassis CAN
}
return 0; // Chassis CAN
}
return -1;
return bus_fwd;
}
const safety_hooks tesla_hooks = {
panda/board/safety/safety_toyota.h
+51 -35
View File
@@ -10,7 +10,7 @@ const int TOYOTA_MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque
// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int TOYOTA_MAX_RT_DELTA = 375; // max delta torque allowed for real time checks
const int TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks
const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks
// longitudinal limits
const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2
@@ -33,8 +33,12 @@ struct sample_t toyota_torque_meas; // last 3 motor torques produced by th
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
// get eps motor torque (0.66 factor in dbc)
if ((to_push->RIR>>21) == 0x260) {
if (addr == 0x260) {
int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
torque_meas_new = to_signed(torque_meas_new, 16);
@@ -50,19 +54,20 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((to_push->RIR>>21) == 0x1D2) {
if (addr == 0x1D2) {
// 5th bit is CRUISE_ACTIVE
int cruise_engaged = to_push->RDLR & 0x20;
if (!cruise_engaged) {
controls_allowed = 0;
} else if (cruise_engaged && !toyota_cruise_engaged_last) {
}
if (cruise_engaged && !toyota_cruise_engaged_last) {
controls_allowed = 1;
}
toyota_cruise_engaged_last = cruise_engaged;
}
// exit controls on rising edge of gas press if interceptor (0x201)
if ((to_push->RIR>>21) == 0x201) {
// exit controls on rising edge of interceptor gas press
if (addr == 0x201) {
gas_interceptor_detected = 1;
int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
@@ -74,7 +79,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press
if ((to_push->RIR>>21) == 0x2C1) {
if (addr == 0x2C1) {
int gas = (to_push->RDHR >> 16) & 0xFF;
if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected && long_controls_allowed) {
controls_allowed = 0;
@@ -82,52 +87,60 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
toyota_gas_prev = gas;
}
int bus = (to_push->RDTR >> 4) & 0xF;
// msgs are only on bus 2 if panda is connected to frc
if (bus == 2) {
toyota_camera_forwarded = 1;
}
// 0x2E4 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high
if ((to_push->RIR>>21) == 0x2E4 && (bus == 0)) {
if ((addr == 0x2E4) && (bus == 0)) {
toyota_giraffe_switch_1 = 1;
}
}
static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
// Check if msg is sent on BUS 0
if (((to_send->RDTR >> 4) & 0xF) == 0) {
if (bus == 0) {
// no IPAS in non IPAS mode
if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) return false;
if ((addr == 0x266) || (addr == 0x167)) {
tx = 0;
}
// GAS PEDAL: safety check
if ((to_send->RIR>>21) == 0x200) {
if (controls_allowed && long_controls_allowed) {
// all messages are fine here
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if (addr == 0x200) {
if (!controls_allowed || !long_controls_allowed) {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
}
// ACCEL: safety check on byte 1-2
if ((to_send->RIR>>21) == 0x343) {
if (addr == 0x343) {
int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF);
desired_accel = to_signed(desired_accel, 16);
if (controls_allowed && long_controls_allowed) {
int violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) return 0;
} else if (desired_accel != 0) {
return 0;
if (!controls_allowed || !long_controls_allowed) {
if (desired_accel != 0) {
tx = 0;
}
}
bool violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) {
tx = 0;
}
}
// STEER: safety check on bytes 2-3
if ((to_send->RIR>>21) == 0x2E4) {
if (addr == 0x2E4) {
int desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF);
desired_torque = to_signed(desired_torque, 16);
int violation = 0;
bool violation = 0;
uint32_t ts = TIM2->CNT;
@@ -167,13 +180,13 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
}
// 1 allows the message through
return true;
return tx;
}
static void toyota_init(int16_t param) {
@@ -185,22 +198,25 @@ static void toyota_init(int16_t param) {
static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
if (toyota_camera_forwarded && !toyota_giraffe_switch_1) {
int addr = to_fwd->RIR>>21;
if (bus_num == 0) {
return 2;
} else if (bus_num == 2) {
bus_fwd = 2;
}
if (bus_num == 2) {
int addr = GET_ADDR(to_fwd);
// block stock lkas messages and stock acc messages (if OP is doing ACC)
int is_lkas_msg = (addr == 0x2E4 || addr == 0x412);
// in TSSP 2.0 the camera does ACC as well, so filter 0x343
// in TSS2, 0.191 is LTA which we need to block to avoid controls collision
int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191));
// in TSS2 the camera does ACC as well, so filter 0x343
int is_acc_msg = (addr == 0x343);
if (is_lkas_msg || (is_acc_msg && long_controls_allowed)) {
return -1;
int block_msg = is_lkas_msg || (is_acc_msg && long_controls_allowed);
if (!block_msg) {
bus_fwd = 0;
}
return 0;
}
}
return -1;
return bus_fwd;
}
const safety_hooks toyota_hooks = {
panda/board/safety/safety_toyota_ipas.h
+34 -20
View File
@@ -35,7 +35,9 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// check standard toyota stuff as well
toyota_rx_hook(to_push);
if ((to_push->RIR>>21) == 0x260) {
int addr = GET_ADDR(to_push);
if (addr == 0x260) {
// get driver steering torque
int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF));
@@ -44,7 +46,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// get steer angle
if ((to_push->RIR>>21) == 0x25) {
if (addr == 0x25) {
int angle_meas_new = ((to_push->RDLR & 0xf) << 8) + ((to_push->RDLR & 0xff00) >> 8);
uint32_t ts = TIM2->CNT;
@@ -55,10 +57,10 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// *** angle real time check
// add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz
int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG + 1.)));
int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG + 1.)));
int highest_rt_angle = rt_angle_last + (rt_angle_last > 0? rt_delta_angle_up:rt_delta_angle_down);
int lowest_rt_angle = rt_angle_last - (rt_angle_last > 0? rt_delta_angle_down:rt_delta_angle_up);
int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG) + 1.)));
int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG) + 1.)));
int highest_rt_angle = rt_angle_last + ((rt_angle_last > 0) ? rt_delta_angle_up : rt_delta_angle_down);
int lowest_rt_angle = rt_angle_last - ((rt_angle_last > 0) ? rt_delta_angle_down : rt_delta_angle_up);
// every RT_INTERVAL or when controls are turned on, set the new limits
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last);
@@ -78,12 +80,12 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// get speed
if ((to_push->RIR>>21) == 0xb4) {
if (addr == 0xb4) {
speed = ((float) (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF))) * 0.01 / 3.6;
}
// get ipas state
if ((to_push->RIR>>21) == 0x262) {
if (addr == 0x262) {
ipas_state = (to_push->RDLR & 0xf);
}
@@ -97,25 +99,34 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int bypass_standard_tx_hook = 0;
int bus = GET_BUS(to_send);
int addr = GET_ADDR(to_send);
// Check if msg is sent on BUS 0
if (((to_send->RDTR >> 4) & 0xF) == 0) {
if (bus == 0) {
// STEER ANGLE
if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) {
if ((addr == 0x266) || (addr == 0x167)) {
angle_control = 1; // we are in angle control mode
int desired_angle = ((to_send->RDLR & 0xf) << 8) + ((to_send->RDLR & 0xff00) >> 8);
int ipas_state_cmd = ((to_send->RDLR & 0xff) >> 4);
int16_t violation = 0;
bool violation = 0;
desired_angle = to_signed(desired_angle, 12);
if (controls_allowed) {
// add 1 to not false trigger the violation
int delta_angle_up = (int) (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG + 1.);
int delta_angle_down = (int) (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG + 1.);
int highest_desired_angle = desired_angle_last + (desired_angle_last > 0? delta_angle_up:delta_angle_down);
int lowest_desired_angle = desired_angle_last - (desired_angle_last > 0? delta_angle_down:delta_angle_up);
float delta_angle_float;
delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG) + 1.;
int delta_angle_up = (int) (delta_angle_float);
delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG) + 1.;
int delta_angle_down = (int) (delta_angle_float);
int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
int lowest_desired_angle = desired_angle_last - ((desired_angle_last > 0) ? delta_angle_down : delta_angle_up);
if ((desired_angle > highest_desired_angle) ||
(desired_angle < lowest_desired_angle)){
violation = 1;
@@ -134,15 +145,18 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
desired_angle_last = desired_angle;
if (violation) {
return false;
tx = 0;
}
return true;
bypass_standard_tx_hook = 1;
}
}
// check standard toyota stuff as well
return toyota_tx_hook(to_send);
// check standard toyota stuff as well if addr isn't IPAS related
if (!bypass_standard_tx_hook) {
tx &= toyota_tx_hook(to_send);
}
return tx;
}
const safety_hooks toyota_ipas_hooks = {
panda/board/safety_declarations.h
+56
View File
@@ -0,0 +1,56 @@
#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
#define GET_LEN(msg) ((msg)->RDTR & 0xf)
#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
// sample struct that keeps 3 samples in memory
struct sample_t {
int values[6];
int min;
int max;
} sample_t_default = {{0}, 0, 0};
// safety code requires floats
struct lookup_t {
float x[3];
float y[3];
};
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
int safety_ignition_hook(void);
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
int to_signed(int d, int bits);
void update_sample(struct sample_t *sample, int sample_new);
bool max_limit_check(int val, const int MAX, const int MIN);
bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
float interpolate(struct lookup_t xy, float x);
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send);
typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len);
typedef int (*ign_hook)(void);
typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
typedef struct {
safety_hook_init init;
ign_hook ignition;
rx_hook rx;
tx_hook tx;
tx_lin_hook tx_lin;
fwd_hook fwd;
} safety_hooks;
// This can be set by the safety hooks.
bool controls_allowed = 0;
bool gas_interceptor_detected = 0;
int gas_interceptor_prev = 0;
// This is set by USB command 0xdf
bool long_controls_allowed = 1;
panda/board/spi_flasher.h
+5 -5
View File
@@ -6,7 +6,7 @@ int unlocked = 0;
void debug_ring_callback(uart_ring *ring) {}
#endif
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {
int resp_len = 0;
// flasher machine
@@ -80,7 +80,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
break;
// **** 0xd6: get version
case 0xd6:
COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN)
COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN);
memcpy(resp, gitversion, sizeof(gitversion));
resp_len = sizeof(gitversion);
break;
@@ -92,8 +92,8 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
return resp_len;
}
int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; }
void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) { }
int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) { return 0; }
void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) { }
int is_enumerated = 0;
void usb_cb_enumeration_complete() {
@@ -101,7 +101,7 @@ void usb_cb_enumeration_complete() {
is_enumerated = 1;
}
void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {
void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
set_led(LED_RED, 0);
for (int i = 0; i < len/4; i++) {
// program byte 1
panda/boardesp/elm327.c
+5 -5
View File
@@ -10,8 +10,8 @@
//#define ELM_DEBUG
#define min(a,b) ((a) < (b) ? (a) : (b))
#define max(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#define MAX(a,b) ((a) > (b) ? (a) : (b))
int ICACHE_FLASH_ATTR spi_comm(char *dat, int len, uint32_t *recvData, int recvDataLen);
#define ELM_PORT 35000
@@ -855,7 +855,7 @@ static void ICACHE_FLASH_ATTR elm_process_obd_cmd_LINFast(const elm_protocol_t*
panda_kline_wakeup_pulse();
} else {
bytelen = min(bytelen, 7);
bytelen = MIN(bytelen, 7);
for(int i = 0; i < bytelen; i++){
msg.dat[i] = elm_decode_hex_byte(&cmd[i*2]);
msg.dat[bytelen] += msg.dat[i];
@@ -1059,7 +1059,7 @@ static void ICACHE_FLASH_ATTR elm_process_obd_cmd_ISO15765(const elm_protocol_t*
return;
}
msg.len = min(msg.len, 7);
msg.len = MIN(msg.len, 7);
for(int i = 0; i < msg.len; i++)
msg.dat[i] = elm_decode_hex_byte(&cmd[i*2]);
@@ -1398,7 +1398,7 @@ static void ICACHE_FLASH_ATTR elm_process_at_cmd(char *cmd, uint16_t len) {
}
tmp = elm_decode_hex_byte(&cmd[2]);
elm_mode_keepalive_period = tmp ? max(tmp, 0x20) * 20 : 0;
elm_mode_keepalive_period = tmp ? MAX(tmp, 0x20) * 20 : 0;
if(lin_bus_initialized){
os_timer_disarm(&elm_proto_aux_timeout);
panda/boardesp/proxy.c
+2 -2
View File
@@ -9,12 +9,12 @@
#include "driver/uart.h"
#include "crypto/sha.h"
#define min(a,b) \
#define MIN(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
#define max(a,b) \
#define MAX(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
panda/boardesp/webserver.c
+5 -5
View File
@@ -14,8 +14,8 @@
#include "obj/gitversion.h"
#include "obj/cert.h"
#define max(a,b) ((a) > (b) ? (a) : (b))
#define min(a,b) ((a) < (b) ? (a) : (b))
#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#define espconn_send_string(conn, x) espconn_send(conn, x, strlen(x))
#define MAX_RESP 0x800
@@ -116,7 +116,7 @@ void ICACHE_FLASH_ATTR st_flash() {
// real content length will always be 0x10 aligned
os_printf("st_flash: flashing\n");
for (int i = 0; i < real_content_length; i += 0x10) {
int rl = min(0x10, real_content_length-i);
int rl = MIN(0x10, real_content_length-i);
usb_cmd(2, rl, 0, 0, 0, &st_firmware[i]);
system_soft_wdt_feed();
}
@@ -288,7 +288,7 @@ static void ICACHE_FLASH_ATTR web_rx_cb(void *arg, char *data, uint16_t len) {
}
} else if (state == RECEIVING_ST_FIRMWARE) {
os_printf("receiving st firmware: %d/%d\n", len, content_length);
memcpy(st_firmware_ptr, data, min(content_length, len));
memcpy(st_firmware_ptr, data, MIN(content_length, len));
st_firmware_ptr += len;
content_length -= len;
@@ -333,7 +333,7 @@ static void ICACHE_FLASH_ATTR web_rx_cb(void *arg, char *data, uint16_t len) {
SHA_init(&ctx);
for (ll = start_address; ll < esp_address-RSANUMBYTES; ll += 0x80) {
spi_flash_read(ll, dat, 0x80);
SHA_update(&ctx, dat, min((esp_address-RSANUMBYTES)-ll, 0x80));
SHA_update(&ctx, dat, MIN((esp_address-RSANUMBYTES)-ll, 0x80));
}
memcpy(digest, SHA_final(&ctx), SHA_DIGEST_SIZE);
panda/drivers/windows/pandaJ2534DLL/MessageRx.h
+1 -1
View File
@@ -21,7 +21,7 @@ public:
}
this->next_part = (this->next_part + 1) % 0x10;
unsigned int payload_len = min(expected_size - msg.size(), max_packet_size);
unsigned int payload_len = MIN(expected_size - msg.size(), max_packet_size);
if (piece.size() < payload_len) {
//A frame was received that could have held more data.
//No examples of this protocol show that happening, so
panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp
+1 -1
View File
@@ -170,7 +170,7 @@ DWORD PandaJ2534Device::msg_tx_thread() {
} else { //Ran out of things that need to be sent now. Sleep!
auto time_diff = std::chrono::duration_cast<std::chrono::milliseconds>
(this->task_queue.front()->expire - std::chrono::steady_clock::now());
sleepDuration = max(1, time_diff.count());
sleepDuration = MAX(1, time_diff.count());
goto break_flow_ctrl_loop;
}
}
panda/python/dfu.py
+3 -4
View File
@@ -18,7 +18,7 @@ class PandaDFU(object):
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11:
try:
this_dfu_serial = device._getASCIIStringDescriptor(3)
this_dfu_serial = device.open().getASCIIStringDescriptor(3)
except Exception:
continue
if this_dfu_serial == dfu_serial or dfu_serial is None:
@@ -35,7 +35,7 @@ class PandaDFU(object):
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11:
try:
dfu_serials.append(device._getASCIIStringDescriptor(3))
dfu_serials.append(device.open().getASCIIStringDescriptor(3))
except Exception:
pass
except Exception:
@@ -73,7 +73,7 @@ class PandaDFU(object):
def program(self, address, dat, block_size=None):
if block_size == None:
block_size = len(dat)
# Set Address Pointer
self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, "\x21" + struct.pack("I", address))
self.status()
@@ -119,4 +119,3 @@ class PandaDFU(object):
stat = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6))
except Exception:
pass
panda/python/esptool.py
+1 -1
View File
@@ -1216,7 +1216,7 @@ def main():
operation_func = globals()[args.operation]
operation_args,_,_,_ = inspect.getargspec(operation_func)
if operation_args[0] == 'esp': # operation function takes an ESPROM connection object
initial_baud = min(ESPROM.ESP_ROM_BAUD, args.baud) # don't sync faster than the default baud rate
initial_baud = MIN(ESPROM.ESP_ROM_BAUD, args.baud) # don't sync faster than the default baud rate
esp = ESPROM(args.port, initial_baud)
esp.connect()
operation_func(esp, args)
panda/tests/automated/5_wifi_udp.py
+1 -1
View File
@@ -34,7 +34,7 @@ def test_udp_doesnt_drop(serial=None):
sys.stdout.flush()
else:
print("UDP WIFI loopback %d messages at speed %d, comp speed is %.2f, percent %.2f" % (msg_count, speed, comp_kbps, saturation_pct))
assert_greater(saturation_pct, 20) #sometimes the wifi can be slow...
assert_greater(saturation_pct, 15) #sometimes the wifi can be slow...
assert_less(saturation_pct, 100)
saturation_pcts.append(saturation_pct)
if len(saturation_pcts) > 0:
panda/tests/build_strict/test_build_strict.sh
Executable
+15
View File
@@ -0,0 +1,15 @@
#!/bin/bash -e
cd ../../board/
make -f Makefile.strict clean
make -f Makefile.strict bin 2> compiler_output.txt
if [[ -s "compiler_output.txt" ]]
then
echo "Found alerts from the compiler:"
cat compiler_output.txt
exit 1
fi
panda/tests/elm_car_simulator.py
+3 -3
View File
@@ -152,7 +152,7 @@ class ELMCarSimulator():
if len(outmsg) <= 5:
self._lin_send(0x10, obd_header + outmsg)
else:
first_msg_len = min(4, len(outmsg)%4) or 4
first_msg_len = MIN(4, len(outmsg)%4) or 4
self._lin_send(0x10, obd_header + b'\x01' +
b'\x00'*(4-first_msg_len) +
outmsg[:first_msg_len])
@@ -229,7 +229,7 @@ class ELMCarSimulator():
outaddr = 0x7E8 if address == 0x7DF or address == 0x7E0 else 0x18DAF110
msgnum = 1
while(self.__can_multipart_data):
datalen = min(7, len(self.__can_multipart_data))
datalen = MIN(7, len(self.__can_multipart_data))
msgpiece = struct.pack("B", 0x20 | msgnum) + self.__can_multipart_data[:datalen]
self._can_send(outaddr, msgpiece)
self.__can_multipart_data = self.__can_multipart_data[7:]
@@ -246,7 +246,7 @@ class ELMCarSimulator():
self._can_send(outaddr,
struct.pack("BBB", len(outmsg)+2, 0x40|data[1], pid) + outmsg)
else:
first_msg_len = min(3, len(outmsg)%7)
first_msg_len = MIN(3, len(outmsg)%7)
payload_len = len(outmsg)+3
msgpiece = struct.pack("BBBBB", 0x10 | ((payload_len>>8)&0xF),
payload_len&0xFF,
panda/tests/misra/test_misra.sh
+19 -3
View File
@@ -1,8 +1,24 @@
#!/bin/bash -e
git clone https://github.com/danmar/cppcheck.git || true
cd cppcheck
git checkout 29e5992e51ecf1ddba469c73a0eed0b28b131de5
git fetch
git checkout 44d6066c6fad32e2b0332b3f2b24bd340febaef8
make -j4
cd ../../../
tests/misra/cppcheck/cppcheck --dump board/main.c
python tests/misra/cppcheck/addons/misra.py board/main.c.dump 2>/tmp/misra/output.txt || true
# whole panda code
tests/misra/cppcheck/cppcheck --dump --enable=all --inline-suppr board/main.c 2>/tmp/misra/cppcheck_output.txt || true
python tests/misra/cppcheck/addons/misra.py board/main.c.dump 2>/tmp/misra/misra_output.txt || true
# violations in safety files
(cat /tmp/misra/misra_output.txt | grep safety) > /tmp/misra/misra_safety_output.txt || true
(cat /tmp/misra/cppcheck_output.txt | grep safety) > /tmp/misra/cppcheck_safety_output.txt || true
if [[ -s "/tmp/misra/misra_safety_output.txt" ]] || [[ -s "/tmp/misra/cppcheck_safety_output.txt" ]]
then
echo "Found Misra violations in the safety code:"
cat /tmp/misra/misra_safety_output.txt
cat /tmp/misra/cppcheck_safety_output.txt
exit 1
fi
pyextra/gunicorn/instrument/__init__.py → panda/tests/safety/__init__.py
View File
panda/tests/safety/libpandasafety_py.py
+16 -29
View File
@@ -30,76 +30,63 @@ typedef struct
uint32_t CNT;
} TIM_TypeDef;
void set_controls_allowed(int c);
int get_controls_allowed(void);
void set_long_controls_allowed(int c);
int get_long_controls_allowed(void);
void set_gas_interceptor_detected(int c);
int get_gas_interceptor_detetcted(void);
void set_controls_allowed(bool c);
bool get_controls_allowed(void);
void set_long_controls_allowed(bool c);
bool get_long_controls_allowed(void);
void set_gas_interceptor_detected(bool c);
bool get_gas_interceptor_detetcted(void);
int get_gas_interceptor_prev(void);
void set_timer(int t);
void set_timer(uint32_t t);
void reset_angle_control(void);
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
int safety_set_mode(uint16_t mode, int16_t param);
void init_tests_toyota(void);
void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void toyota_init(int16_t param);
int get_toyota_torque_meas_min(void);
int get_toyota_torque_meas_max(void);
int get_toyota_gas_prev(void);
void set_toyota_torque_meas(int min, int max);
void set_toyota_desired_torque_last(int t);
void set_toyota_camera_forwarded(int t);
void set_toyota_rt_torque_last(int t);
void init_tests_honda(void);
int get_honda_ego_speed(void);
void honda_init(int16_t param);
void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int get_honda_brake_prev(void);
int get_honda_gas_prev(void);
void set_honda_alt_brake_msg(bool);
void set_honda_bosch_hardware(bool);
void init_tests_cadillac(void);
void cadillac_init(int16_t param);
void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void set_cadillac_desired_torque_last(int t);
void set_cadillac_rt_torque_last(int t);
void set_cadillac_torque_driver(int min, int max);
void init_tests_gm(void);
void gm_init(int16_t param);
void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void set_gm_desired_torque_last(int t);
void set_gm_rt_torque_last(int t);
void set_gm_torque_driver(int min, int max);
void init_tests_hyundai(void);
void nooutput_init(int16_t param);
void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void set_hyundai_desired_torque_last(int t);
void set_hyundai_rt_torque_last(int t);
void set_hyundai_torque_driver(int min, int max);
void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void set_hyundai_giraffe_switch_2(int t);
void set_hyundai_camera_bus(int t);
void init_tests_chrysler(void);
void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void set_chrysler_desired_torque_last(int t);
void set_chrysler_rt_torque_last(int t);
void set_chrysler_camera_detected(int t);
int get_chrysler_torque_meas_min(void);
int get_chrysler_torque_meas_max(void);
void set_chrysler_torque_meas(int min, int max);
void init_tests_subaru(void);
void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void set_subaru_desired_torque_last(int t);
void set_subaru_rt_torque_last(int t);
void set_subaru_torque_driver(int min, int max);
panda/tests/safety/test.c
+27 -9
View File
@@ -32,30 +32,32 @@ struct sample_t subaru_torque_driver;
TIM_TypeDef timer;
TIM_TypeDef *TIM2 = &timer;
#define min(a,b) \
#define MIN(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
#define max(a,b) \
#define MAX(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#define UNUSED(x) (void)(x)
#define PANDA
#define NULL ((void*)0)
#define static
#include "safety.h"
void set_controls_allowed(int c){
void set_controls_allowed(bool c){
controls_allowed = c;
}
void set_long_controls_allowed(int c){
void set_long_controls_allowed(bool c){
long_controls_allowed = c;
}
void set_gas_interceptor_detected(int c){
void set_gas_interceptor_detected(bool c){
gas_interceptor_detected = c;
}
@@ -63,15 +65,15 @@ void reset_angle_control(void){
angle_control = 0;
}
int get_controls_allowed(void){
bool get_controls_allowed(void){
return controls_allowed;
}
int get_long_controls_allowed(void){
bool get_long_controls_allowed(void){
return long_controls_allowed;
}
int get_gas_interceptor_detected(void){
bool get_gas_interceptor_detected(void){
return gas_interceptor_detected;
}
@@ -79,10 +81,14 @@ int get_gas_interceptor_prev(void){
return gas_interceptor_prev;
}
void set_timer(int t){
void set_timer(uint32_t t){
timer.CNT = t;
}
void set_toyota_camera_forwarded(int t){
toyota_camera_forwarded = t;
}
void set_toyota_torque_meas(int min, int max){
toyota_torque_meas.min = min;
toyota_torque_meas.max = max;
@@ -103,6 +109,18 @@ void set_hyundai_torque_driver(int min, int max){
hyundai_torque_driver.max = max;
}
void set_hyundai_camera_bus(int t){
hyundai_camera_bus = t;
}
void set_hyundai_giraffe_switch_2(int t){
hyundai_giraffe_switch_2 = t;
}
void set_chrysler_camera_detected(int t){
chrysler_camera_detected = t;
}
void set_chrysler_torque_meas(int min, int max){
chrysler_torque_meas.min = min;
chrysler_torque_meas.max = max;
panda/tests/safety/test_cadillac.py
+38 -24
View File
@@ -31,9 +31,16 @@ class TestCadillacSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.cadillac_init(0)
cls.safety.safety_set_mode(6, 0)
cls.safety.init_tests_cadillac()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def _set_prev_torque(self, t):
self.safety.set_cadillac_desired_torque_last(t)
self.safety.set_cadillac_rt_torque_last(t)
@@ -46,10 +53,6 @@ class TestCadillacSafety(unittest.TestCase):
to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8)
return to_send
def _torque_driver_msg_array(self, torque):
for i in range(3):
self.safety.cadillac_ipas_rx_hook(self._torque_driver_msg(torque))
def _torque_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x151 << 21
@@ -72,7 +75,7 @@ class TestCadillacSafety(unittest.TestCase):
to_push[0].RDLR = 0x800000
to_push[0].RDTR = 0
self.safety.cadillac_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertTrue(self.safety.get_controls_allowed())
def test_disable_control_allowed_from_cruise(self):
@@ -82,7 +85,7 @@ class TestCadillacSafety(unittest.TestCase):
to_push[0].RDTR = 0
self.safety.set_controls_allowed(1)
self.safety.cadillac_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertFalse(self.safety.get_controls_allowed())
def test_torque_absolute_limits(self):
@@ -98,22 +101,22 @@ class TestCadillacSafety(unittest.TestCase):
else:
send = torque == 0
self.assertEqual(send, self.safety.cadillac_tx_hook(self._torque_msg(torque)))
self.assertEqual(send, self.safety.safety_tx_hook(self._torque_msg(torque)))
def test_non_realtime_limit_up(self):
self.safety.set_cadillac_torque_driver(0, 0)
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
def test_non_realtime_limit_down(self):
self.safety.set_cadillac_torque_driver(0, 0)
@@ -127,10 +130,10 @@ class TestCadillacSafety(unittest.TestCase):
t *= -sign
self.safety.set_cadillac_torque_driver(t, t)
self._set_prev_torque(MAX_TORQUE * sign)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(MAX_TORQUE * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_TORQUE * sign)))
self.safety.set_cadillac_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_TORQUE)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_TORQUE)))
# spot check some individual cases
for sign in [-1, 1]:
@@ -139,20 +142,20 @@ class TestCadillacSafety(unittest.TestCase):
delta = 1 * sign
self._set_prev_torque(torque_desired)
self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
self._set_prev_torque(torque_desired + delta)
self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired + delta)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
self._set_prev_torque(MAX_TORQUE * sign)
self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN) * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN) * sign)))
self._set_prev_torque(MAX_TORQUE * sign)
self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(0)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
self._set_prev_torque(MAX_TORQUE * sign)
self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN + 1) * sign)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN + 1) * sign)))
def test_realtime_limits(self):
@@ -164,18 +167,29 @@ class TestCadillacSafety(unittest.TestCase):
self.safety.set_cadillac_torque_driver(0, 0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self._set_prev_torque(0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
self.safety.set_timer(RT_INTERVAL + 1)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
def test_fwd_hook(self):
# nothing allowed
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
for b in buss:
for m in msgs:
# assume len 8
self.assertEqual(-1, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
if __name__ == "__main__":
panda/tests/safety/test_chrysler.py
+52 -49
View File
@@ -35,9 +35,16 @@ class TestChryslerSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.nooutput_init(0)
cls.safety.safety_set_mode(9, 0)
cls.safety.init_tests_chrysler()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def _button_msg(self, buttons):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 1265 << 21
@@ -70,9 +77,9 @@ class TestChryslerSafety(unittest.TestCase):
self.safety.set_controls_allowed(enabled)
self._set_prev_torque(t)
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t)))
else:
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
def test_manually_enable_controls_allowed(self):
self.safety.set_controls_allowed(1)
@@ -85,7 +92,7 @@ class TestChryslerSafety(unittest.TestCase):
to_push[0].RIR = 0x1f4 << 21
to_push[0].RDLR = 0x380000
self.safety.chrysler_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertTrue(self.safety.get_controls_allowed())
def test_disable_control_allowed_from_cruise(self):
@@ -94,17 +101,17 @@ class TestChryslerSafety(unittest.TestCase):
to_push[0].RDLR = 0
self.safety.set_controls_allowed(1)
self.safety.chrysler_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertFalse(self.safety.get_controls_allowed())
def test_non_realtime_limit_up(self):
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
def test_non_realtime_limit_down(self):
self.safety.set_controls_allowed(True)
@@ -113,12 +120,12 @@ class TestChryslerSafety(unittest.TestCase):
torque_meas = MAX_STEER - MAX_TORQUE_ERROR - 20
self.safety.set_chrysler_torque_meas(torque_meas, torque_meas)
self.safety.set_chrysler_desired_torque_last(MAX_STEER)
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN)))
self.safety.set_chrysler_rt_torque_last(MAX_STEER)
self.safety.set_chrysler_torque_meas(torque_meas, torque_meas)
self.safety.set_chrysler_desired_torque_last(MAX_STEER)
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN + 1)))
def test_exceed_torque_sensor(self):
self.safety.set_controls_allowed(True)
@@ -127,9 +134,9 @@ class TestChryslerSafety(unittest.TestCase):
self._set_prev_torque(0)
for t in np.arange(0, MAX_TORQUE_ERROR + 2, 2): # step needs to be smaller than MAX_TORQUE_ERROR
t *= sign
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2))))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2))))
def test_realtime_limit_up(self):
self.safety.set_controls_allowed(True)
@@ -140,65 +147,61 @@ class TestChryslerSafety(unittest.TestCase):
for t in np.arange(0, MAX_RT_DELTA+1, 1):
t *= sign
self.safety.set_chrysler_torque_meas(t, t)
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self._set_prev_torque(0)
for t in np.arange(0, MAX_RT_DELTA+1, 1):
t *= sign
self.safety.set_chrysler_torque_meas(t, t)
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
self.safety.set_timer(RT_INTERVAL + 1)
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(sign * MAX_RT_DELTA)))
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * MAX_RT_DELTA)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
def test_torque_measurements(self):
self.safety.chrysler_rx_hook(self._torque_meas_msg(50))
self.safety.chrysler_rx_hook(self._torque_meas_msg(-50))
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(50))
self.safety.safety_rx_hook(self._torque_meas_msg(-50))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min())
self.assertEqual(50, self.safety.get_chrysler_torque_meas_max())
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min())
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
self.assertEqual(0, self.safety.get_chrysler_torque_meas_min())
def _replay_drive(self, csv_reader):
for row in csv_reader:
if len(row) != 4: # sometimes truncated at end of the file
continue
if row[0] == 'time': # skip CSV header
continue
addr = int(row[1])
bus = int(row[2])
data_str = row[3] # Example '081407ff0806e06f'
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDHR = swap_bytes(data_str[8:])
to_send[0].RDLR = swap_bytes(data_str[:8])
if (bus == 128):
self.assertTrue(self.safety.chrysler_tx_hook(to_send), msg=row)
else:
self.safety.chrysler_rx_hook(to_send)
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
chrysler_camera_detected = [0, 1]
def test_replay_drive(self):
# In Cabana, click "Save Log" and then put the downloaded CSV in this directory.
test_files = glob.glob('chrysler_*.csv')
for filename in test_files:
print 'testing %s' % filename
with open(filename) as csvfile:
reader = csv.reader(csvfile)
self._replay_drive(reader)
for ccd in chrysler_camera_detected:
self.safety.set_chrysler_camera_detected(ccd)
blocked_msgs = [658, 678]
for b in buss:
for m in msgs:
if not ccd:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0
else:
fwd_bus = -1
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
if __name__ == "__main__":
panda/tests/safety/test_gm.py
+57 -39
View File
@@ -32,9 +32,16 @@ class TestGmSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.gm_init(0)
cls.safety.safety_set_mode(3, 0)
cls.safety.init_tests_gm()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def _speed_msg(self, speed):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 842 << 21
@@ -98,62 +105,62 @@ class TestGmSafety(unittest.TestCase):
def test_resume_button(self):
RESUME_BTN = 2
self.safety.set_controls_allowed(0)
self.safety.gm_rx_hook(self._button_msg(RESUME_BTN))
self.safety.safety_rx_hook(self._button_msg(RESUME_BTN))
self.assertTrue(self.safety.get_controls_allowed())
def test_set_button(self):
SET_BTN = 3
self.safety.set_controls_allowed(0)
self.safety.gm_rx_hook(self._button_msg(SET_BTN))
self.safety.safety_rx_hook(self._button_msg(SET_BTN))
self.assertTrue(self.safety.get_controls_allowed())
def test_cancel_button(self):
CANCEL_BTN = 6
self.safety.set_controls_allowed(1)
self.safety.gm_rx_hook(self._button_msg(CANCEL_BTN))
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN))
self.assertFalse(self.safety.get_controls_allowed())
def test_disengage_on_brake(self):
self.safety.set_controls_allowed(1)
self.safety.gm_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed())
def test_allow_brake_at_zero_speed(self):
# Brake was already pressed
self.safety.gm_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.safety.set_controls_allowed(1)
self.safety.gm_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.gm_rx_hook(self._brake_msg(False))
self.safety.safety_rx_hook(self._brake_msg(False))
def test_not_allow_brake_when_moving(self):
# Brake was already pressed
self.safety.gm_rx_hook(self._brake_msg(True))
self.safety.gm_rx_hook(self._speed_msg(100))
self.safety.safety_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._speed_msg(100))
self.safety.set_controls_allowed(1)
self.safety.gm_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.gm_rx_hook(self._brake_msg(False))
self.safety.safety_rx_hook(self._brake_msg(False))
def test_disengage_on_gas(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.set_controls_allowed(1)
self.safety.gm_rx_hook(self._gas_msg(True))
self.safety.safety_rx_hook(self._gas_msg(True))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
self.assertTrue(self.safety.get_controls_allowed())
self.safety.gm_rx_hook(self._gas_msg(False))
self.safety.safety_rx_hook(self._gas_msg(False))
def test_allow_engage_with_gas_pressed(self):
self.safety.gm_rx_hook(self._gas_msg(True))
self.safety.safety_rx_hook(self._gas_msg(True))
self.safety.set_controls_allowed(1)
self.safety.gm_rx_hook(self._gas_msg(True))
self.safety.safety_rx_hook(self._gas_msg(True))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.gm_rx_hook(self._gas_msg(False))
self.safety.safety_rx_hook(self._gas_msg(False))
def test_brake_safety_check(self):
for long_controls_allowed in [0, 1]:
@@ -162,9 +169,9 @@ class TestGmSafety(unittest.TestCase):
for b in range(0, 500):
self.safety.set_controls_allowed(enabled)
if abs(b) > MAX_BRAKE or ((not enabled or not long_controls_allowed) and b != 0):
self.assertFalse(self.safety.gm_tx_hook(self._send_brake_msg(b)))
self.assertFalse(self.safety.safety_tx_hook(self._send_brake_msg(b)))
else:
self.assertTrue(self.safety.gm_tx_hook(self._send_brake_msg(b)))
self.assertTrue(self.safety.safety_tx_hook(self._send_brake_msg(b)))
self.safety.set_long_controls_allowed(True)
def test_gas_safety_check(self):
@@ -174,9 +181,9 @@ class TestGmSafety(unittest.TestCase):
for g in range(0, 2**12-1):
self.safety.set_controls_allowed(enabled)
if abs(g) > MAX_GAS or ((not enabled or not long_controls_allowed) and g != MAX_REGEN):
self.assertFalse(self.safety.gm_tx_hook(self._send_gas_msg(g)))
self.assertFalse(self.safety.safety_tx_hook(self._send_gas_msg(g)))
else:
self.assertTrue(self.safety.gm_tx_hook(self._send_gas_msg(g)))
self.assertTrue(self.safety.safety_tx_hook(self._send_gas_msg(g)))
self.safety.set_long_controls_allowed(True)
def test_steer_safety_check(self):
@@ -185,9 +192,9 @@ class TestGmSafety(unittest.TestCase):
self.safety.set_controls_allowed(enabled)
self._set_prev_torque(t)
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t)))
else:
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
def test_manually_enable_controls_allowed(self):
self.safety.set_controls_allowed(1)
@@ -200,15 +207,15 @@ class TestGmSafety(unittest.TestCase):
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
def test_non_realtime_limit_down(self):
self.safety.set_gm_torque_driver(0, 0)
@@ -222,10 +229,10 @@ class TestGmSafety(unittest.TestCase):
t *= -sign
self.safety.set_gm_torque_driver(t, t)
self._set_prev_torque(MAX_STEER * sign)
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(MAX_STEER * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
self.safety.set_gm_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(-MAX_STEER)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
# spot check some individual cases
for sign in [-1, 1]:
@@ -234,20 +241,20 @@ class TestGmSafety(unittest.TestCase):
delta = 1 * sign
self._set_prev_torque(torque_desired)
self.safety.set_gm_torque_driver(-driver_torque, -driver_torque)
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(torque_desired)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
self._set_prev_torque(torque_desired + delta)
self.safety.set_gm_torque_driver(-driver_torque, -driver_torque)
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(torque_desired + delta)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(0)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
def test_realtime_limits(self):
@@ -259,18 +266,29 @@ class TestGmSafety(unittest.TestCase):
self.safety.set_gm_torque_driver(0, 0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self._set_prev_torque(0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
self.safety.set_timer(RT_INTERVAL + 1)
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
def test_fwd_hook(self):
# nothing allowed
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
for b in buss:
for m in msgs:
# assume len 8
self.assertEqual(-1, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
if __name__ == "__main__":
panda/tests/safety/test_honda.py
+73 -38
View File
@@ -9,9 +9,17 @@ class TestHondaSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.honda_init(0)
cls.safety.safety_set_mode(1, 0)
cls.safety.init_tests_honda()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def _speed_msg(self, speed):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x158 << 21
@@ -75,85 +83,85 @@ class TestHondaSafety(unittest.TestCase):
def test_resume_button(self):
RESUME_BTN = 4
self.safety.set_controls_allowed(0)
self.safety.honda_rx_hook(self._button_msg(RESUME_BTN, 0x1A6))
self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x1A6))
self.assertTrue(self.safety.get_controls_allowed())
def test_set_button(self):
SET_BTN = 3
self.safety.set_controls_allowed(0)
self.safety.honda_rx_hook(self._button_msg(SET_BTN, 0x1A6))
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))
self.assertTrue(self.safety.get_controls_allowed())
def test_cancel_button(self):
CANCEL_BTN = 2
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6))
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6))
self.assertFalse(self.safety.get_controls_allowed())
def test_sample_speed(self):
self.assertEqual(0, self.safety.get_honda_ego_speed())
self.safety.honda_rx_hook(self._speed_msg(100))
self.safety.safety_rx_hook(self._speed_msg(100))
self.assertEqual(100, self.safety.get_honda_ego_speed())
def test_prev_brake(self):
self.assertFalse(self.safety.get_honda_brake_prev())
self.safety.honda_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertTrue(self.safety.get_honda_brake_prev())
def test_disengage_on_brake(self):
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._brake_msg(1))
self.safety.safety_rx_hook(self._brake_msg(1))
self.assertFalse(self.safety.get_controls_allowed())
def test_alt_disengage_on_brake(self):
self.safety.set_honda_alt_brake_msg(1)
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._alt_brake_msg(1))
self.safety.safety_rx_hook(self._alt_brake_msg(1))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_honda_alt_brake_msg(0)
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._alt_brake_msg(1))
self.safety.safety_rx_hook(self._alt_brake_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
def test_allow_brake_at_zero_speed(self):
# Brake was already pressed
self.safety.honda_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.honda_rx_hook(self._brake_msg(False)) # reset no brakes
self.safety.safety_rx_hook(self._brake_msg(False)) # reset no brakes
def test_not_allow_brake_when_moving(self):
# Brake was already pressed
self.safety.honda_rx_hook(self._brake_msg(True))
self.safety.honda_rx_hook(self._speed_msg(100))
self.safety.safety_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._speed_msg(100))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._brake_msg(True))
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed())
def test_prev_gas(self):
self.safety.honda_rx_hook(self._gas_msg(False))
self.safety.safety_rx_hook(self._gas_msg(False))
self.assertFalse(self.safety.get_honda_gas_prev())
self.safety.honda_rx_hook(self._gas_msg(True))
self.safety.safety_rx_hook(self._gas_msg(True))
self.assertTrue(self.safety.get_honda_gas_prev())
def test_prev_gas_interceptor(self):
self.safety.honda_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.assertFalse(self.safety.get_gas_interceptor_prev())
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_gas_interceptor_prev())
self.safety.honda_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_disengage_on_gas(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.honda_rx_hook(self._gas_msg(0))
self.safety.safety_rx_hook(self._gas_msg(0))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._gas_msg(1))
self.safety.safety_rx_hook(self._gas_msg(1))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
@@ -161,31 +169,31 @@ class TestHondaSafety(unittest.TestCase):
self.safety.set_long_controls_allowed(True)
def test_allow_engage_with_gas_pressed(self):
self.safety.honda_rx_hook(self._gas_msg(1))
self.safety.safety_rx_hook(self._gas_msg(1))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._gas_msg(1))
self.safety.safety_rx_hook(self._gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
def test_disengage_on_gas_interceptor(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.honda_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
self.assertTrue(self.safety.get_controls_allowed())
self.safety.honda_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
self.safety.set_long_controls_allowed(True)
def test_allow_engage_with_gas_interceptor_pressed(self):
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.honda_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_brake_safety_check(self):
@@ -198,7 +206,7 @@ class TestHondaSafety(unittest.TestCase):
send = MAX_BRAKE >= brake >= 0
else:
send = brake == 0
self.assertEqual(send, self.safety.honda_tx_hook(self._send_brake_msg(brake)))
self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
self.safety.set_long_controls_allowed(True)
def test_gas_interceptor_safety_check(self):
@@ -211,13 +219,13 @@ class TestHondaSafety(unittest.TestCase):
send = True
else:
send = gas == 0
self.assertEqual(send, self.safety.honda_tx_hook(self._send_interceptor_msg(gas, 0x200)))
self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200)))
self.safety.set_long_controls_allowed(True)
def test_steer_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000)))
self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000)))
self.assertTrue(self.safety.safety_tx_hook(self._send_steer_msg(0x0000)))
self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000)))
def test_spam_cancel_safety_check(self):
RESUME_BTN = 4
@@ -226,12 +234,39 @@ class TestHondaSafety(unittest.TestCase):
BUTTON_MSG = 0x296
self.safety.set_honda_bosch_hardware(1)
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.honda_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG)))
self.assertFalse(self.safety.honda_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
self.assertFalse(self.safety.honda_tx_hook(self._button_msg(SET_BTN, BUTTON_MSG)))
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG)))
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN, BUTTON_MSG)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self.safety.honda_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
long_controls_allowed = [0, 1]
self.safety.set_honda_bosch_hardware(0)
for l in long_controls_allowed:
self.safety.set_long_controls_allowed(l)
blocked_msgs = [0xE4, 0x194, 0x33D]
if l:
blocked_msgs += [0x1FA ,0x30C, 0x39F]
for b in buss:
for m in msgs:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
self.safety.set_long_controls_allowed(True)
if __name__ == "__main__":
panda/tests/safety/test_honda_bosch.py
Executable
+42
View File
@@ -0,0 +1,42 @@
#!/usr/bin/env python2
import unittest
import numpy as np
import libpandasafety_py
MAX_BRAKE = 255
class TestHondaSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.safety_set_mode(4, 0)
cls.safety.init_tests_honda()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
blocked_msgs = [0xE4, 0x33D]
for b in buss:
for m in msgs:
if b == 0:
fwd_bus = -1
elif b == 1:
fwd_bus = -1 if m in blocked_msgs else 2
elif b == 2:
fwd_bus = 1
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
if __name__ == "__main__":
unittest.main()
panda/tests/safety/test_hyundai.py
+57 -25
View File
@@ -29,9 +29,16 @@ class TestHyundaiSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.nooutput_init(0)
cls.safety.safety_set_mode(7, 0)
cls.safety.init_tests_hyundai()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def _button_msg(self, buttons):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 1265 << 21
@@ -63,9 +70,9 @@ class TestHyundaiSafety(unittest.TestCase):
self.safety.set_controls_allowed(enabled)
self._set_prev_torque(t)
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t)))
else:
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
def test_manually_enable_controls_allowed(self):
self.safety.set_controls_allowed(1)
@@ -78,7 +85,7 @@ class TestHyundaiSafety(unittest.TestCase):
to_push[0].RIR = 1057 << 21
to_push[0].RDLR = 1 << 13
self.safety.hyundai_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertTrue(self.safety.get_controls_allowed())
def test_disable_control_allowed_from_cruise(self):
@@ -87,7 +94,7 @@ class TestHyundaiSafety(unittest.TestCase):
to_push[0].RDLR = 0
self.safety.set_controls_allowed(1)
self.safety.hyundai_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertFalse(self.safety.get_controls_allowed())
def test_non_realtime_limit_up(self):
@@ -95,15 +102,15 @@ class TestHyundaiSafety(unittest.TestCase):
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
def test_non_realtime_limit_down(self):
self.safety.set_hyundai_torque_driver(0, 0)
@@ -117,10 +124,10 @@ class TestHyundaiSafety(unittest.TestCase):
t *= -sign
self.safety.set_hyundai_torque_driver(t, t)
self._set_prev_torque(MAX_STEER * sign)
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(MAX_STEER * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
self.safety.set_hyundai_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_STEER)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
# spot check some individual cases
for sign in [-1, 1]:
@@ -129,20 +136,20 @@ class TestHyundaiSafety(unittest.TestCase):
delta = 1 * sign
self._set_prev_torque(torque_desired)
self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque)
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(torque_desired)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
self._set_prev_torque(torque_desired + delta)
self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque)
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(torque_desired + delta)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(0)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
def test_realtime_limits(self):
@@ -154,18 +161,18 @@ class TestHyundaiSafety(unittest.TestCase):
self.safety.set_hyundai_torque_driver(0, 0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self._set_prev_torque(0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
self.safety.set_timer(RT_INTERVAL + 1)
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
#def test_spam_cancel_safety_check(self):
@@ -174,12 +181,37 @@ class TestHyundaiSafety(unittest.TestCase):
# CANCEL_BTN = 4
# BUTTON_MSG = 1265
# self.safety.set_controls_allowed(0)
# self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(CANCEL_BTN)))
# self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN)))
# self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(SET_BTN)))
# self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN)))
# self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN)))
# self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN)))
# # do not block resume if we are engaged already
# self.safety.set_controls_allowed(1)
# self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN)))
# self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN)))
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
hyundai_giraffe_switch_2 = [0, 1]
self.safety.set_hyundai_camera_bus(2)
for hgs in hyundai_giraffe_switch_2:
self.safety.set_hyundai_giraffe_switch_2(hgs)
blocked_msgs = [832]
for b in buss:
for m in msgs:
if hgs:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0
else:
fwd_bus = -1
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
if __name__ == "__main__":
panda/tests/safety/test_subaru.py
+45 -21
View File
@@ -29,9 +29,16 @@ class TestSubaruSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.nooutput_init(0)
cls.safety.safety_set_mode(10, 0)
cls.safety.init_tests_subaru()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def _set_prev_torque(self, t):
self.safety.set_subaru_desired_torque_last(t)
self.safety.set_subaru_rt_torque_last(t)
@@ -60,7 +67,7 @@ class TestSubaruSafety(unittest.TestCase):
to_push[0].RIR = 0x240 << 21
to_push[0].RDHR = 1 << 9
self.safety.subaru_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertTrue(self.safety.get_controls_allowed())
def test_disable_control_allowed_from_cruise(self):
@@ -69,7 +76,7 @@ class TestSubaruSafety(unittest.TestCase):
to_push[0].RDHR = 0
self.safety.set_controls_allowed(1)
self.safety.subaru_rx_hook(to_push)
self.safety.safety_rx_hook(to_push)
self.assertFalse(self.safety.get_controls_allowed())
def test_steer_safety_check(self):
@@ -78,9 +85,9 @@ class TestSubaruSafety(unittest.TestCase):
self.safety.set_controls_allowed(enabled)
self._set_prev_torque(t)
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t)))
else:
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
def test_manually_enable_controls_allowed(self):
self.safety.set_controls_allowed(1)
@@ -93,15 +100,15 @@ class TestSubaruSafety(unittest.TestCase):
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
def test_non_realtime_limit_down(self):
self.safety.set_subaru_torque_driver(0, 0)
@@ -115,10 +122,10 @@ class TestSubaruSafety(unittest.TestCase):
t *= -sign
self.safety.set_subaru_torque_driver(t, t)
self._set_prev_torque(MAX_STEER * sign)
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(MAX_STEER * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
self.safety.set_subaru_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(-MAX_STEER)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
# spot check some individual cases
for sign in [-1, 1]:
@@ -127,20 +134,20 @@ class TestSubaruSafety(unittest.TestCase):
delta = 1 * sign
self._set_prev_torque(torque_desired)
self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque)
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(torque_desired)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
self._set_prev_torque(torque_desired + delta)
self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque)
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(torque_desired + delta)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(0)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
self._set_prev_torque(MAX_STEER * sign)
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
def test_realtime_limits(self):
@@ -152,18 +159,35 @@ class TestSubaruSafety(unittest.TestCase):
self.safety.set_subaru_torque_driver(0, 0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self._set_prev_torque(0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
self.safety.set_timer(RT_INTERVAL + 1)
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
blocked_msgs = [290, 356, 545, 802]
for b in buss:
for m in msgs:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
if __name__ == "__main__":
panda/tests/safety/test_toyota.py
+90 -274
View File
@@ -15,12 +15,6 @@ RT_INTERVAL = 250000
MAX_TORQUE_ERROR = 350
IPAS_OVERRIDE_THRESHOLD = 200
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
def twos_comp(val, bits):
if val >= 0:
return val
@@ -37,9 +31,16 @@ class TestToyotaSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.toyota_init(100)
cls.safety.safety_set_mode(2, 100)
cls.safety.init_tests_toyota()
def _send_msg(self, bus, addr, length):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = length
to_send[0].RDTR = bus << 4
return to_send
def _set_prev_torque(self, t):
self.safety.set_toyota_desired_torque_last(t)
self.safety.set_toyota_rt_torque_last(t)
@@ -53,30 +54,6 @@ class TestToyotaSafety(unittest.TestCase):
to_send[0].RDHR = t | ((t & 0xFF) << 16)
return to_send
def _torque_driver_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x260 << 21
t = twos_comp(torque, 16)
to_send[0].RDLR = t | ((t & 0xFF) << 16)
return to_send
def _torque_driver_msg_array(self, torque):
for i in range(6):
self.safety.toyota_ipas_rx_hook(self._torque_driver_msg(torque))
def _angle_meas_msg(self, angle):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x25 << 21
t = twos_comp(angle, 12)
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
return to_send
def _angle_meas_msg_array(self, angle):
for i in range(6):
self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(angle))
def _torque_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x2E4 << 21
@@ -85,32 +62,6 @@ class TestToyotaSafety(unittest.TestCase):
to_send[0].RDLR = t | ((t & 0xFF) << 16)
return to_send
def _ipas_state_msg(self, state):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x262 << 21
to_send[0].RDLR = state & 0xF
return to_send
def _ipas_control_msg(self, angle, state):
# note: we command 2/3 of the angle due to CAN conversion
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x266 << 21
t = twos_comp(angle, 12)
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
to_send[0].RDLR |= ((state & 0xf) << 4)
return to_send
def _speed_msg(self, speed):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0xb4 << 21
speed = int(speed * 100 * 3.6)
to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00)
return to_send
def _accel_msg(self, accel):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x343 << 21
@@ -121,8 +72,8 @@ class TestToyotaSafety(unittest.TestCase):
def _send_gas_msg(self, gas):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x200 << 21
to_send[0].RDLR = gas
to_send[0].RIR = 0x2C1 << 21
to_send[0].RDHR = (gas & 0xFF) << 16
return to_send
@@ -134,10 +85,10 @@ class TestToyotaSafety(unittest.TestCase):
return to_send
def _pcm_cruise_msg(self, cruise_on, gas_pressed):
def _pcm_cruise_msg(self, cruise_on):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x1D2 << 21
to_send[0].RDLR = (cruise_on << 5) | ((not gas_pressed) << 4 )
to_send[0].RDLR = cruise_on << 5
return to_send
@@ -149,37 +100,35 @@ class TestToyotaSafety(unittest.TestCase):
self.assertTrue(self.safety.get_controls_allowed())
def test_enable_control_allowed_from_cruise(self):
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.safety.safety_rx_hook(self._pcm_cruise_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, False))
self.safety.safety_rx_hook(self._pcm_cruise_msg(True))
self.assertTrue(self.safety.get_controls_allowed())
def test_disable_control_allowed_from_cruise(self):
self.safety.set_controls_allowed(1)
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.safety.safety_rx_hook(self._pcm_cruise_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
def test_prev_gas(self):
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.assertFalse(self.safety.get_toyota_gas_prev())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, True))
self.assertTrue(self.safety.get_toyota_gas_prev())
for g in range(0, 256):
self.safety.safety_rx_hook(self._send_gas_msg(g))
self.assertEqual(g, self.safety.get_toyota_gas_prev())
def test_prev_gas_interceptor(self):
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.assertFalse(self.safety.get_gas_interceptor_prev())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_gas_interceptor_prev())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_disengage_on_gas(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, False))
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, False))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, True))
self.safety.safety_rx_hook(self._send_gas_msg(0))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_gas_msg(1))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
@@ -187,32 +136,33 @@ class TestToyotaSafety(unittest.TestCase):
self.safety.set_long_controls_allowed(True)
def test_allow_engage_with_gas_pressed(self):
self.safety.toyota_rx_hook(self._pcm_cruise_msg(False, True))
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, True))
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._pcm_cruise_msg(True, True))
self.safety.safety_rx_hook(self._send_gas_msg(1))
self.assertTrue(self.safety.get_controls_allowed())
def test_disengage_on_gas_interceptor(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
self.safety.toyota_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(1)
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_controls_allowed(True)
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
if long_controls_allowed:
self.assertFalse(self.safety.get_controls_allowed())
else:
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
self.safety.set_long_controls_allowed(True)
def test_allow_engage_with_gas_interceptor_pressed(self):
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.set_controls_allowed(1)
self.safety.toyota_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
self.safety.set_gas_interceptor_detected(False)
def test_accel_actuation_limits(self):
@@ -225,7 +175,7 @@ class TestToyotaSafety(unittest.TestCase):
send = MIN_ACCEL <= accel <= MAX_ACCEL
else:
send = accel == 0
self.assertEqual(send, self.safety.toyota_tx_hook(self._accel_msg(accel)))
self.assertEqual(send, self.safety.safety_tx_hook(self._accel_msg(accel)))
self.safety.set_long_controls_allowed(True)
def test_torque_absolute_limits(self):
@@ -241,16 +191,16 @@ class TestToyotaSafety(unittest.TestCase):
else:
send = torque == 0
self.assertEqual(send, self.safety.toyota_tx_hook(self._torque_msg(torque)))
self.assertEqual(send, self.safety.safety_tx_hook(self._torque_msg(torque)))
def test_non_realtime_limit_up(self):
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
def test_non_realtime_limit_down(self):
self.safety.set_controls_allowed(True)
@@ -258,12 +208,12 @@ class TestToyotaSafety(unittest.TestCase):
self.safety.set_toyota_rt_torque_last(1000)
self.safety.set_toyota_torque_meas(500, 500)
self.safety.set_toyota_desired_torque_last(1000)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN)))
self.safety.set_toyota_rt_torque_last(1000)
self.safety.set_toyota_torque_meas(500, 500)
self.safety.set_toyota_desired_torque_last(1000)
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1)))
def test_exceed_torque_sensor(self):
self.safety.set_controls_allowed(True)
@@ -272,9 +222,9 @@ class TestToyotaSafety(unittest.TestCase):
self._set_prev_torque(0)
for t in np.arange(0, MAX_TORQUE_ERROR + 10, 10):
t *= sign
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10))))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10))))
def test_realtime_limit_up(self):
self.safety.set_controls_allowed(True)
@@ -285,211 +235,77 @@ class TestToyotaSafety(unittest.TestCase):
for t in np.arange(0, 380, 10):
t *= sign
self.safety.set_toyota_torque_meas(t, t)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * 380)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * 380)))
self._set_prev_torque(0)
for t in np.arange(0, 370, 10):
t *= sign
self.safety.set_toyota_torque_meas(t, t)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
self.safety.set_timer(RT_INTERVAL + 1)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(sign * 370)))
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(sign * 380)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * 370)))
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * 380)))
def test_torque_measurements(self):
self.safety.toyota_rx_hook(self._torque_meas_msg(50))
self.safety.toyota_rx_hook(self._torque_meas_msg(-50))
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(50))
self.safety.safety_rx_hook(self._torque_meas_msg(-50))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.assertEqual(-51, self.safety.get_toyota_torque_meas_min())
self.assertEqual(51, self.safety.get_toyota_torque_meas_max())
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.assertEqual(1, self.safety.get_toyota_torque_meas_max())
self.assertEqual(-51, self.safety.get_toyota_torque_meas_min())
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.safety.safety_rx_hook(self._torque_meas_msg(0))
self.assertEqual(1, self.safety.get_toyota_torque_meas_max())
self.assertEqual(-1, self.safety.get_toyota_torque_meas_min())
def test_ipas_override(self):
## angle control is not active
self.safety.set_controls_allowed(1)
# 3 consecutive msgs where driver exceeds threshold but angle_control isn't active
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
self.assertTrue(self.safety.get_controls_allowed())
# ipas state is override
self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(5))
self.assertTrue(self.safety.get_controls_allowed())
## now angle control is active
self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 0))
self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(0))
# 3 consecutive msgs where driver does exceed threshold
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
self.assertFalse(self.safety.get_controls_allowed())
# ipas state is override and torque isn't overriding any more
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(0)
self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(5))
self.assertFalse(self.safety.get_controls_allowed())
# 3 consecutive msgs where driver does not exceed threshold and
# ipas state is not override
self.safety.set_controls_allowed(1)
self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(0))
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD)
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD)
self.assertTrue(self.safety.get_controls_allowed())
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_when_disabled(self):
self.safety.set_controls_allowed(0)
# test angle cmd too far from actual
angle_refs = [-10, 10]
deltas = range(-2, 3)
expected_results = [False, True, True, True, False]
for a in angle_refs:
self._angle_meas_msg_array(a)
for i, d in enumerate(deltas):
self.assertEqual(expected_results[i], self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + d, 1)))
# test ipas state cmd enabled
self._angle_meas_msg_array(0)
self.assertEqual(0, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 3)))
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_when_enabled(self):
# ipas angle cmd should pass through when controls are enabled
self.safety.set_controls_allowed(1)
self._angle_meas_msg_array(0)
self.safety.toyota_ipas_rx_hook(self._speed_msg(0.1))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 1)))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(4, 1)))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 3)))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-4, 3)))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-8, 3)))
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_rate_when_disabled(self):
# as long as the command is close to the measured, no rate limit is enforced when
# controls are disabled
self.safety.set_controls_allowed(0)
self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(0))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 1)))
self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(100))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(100, 1)))
self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(-100))
self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-100, 1)))
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_rate_when_enabled(self):
# when controls are allowed, angle cmd rate limit is enforced
# test 1: no limitations if we stay within limits
speeds = [0., 1., 5., 10., 15., 100.]
angles = [-300, -100, -10, 0, 10, 100, 300]
for a in angles:
for s in speeds:
# first test against false positives
self._angle_meas_msg_array(a)
self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1))
self.safety.set_controls_allowed(1)
self.safety.toyota_ipas_rx_hook(self._speed_msg(s))
max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1)))
self.assertTrue(self.safety.get_controls_allowed())
# now inject too high rates
self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) *
(max_delta_up + 1), 1)))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_controls_allowed(1)
self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) *
(max_delta_down + 1), 1)))
self.assertFalse(self.safety.get_controls_allowed())
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_measured_rate(self):
speeds = [0., 1., 5., 10., 15., 100.]
angles = [-300, -100, -10, 0, 10, 100, 300]
angles = [10]
for a in angles:
for s in speeds:
self._angle_meas_msg_array(a)
self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1))
self.safety.set_controls_allowed(1)
self.safety.toyota_ipas_rx_hook(self._speed_msg(s))
max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(a))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(a + 150))
self.assertFalse(self.safety.get_controls_allowed())
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_gas_interceptor_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.toyota_tx_hook(self._send_interceptor_msg(0, 0x200)))
self.assertFalse(self.safety.toyota_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0, 0x200)))
self.assertFalse(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
self.safety.set_controls_allowed(1)
self.assertTrue(self.safety.toyota_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
long_controls_allowed = [0, 1]
toyota_camera_forwarded = [0, 1]
for tcf in toyota_camera_forwarded:
self.safety.set_toyota_camera_forwarded(tcf)
for lca in long_controls_allowed:
self.safety.set_long_controls_allowed(lca)
blocked_msgs = [0x2E4, 0x412, 0x191]
if lca:
blocked_msgs += [0x343]
for b in buss:
for m in msgs:
if tcf:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0
else:
fwd_bus = -1
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
self.safety.set_long_controls_allowed(True)
if __name__ == "__main__":
panda/tests/safety/test_toyota_ipas.py
+247
View File
@@ -0,0 +1,247 @@
#!/usr/bin/env python2
import unittest
import numpy as np
import libpandasafety_py
IPAS_OVERRIDE_THRESHOLD = 200
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
def twos_comp(val, bits):
if val >= 0:
return val
else:
return (2**bits) + val
def sign(a):
if a > 0:
return 1
else:
return -1
class TestToyotaSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.safety_set_mode(0x1335, 66)
cls.safety.init_tests_toyota()
def _torque_driver_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x260 << 21
t = twos_comp(torque, 16)
to_send[0].RDLR = t | ((t & 0xFF) << 16)
return to_send
def _torque_driver_msg_array(self, torque):
for i in range(6):
self.safety.safety_rx_hook(self._torque_driver_msg(torque))
def _angle_meas_msg(self, angle):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x25 << 21
t = twos_comp(angle, 12)
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
return to_send
def _angle_meas_msg_array(self, angle):
for i in range(6):
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
def _ipas_state_msg(self, state):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x262 << 21
to_send[0].RDLR = state & 0xF
return to_send
def _ipas_control_msg(self, angle, state):
# note: we command 2/3 of the angle due to CAN conversion
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x266 << 21
t = twos_comp(angle, 12)
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
to_send[0].RDLR |= ((state & 0xf) << 4)
return to_send
def _speed_msg(self, speed):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0xb4 << 21
speed = int(speed * 100 * 3.6)
to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00)
return to_send
def test_ipas_override(self):
## angle control is not active
self.safety.set_controls_allowed(1)
# 3 consecutive msgs where driver exceeds threshold but angle_control isn't active
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
self.assertTrue(self.safety.get_controls_allowed())
# ipas state is override
self.safety.safety_rx_hook(self._ipas_state_msg(5))
self.assertTrue(self.safety.get_controls_allowed())
## now angle control is active
self.safety.safety_tx_hook(self._ipas_control_msg(0, 0))
self.safety.safety_rx_hook(self._ipas_state_msg(0))
# 3 consecutive msgs where driver does exceed threshold
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
self.assertFalse(self.safety.get_controls_allowed())
# ipas state is override and torque isn't overriding any more
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(0)
self.safety.safety_rx_hook(self._ipas_state_msg(5))
self.assertFalse(self.safety.get_controls_allowed())
# 3 consecutive msgs where driver does not exceed threshold and
# ipas state is not override
self.safety.set_controls_allowed(1)
self.safety.safety_rx_hook(self._ipas_state_msg(0))
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD)
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD)
self.assertTrue(self.safety.get_controls_allowed())
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_when_disabled(self):
self.safety.set_controls_allowed(0)
# test angle cmd too far from actual
angle_refs = [-10, 10]
deltas = range(-2, 3)
expected_results = [False, True, True, True, False]
for a in angle_refs:
self._angle_meas_msg_array(a)
for i, d in enumerate(deltas):
self.assertEqual(expected_results[i], self.safety.safety_tx_hook(self._ipas_control_msg(a + d, 1)))
# test ipas state cmd enabled
self._angle_meas_msg_array(0)
self.assertEqual(0, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3)))
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_when_enabled(self):
# ipas angle cmd should pass through when controls are enabled
self.safety.set_controls_allowed(1)
self._angle_meas_msg_array(0)
self.safety.safety_rx_hook(self._speed_msg(0.1))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1)))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(4, 1)))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3)))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-4, 3)))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-8, 3)))
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_rate_when_disabled(self):
# as long as the command is close to the measured, no rate limit is enforced when
# controls are disabled
self.safety.set_controls_allowed(0)
self.safety.safety_rx_hook(self._angle_meas_msg(0))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1)))
self.safety.safety_rx_hook(self._angle_meas_msg(100))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(100, 1)))
self.safety.safety_rx_hook(self._angle_meas_msg(-100))
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-100, 1)))
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_cmd_rate_when_enabled(self):
# when controls are allowed, angle cmd rate limit is enforced
# test 1: no limitations if we stay within limits
speeds = [0., 1., 5., 10., 15., 100.]
angles = [-300, -100, -10, 0, 10, 100, 300]
for a in angles:
for s in speeds:
# first test against false positives
self._angle_meas_msg_array(a)
self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))
self.safety.set_controls_allowed(1)
self.safety.safety_rx_hook(self._speed_msg(s))
max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1)))
self.assertTrue(self.safety.get_controls_allowed())
# now inject too high rates
self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) *
(max_delta_up + 1), 1)))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_controls_allowed(1)
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) *
(max_delta_down + 1), 1)))
self.assertFalse(self.safety.get_controls_allowed())
# reset no angle control at the end of the test
self.safety.reset_angle_control()
def test_angle_measured_rate(self):
speeds = [0., 1., 5., 10., 15., 100.]
angles = [-300, -100, -10, 0, 10, 100, 300]
angles = [10]
for a in angles:
for s in speeds:
self._angle_meas_msg_array(a)
self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))
self.safety.set_controls_allowed(1)
self.safety.safety_rx_hook(self._speed_msg(s))
max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
self.safety.safety_rx_hook(self._angle_meas_msg(a))
self.assertTrue(self.safety.get_controls_allowed())
self.safety.safety_rx_hook(self._angle_meas_msg(a + 150))
self.assertFalse(self.safety.get_controls_allowed())
# reset no angle control at the end of the test
self.safety.reset_angle_control()
if __name__ == "__main__":
unittest.main()
panda/tests/safety/trim_csv.py
-21
View File
@@ -1,21 +0,0 @@
#!/usr/bin/env python2
# This trims CAN message CSV files to just the messages relevant for Panda testing.
# Usage:
# cat input.csv | ./trim_csv.py > output.csv
import fileinput
addr_to_keep = [544, 0x1f4, 0x292] # For Chrysler, update to the addresses that matter for you.
for line in fileinput.input():
line = line.strip()
cols = line.split(',')
if len(cols) != 4:
continue # malformed, such as at the end or every 60s.
(_, addr, bus, _) = cols
if (addr == 'addr'):
continue
if (int(bus) == 128): # Keep all messages sent by OpenPilot.
print line
elif (int(addr) in addr_to_keep):
print line
panda/tests/safety_replay/Dockerfile
+20
View File
@@ -0,0 +1,20 @@
FROM ubuntu:16.04
RUN apt-get update && apt-get install -y make clang python python-pip git libarchive-dev libusb-1.0-0
COPY tests/safety_replay/requirements.txt requirements.txt
RUN pip install -r requirements.txt
COPY tests/safety_replay/install_capnp.sh install_capnp.sh
RUN ./install_capnp.sh
RUN mkdir /openpilot
WORKDIR /openpilot
RUN git clone https://github.com/commaai/cereal.git || true
WORKDIR /openpilot/cereal
RUN git checkout f7043fde062cbfd49ec90af669901a9caba52de9
COPY . /openpilot/panda
WORKDIR /openpilot/panda/tests/safety_replay
RUN git clone https://github.com/commaai/openpilot-tools.git tools || true
WORKDIR tools
RUN git checkout b6461274d684915f39dc45efc5292ea890698da9
pyextra/jsonrpc/backend/__init__.py → panda/tests/safety_replay/__init__.py
View File
panda/tests/safety_replay/helpers.py
+104
View File
@@ -0,0 +1,104 @@
import struct
import panda.tests.safety.libpandasafety_py as libpandasafety_py
safety_modes = {
"NOOUTPUT": 0,
"HONDA": 1,
"TOYOTA": 2,
"GM": 3,
"HONDA_BOSCH": 4,
"FORD": 5,
"CADILLAC": 6,
"HYUNDAI": 7,
"TESLA": 8,
"CHRYSLER": 9,
"SUBARU": 10,
"GM_ASCM": 0x1334,
"TOYOTA_IPAS": 0x1335,
"ALLOUTPUT": 0x1337,
"ELM327": 0xE327
}
def to_signed(d, bits):
ret = d
if d >= (1 << (bits - 1)):
ret = d - (1 << bits)
return ret
def is_steering_msg(mode, addr):
ret = False
if mode == safety_modes["HONDA"] or mode == safety_modes["HONDA_BOSCH"]:
ret = (addr == 0xE4) or (addr == 0x194) or (addr == 0x33D)
elif mode == safety_modes["TOYOTA"]:
ret = addr == 0x2E4
elif mode == safety_modes["GM"]:
ret = addr == 384
elif mode == safety_modes["HYUNDAI"]:
ret = addr == 832
elif mode == safety_modes["CHRYSLER"]:
ret = addr == 0x292
elif mode == safety_modes["SUBARU"]:
ret = addr == 0x122
return ret
def get_steer_torque(mode, to_send):
ret = 0
if mode == safety_modes["HONDA"] or mode == safety_modes["HONDA_BOSCH"]:
ret = to_send.RDLR & 0xFFFF0000
elif mode == safety_modes["TOYOTA"]:
ret = (to_send.RDLR & 0xFF00) | ((to_send.RDLR >> 16) & 0xFF)
ret = to_signed(ret, 16)
elif mode == safety_modes["GM"]:
ret = ((to_send.RDLR & 0x7) << 8) + ((to_send.RDLR & 0xFF00) >> 8)
ret = to_signed(ret, 11)
elif mode == safety_modes["HYUNDAI"]:
ret = ((to_send.RDLR >> 16) & 0x7ff) - 1024
elif mode == safety_modes["CHRYSLER"]:
ret = ((to_send.RDLR & 0x7) << 8) + ((to_send.RDLR & 0xFF00) >> 8) - 1024
elif mode == safety_modes["SUBARU"]:
ret = ((to_send.RDLR >> 16) & 0x1FFF)
ret = to_signed(ret, 13)
return ret
def set_desired_torque_last(safety, mode, torque):
if mode == safety_modes["HONDA"] or mode == safety_modes["HONDA_BOSCH"]:
pass # honda safety mode doesn't enforce a rate on steering msgs
elif mode == safety_modes["TOYOTA"]:
safety.set_toyota_desired_torque_last(torque)
elif mode == safety_modes["GM"]:
safety.set_gm_desired_torque_last(torque)
elif mode == safety_modes["HYUNDAI"]:
safety.set_hyundai_desired_torque_last(torque)
elif mode == safety_modes["CHRYSLER"]:
safety.set_chrysler_desired_torque_last(torque)
elif mode == safety_modes["SUBARU"]:
safety.set_subaru_desired_torque_last(torque)
def package_can_msg(msg):
addr_shift = 3 if msg.address >= 0x800 else 21
rdlr, rdhr = struct.unpack('II', msg.dat.ljust(8, b'\x00'))
ret = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
ret[0].RIR = msg.address << addr_shift
ret[0].RDTR = len(msg.dat) | ((msg.src & 0xF) << 4)
ret[0].RDHR = rdhr
ret[0].RDLR = rdlr
return ret
def init_segment(safety, lr, mode):
sendcan = (msg for msg in lr if msg.which() == 'sendcan')
steering_msgs = (can for msg in sendcan for can in msg.sendcan if is_steering_msg(mode, can.address))
msg = next(steering_msgs, None)
if msg is None:
# no steering msgs
return
to_send = package_can_msg(msg)
torque = get_steer_torque(mode, to_send)
if torque != 0:
safety.set_controls_allowed(1)
set_desired_torque_last(safety, mode, torque)
assert safety.safety_tx_hook(to_send), "failed to initialize panda safety for segment"
panda/tests/safety_replay/install_capnp.sh
Executable
+20
View File
@@ -0,0 +1,20 @@
#!/bin/bash -e
apt-get install -y autoconf curl libtool
curl -O https://capnproto.org/capnproto-c++-0.6.1.tar.gz
tar xvf capnproto-c++-0.6.1.tar.gz
cd capnproto-c++-0.6.1
./configure --prefix=/usr/local CPPFLAGS=-DPIC CFLAGS=-fPIC CXXFLAGS=-fPIC LDFLAGS=-fPIC --disable-shared --enable-static
make -j4
make install
cd ..
git clone https://github.com/commaai/c-capnproto.git
cd c-capnproto
git checkout 2e625acacf58a5f5c8828d8453d1f8dacc700a96
git submodule update --init --recursive
autoreconf -f -i -s
CFLAGS="-fPIC" ./configure --prefix=/usr/local
make -j4
make install
panda/tests/safety_replay/replay_drive.py
Executable
+70
View File
@@ -0,0 +1,70 @@
#!/usr/bin/env python2
import os
import sys
import panda.tests.safety.libpandasafety_py as libpandasafety_py
from panda.tests.safety_replay.helpers import is_steering_msg, get_steer_torque, \
set_desired_torque_last, package_can_msg, \
init_segment, safety_modes
from tools.lib.logreader import LogReader
# replay a drive to check for safety violations
def replay_drive(lr, safety_mode, param):
safety = libpandasafety_py.libpandasafety
err = safety.safety_set_mode(safety_mode, param)
assert err == 0, "invalid safety mode: %d" % safety_mode
if "SEGMENT" in os.environ:
init_segment(safety, lr, mode)
tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0
blocked_addrs = set()
start_t = None
for msg in lr:
if start_t is None:
start_t = msg.logMonoTime
safety.set_timer(((msg.logMonoTime / 1000)) % 0xFFFFFFFF)
if msg.which() == 'sendcan':
for canmsg in msg.sendcan:
to_send = package_can_msg(canmsg)
sent = safety.safety_tx_hook(to_send)
if not sent:
tx_blocked += 1
tx_controls_blocked += safety.get_controls_allowed()
blocked_addrs.add(canmsg.address)
if "DEBUG" in os.environ:
print "blocked %d at %f" % (canmsg.address, (msg.logMonoTime - start_t)/(1e9))
tx_controls += safety.get_controls_allowed()
tx_tot += 1
elif msg.which() == 'can':
for canmsg in msg.can:
# ignore msgs we sent
if canmsg.src >= 128:
continue
to_push = package_can_msg(canmsg)
safety.safety_rx_hook(to_push)
print "total openpilot msgs:", tx_tot
print "total msgs with controls allowed:", tx_controls
print "blocked msgs:", tx_blocked
print "blocked with controls allowed:", tx_controls_blocked
print "blocked addrs:", blocked_addrs
return tx_controls_blocked == 0
if __name__ == "__main__":
if sys.argv[2] in safety_modes:
mode = safety_modes[sys.argv[2]]
else:
mode = int(sys.argv[2])
param = 0 if len(sys.argv) < 4 else int(sys.argv[3])
lr = LogReader(sys.argv[1])
print "replaying drive %s with safety mode %d and param %d" % (sys.argv[1], mode, param)
replay_drive(lr, mode, param)
panda/tests/safety_replay/requirements.txt
+8
View File
@@ -0,0 +1,8 @@
aenum
cffi==1.11.4
libusb1==1.6.6
numpy==1.14.5
requests
subprocess32
libarchive
pycapnp
panda/tests/safety_replay/test_safety_replay.py
Executable
+41
View File
@@ -0,0 +1,41 @@
#!/usr/bin/env python2
import os
import requests
from helpers import safety_modes
from replay_drive import replay_drive
from tools.lib.logreader import LogReader
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
# (route, safety mode, param)
logs = [
("b0c9d2329ad1606b|2019-05-30--20-23-57.bz2", "HONDA", 0), # HONDA.CIVIC
("38bfd238edecbcd7|2019-06-07--10-15-25.bz2", "TOYOTA", 66), # TOYOTA.PRIUS
("f89c604cf653e2bf|2018-09-29--13-46-50.bz2", "GM", 0), # GM.VOLT
("0375fdf7b1ce594d|2019-05-21--20-10-33.bz2", "HONDA_BOSCH", 1), # HONDA.ACCORD
("02ec6bea180a4d36|2019-04-17--11-21-35.bz2", "HYUNDAI", 0), # HYUNDAI.SANTA_FE
("03efb1fda29e30fe|2019-02-21--18-03-45.bz2", "CHRYSLER", 0), # CHRYSLER.PACIFICA_2018_HYBRID
("791340bc01ed993d|2019-04-08--10-26-00.bz2", "SUBARU", 0), # SUBARU.IMPREZA
]
if __name__ == "__main__":
for route, _, _ in logs:
if not os.path.isfile(route):
with open(route, "w") as f:
f.write(requests.get(BASE_URL + route).content)
failed = []
for route, mode, param in logs:
lr = LogReader(route)
m = safety_modes.get(mode, mode)
print "\nreplaying %s with safety mode %d and param %s" % (route, m, param)
if not replay_drive(lr, m, int(param)):
failed.append(route)
for f in failed:
print "\n**** failed on %s ****" % f
assert len(failed) == 0, "\nfailed on %d logs" % len(failed)

Some files were not shown because too many files have changed in this diff Show More