2253dd3 fix volt ign detect
3b299d7 add ignition and refactor
af9af6d Merge pull request #110 from Jamezz/volt
13e850e more correct
f295063 add new define to tests
fec9758 gate that with debug
5516ebf one more ifdef
cac7b31 only panda has float
938d474 fpu enable
ffbf0c7 cleaner
de30f27 Revert "need f to not be double"
4142acf need f to not be double
3eb15c8 refactor to share code
a4c8b64 change to O2 to fix make recover
711fd11 Enable compiler optimizations, fix things it breaks
2e6f774 block IPAS in main toyota safety mode
e7a2b3a add ipas tests
894572c fix tests
367c9ad add safety toyota ipas
95919b9 Bounty: panda high quality CAN autobaud (#96)
6557cd2 Toyota Safety: allow controls only on rising edge of cruise_engaged
02c1ddf Revert "added steer override check when IPAS is in control (#106)"
9f925ba Fix the merge mess
23d3833 Merge from comma upstream
a0cc51a Undo safety mode override
ea1c1dc make wlan interface name generic
6dbd8c9 Implement WebUSB and upgrade WinUSB to 2.0 (#107)
4fc83a5 Add safety hook for ignition and have GM use gear selector to determine ignition
52b2ac0 switch from travis to circleci
48e2374 build panda esp image
065572a circleci build stm image
7a1f319 add panda python package test and fix safety test
021dde7 move saftey test helper files into safety folder
ce0545f add ci files
6a3307c no LIN over ELM
7d21acb added steer override check when IPAS is in control (#106)
1c88caf Safety code testing (#104)
f4efd1f Merge pull request #101 from adhintz/master
c02618b Merge pull request #102 from quillford/master
1ba5f8a added link to wiki for user scripts
de2b19e add support for multiple buses to can_unique and can_bittransition output data in sorted order.
git-subtree-dir: panda
git-subtree-split: 2253dd3c48e21abb82fe161d6f58237490111206
pull/236/head
Vehicle Researcher
7 years ago
parent
a8d110ad74
commit
e6e6ad2e1f
34 changed files with 1502 additions and 135 deletions
@ -0,0 +1,41 @@ |
|||||||
|
version: 2 |
||||||
|
jobs: |
||||||
|
safety: |
||||||
|
machine: |
||||||
|
docker_layer_caching: true |
||||||
|
steps: |
||||||
|
- checkout |
||||||
|
- run: |
||||||
|
name: Build image |
||||||
|
command: "docker build -t panda_safety -f tests/safety/Dockerfile ." |
||||||
|
- run: |
||||||
|
name: Run safety test |
||||||
|
command: | |
||||||
|
docker run panda_safety /bin/bash -c "cd /panda/tests/safety; ./test.sh" |
||||||
|
build: |
||||||
|
machine: |
||||||
|
docker_layer_caching: true |
||||||
|
steps: |
||||||
|
- checkout |
||||||
|
- run: |
||||||
|
name: Build image |
||||||
|
command: "docker build -t panda_build -f tests/build/Dockerfile ." |
||||||
|
- run: |
||||||
|
name: Test python package installer |
||||||
|
command: | |
||||||
|
docker run panda_build /bin/bash -c "cd /panda; python setup.py install" |
||||||
|
- run: |
||||||
|
name: Build STM image |
||||||
|
command: | |
||||||
|
docker run panda_build /bin/bash -c "cd /panda/board; make bin" |
||||||
|
- run: |
||||||
|
name: Build ESP image |
||||||
|
command: | |
||||||
|
docker run panda_build /bin/bash -c "cd /panda/boardesp; make user1.bin" |
||||||
|
|
||||||
|
workflows: |
||||||
|
version: 2 |
||||||
|
main: |
||||||
|
jobs: |
||||||
|
- safety |
||||||
|
- build |
||||||
@ -1,20 +0,0 @@ |
|||||||
language: python |
|
||||||
|
|
||||||
cache: |
|
||||||
directories: |
|
||||||
- build/commaai/panda/boardesp/esp-open-sdk/crosstool-NG |
|
||||||
|
|
||||||
addons: |
|
||||||
apt: |
|
||||||
packages: |
|
||||||
- gcc-arm-none-eabi |
|
||||||
- libnewlib-arm-none-eabi |
|
||||||
- gperf |
|
||||||
- texinfo |
|
||||||
- help2man |
|
||||||
|
|
||||||
script: |
|
||||||
- python setup.py install |
|
||||||
- pushd board && make bin && popd |
|
||||||
- pushd boardesp && git clone --recursive https://github.com/pfalcon/esp-open-sdk.git && pushd esp-open-sdk && git checkout 03f5e898a059451ec5f3de30e7feff30455f7cec && LD_LIBRARY_PATH="" make STANDALONE=y && popd && popd |
|
||||||
- pushd boardesp && make user1.bin && popd |
|
||||||
@ -0,0 +1,193 @@ |
|||||||
|
// uses tons from safety_toyota
|
||||||
|
// TODO: refactor to repeat less code
|
||||||
|
|
||||||
|
// IPAS override
|
||||||
|
const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
|
||||||
|
|
||||||
|
struct lookup_t { |
||||||
|
float x[3]; |
||||||
|
float y[3]; |
||||||
|
}; |
||||||
|
|
||||||
|
// 2m/s are added to be less restrictive
|
||||||
|
const struct lookup_t LOOKUP_ANGLE_RATE_UP = { |
||||||
|
{2., 7., 17.}, |
||||||
|
{5., .8, .15}}; |
||||||
|
|
||||||
|
const struct lookup_t LOOKUP_ANGLE_RATE_DOWN = { |
||||||
|
{2., 7., 17.}, |
||||||
|
{5., 3.5, .4}}; |
||||||
|
|
||||||
|
const float RT_ANGLE_FUDGE = 1.5; // for RT checks allow 50% more angle change
|
||||||
|
const float CAN_TO_DEG = 2. / 3.; // convert angles from CAN unit to degrees
|
||||||
|
|
||||||
|
int ipas_state = 1; // 1 disabled, 3 executing angle control, 5 override
|
||||||
|
int angle_control = 0; // 1 if direct angle control packets are seen
|
||||||
|
float speed = 0.; |
||||||
|
|
||||||
|
struct sample_t angle_meas; // last 3 steer angles
|
||||||
|
struct sample_t torque_driver; // last 3 driver steering torque
|
||||||
|
|
||||||
|
// state of angle limits
|
||||||
|
int16_t desired_angle_last = 0; // last desired steer angle
|
||||||
|
int16_t rt_angle_last = 0; // last desired torque for real time check
|
||||||
|
uint32_t ts_angle_last = 0; |
||||||
|
|
||||||
|
int controls_allowed_last = 0; |
||||||
|
|
||||||
|
int to_signed(int d, int bits) { |
||||||
|
if (d >= (1 << (bits - 1))) { |
||||||
|
d -= (1 << bits); |
||||||
|
} |
||||||
|
return d; |
||||||
|
} |
||||||
|
|
||||||
|
// interp function that holds extreme values
|
||||||
|
float interpolate(struct lookup_t xy, float x) { |
||||||
|
int size = sizeof(xy.x) / sizeof(xy.x[0]); |
||||||
|
// x is lower than the first point in the x array. Return the first point
|
||||||
|
if (x <= xy.x[0]) { |
||||||
|
return 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++) { |
||||||
|
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 no such point is found, then x > xy.x[size-1]. Return last point
|
||||||
|
return xy.y[size - 1]; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
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) { |
||||||
|
// get driver steering torque
|
||||||
|
int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF)); |
||||||
|
|
||||||
|
// update array of samples
|
||||||
|
update_sample(&torque_driver, torque_driver_new); |
||||||
|
} |
||||||
|
|
||||||
|
// get steer angle
|
||||||
|
if ((to_push->RIR>>21) == 0x25) { |
||||||
|
int angle_meas_new = ((to_push->RDLR & 0xf) << 8) + ((to_push->RDLR & 0xff00) >> 8); |
||||||
|
uint32_t ts = TIM2->CNT; |
||||||
|
|
||||||
|
angle_meas_new = to_signed(angle_meas_new, 12); |
||||||
|
|
||||||
|
// update array of samples
|
||||||
|
update_sample(&angle_meas, angle_meas_new); |
||||||
|
|
||||||
|
// *** 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); |
||||||
|
|
||||||
|
// every RT_INTERVAL or when controls are turned on, set the new limits
|
||||||
|
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); |
||||||
|
if ((ts_elapsed > RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) { |
||||||
|
rt_angle_last = angle_meas_new; |
||||||
|
ts_angle_last = ts; |
||||||
|
} |
||||||
|
|
||||||
|
// check for violation
|
||||||
|
if (angle_control && |
||||||
|
((angle_meas_new < lowest_rt_angle) || |
||||||
|
(angle_meas_new > highest_rt_angle))) { |
||||||
|
controls_allowed = 0; |
||||||
|
} |
||||||
|
|
||||||
|
controls_allowed_last = controls_allowed; |
||||||
|
} |
||||||
|
|
||||||
|
// get speed
|
||||||
|
if ((to_push->RIR>>21) == 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) { |
||||||
|
ipas_state = (to_push->RDLR & 0xf); |
||||||
|
} |
||||||
|
|
||||||
|
// exit controls on high steering override
|
||||||
|
if (angle_control && ((torque_driver.min > IPAS_OVERRIDE_THRESHOLD) || |
||||||
|
(torque_driver.max < -IPAS_OVERRIDE_THRESHOLD) || |
||||||
|
(ipas_state==5))) { |
||||||
|
controls_allowed = 0; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { |
||||||
|
|
||||||
|
// Check if msg is sent on BUS 0
|
||||||
|
if (((to_send->RDTR >> 4) & 0xF) == 0) { |
||||||
|
|
||||||
|
// STEER ANGLE
|
||||||
|
if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 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; |
||||||
|
|
||||||
|
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); |
||||||
|
if ((desired_angle > highest_desired_angle) ||
|
||||||
|
(desired_angle < lowest_desired_angle)){ |
||||||
|
violation = 1; |
||||||
|
controls_allowed = 0; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// desired steer angle should be the same as steer angle measured when controls are off
|
||||||
|
if ((!controls_allowed) &&
|
||||||
|
((desired_angle < (angle_meas.min - 1)) || |
||||||
|
(desired_angle > (angle_meas.max + 1)) || |
||||||
|
(ipas_state_cmd != 1))) { |
||||||
|
violation = 1; |
||||||
|
} |
||||||
|
|
||||||
|
desired_angle_last = desired_angle; |
||||||
|
|
||||||
|
if (violation) { |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// check standard toyota stuff as well
|
||||||
|
return toyota_tx_hook(to_send); |
||||||
|
} |
||||||
|
|
||||||
|
const safety_hooks toyota_ipas_hooks = { |
||||||
|
.init = toyota_init, |
||||||
|
.rx = toyota_ipas_rx_hook, |
||||||
|
.tx = toyota_ipas_tx_hook, |
||||||
|
.tx_lin = toyota_tx_lin_hook, |
||||||
|
.ignition = toyota_ign_hook, |
||||||
|
.fwd = toyota_fwd_hook, |
||||||
|
}; |
||||||
|
|
||||||
@ -0,0 +1,17 @@ |
|||||||
|
FROM ubuntu:16.04 |
||||||
|
|
||||||
|
RUN apt-get update && apt-get install -y gcc-arm-none-eabi libnewlib-arm-none-eabi python python-pip gcc g++ git autoconf gperf bison flex automake texinfo wget help2man gawk libtool libtool-bin ncurses-dev unzip unrar-free libexpat-dev sed bzip2 |
||||||
|
|
||||||
|
RUN pip install pycrypto==2.6.1 |
||||||
|
|
||||||
|
# Build esp toolchain |
||||||
|
RUN mkdir -p /panda/boardesp |
||||||
|
WORKDIR /panda/boardesp |
||||||
|
RUN git clone --recursive https://github.com/pfalcon/esp-open-sdk.git |
||||||
|
WORKDIR /panda/boardesp/esp-open-sdk |
||||||
|
RUN git checkout 03f5e898a059451ec5f3de30e7feff30455f7ce |
||||||
|
RUN CT_ALLOW_BUILD_AS_ROOT_SURE=1 make STANDALONE=y |
||||||
|
|
||||||
|
COPY . /panda |
||||||
|
|
||||||
|
WORKDIR /panda |
||||||
@ -0,0 +1,6 @@ |
|||||||
|
FROM ubuntu:16.04 |
||||||
|
|
||||||
|
RUN apt-get update && apt-get install -y clang make python python-pip |
||||||
|
COPY tests/safety/requirements.txt /panda/tests/safety/requirements.txt |
||||||
|
RUN pip install -r /panda/tests/safety/requirements.txt |
||||||
|
COPY . /panda |
||||||
@ -0,0 +1,18 @@ |
|||||||
|
CC = clang
|
||||||
|
CCFLAGS = -O3 -fPIC -DPANDA -I.
|
||||||
|
|
||||||
|
.PHONY: all |
||||||
|
all: libpandasafety.so |
||||||
|
|
||||||
|
libpandasafety.so: test.o |
||||||
|
$(CC) -shared -o '$@' $^ -lm
|
||||||
|
|
||||||
|
test.o: test.c |
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CCFLAGS) -MMD -c -I../../board -o '$@' '$<'
|
||||||
|
|
||||||
|
.PHONY: clean |
||||||
|
clean: |
||||||
|
rm -f libpandasafety.so test.o test.d
|
||||||
|
|
||||||
|
-include test.d |
||||||
@ -0,0 +1,60 @@ |
|||||||
|
import os |
||||||
|
import subprocess |
||||||
|
|
||||||
|
from cffi import FFI |
||||||
|
|
||||||
|
can_dir = os.path.dirname(os.path.abspath(__file__)) |
||||||
|
libpandasafety_fn = os.path.join(can_dir, "libpandasafety.so") |
||||||
|
subprocess.check_call(["make"], cwd=can_dir) |
||||||
|
|
||||||
|
ffi = FFI() |
||||||
|
ffi.cdef(""" |
||||||
|
typedef struct |
||||||
|
{ |
||||||
|
uint32_t TIR; /*!< CAN TX mailbox identifier register */ |
||||||
|
uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */ |
||||||
|
uint32_t TDLR; /*!< CAN mailbox data low register */ |
||||||
|
uint32_t TDHR; /*!< CAN mailbox data high register */ |
||||||
|
} CAN_TxMailBox_TypeDef; |
||||||
|
|
||||||
|
typedef struct |
||||||
|
{ |
||||||
|
uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */ |
||||||
|
uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */ |
||||||
|
uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */ |
||||||
|
uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */ |
||||||
|
} CAN_FIFOMailBox_TypeDef; |
||||||
|
|
||||||
|
typedef struct |
||||||
|
{ |
||||||
|
uint32_t CNT; |
||||||
|
} TIM_TypeDef; |
||||||
|
|
||||||
|
void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); |
||||||
|
int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); |
||||||
|
void toyota_init(int16_t param); |
||||||
|
void set_controls_allowed(int c); |
||||||
|
void reset_angle_control(void); |
||||||
|
int get_controls_allowed(void); |
||||||
|
void init_tests_toyota(void); |
||||||
|
void set_timer(int t); |
||||||
|
void set_torque_meas(int min, int max); |
||||||
|
void set_rt_torque_last(int t); |
||||||
|
void set_desired_torque_last(int t); |
||||||
|
int get_torque_meas_min(void); |
||||||
|
int get_torque_meas_max(void); |
||||||
|
|
||||||
|
void init_tests_honda(void); |
||||||
|
int get_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_brake_prev(void); |
||||||
|
int get_gas_prev(void); |
||||||
|
|
||||||
|
void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); |
||||||
|
int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); |
||||||
|
|
||||||
|
""") |
||||||
|
|
||||||
|
libpandasafety = ffi.dlopen(libpandasafety_fn) |
||||||
@ -0,0 +1,2 @@ |
|||||||
|
cffi==1.11.4 |
||||||
|
numpy==1.14.1 |
||||||
@ -0,0 +1,108 @@ |
|||||||
|
#include <stdint.h> |
||||||
|
#include <stdbool.h> |
||||||
|
|
||||||
|
typedef struct |
||||||
|
{ |
||||||
|
uint32_t TIR; /*!< CAN TX mailbox identifier register */ |
||||||
|
uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */ |
||||||
|
uint32_t TDLR; /*!< CAN mailbox data low register */ |
||||||
|
uint32_t TDHR; /*!< CAN mailbox data high register */ |
||||||
|
} CAN_TxMailBox_TypeDef; |
||||||
|
|
||||||
|
typedef struct |
||||||
|
{ |
||||||
|
uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */ |
||||||
|
uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */ |
||||||
|
uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */ |
||||||
|
uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */ |
||||||
|
} CAN_FIFOMailBox_TypeDef; |
||||||
|
|
||||||
|
typedef struct |
||||||
|
{ |
||||||
|
uint32_t CNT; |
||||||
|
} TIM_TypeDef; |
||||||
|
|
||||||
|
struct sample_t torque_meas; |
||||||
|
|
||||||
|
TIM_TypeDef timer; |
||||||
|
TIM_TypeDef *TIM2 = &timer; |
||||||
|
|
||||||
|
#define min(a,b) \ |
||||||
|
({ __typeof__ (a) _a = (a); \
|
||||||
|
__typeof__ (b) _b = (b); \
|
||||||
|
_a < _b ? _a : _b; }) |
||||||
|
|
||||||
|
#define max(a,b) \ |
||||||
|
({ __typeof__ (a) _a = (a); \
|
||||||
|
__typeof__ (b) _b = (b); \
|
||||||
|
_a > _b ? _a : _b; }) |
||||||
|
|
||||||
|
|
||||||
|
#define PANDA |
||||||
|
#define static |
||||||
|
#include "safety.h" |
||||||
|
|
||||||
|
void set_controls_allowed(int c){ |
||||||
|
controls_allowed = c; |
||||||
|
} |
||||||
|
|
||||||
|
void reset_angle_control(void){ |
||||||
|
angle_control = 0; |
||||||
|
} |
||||||
|
|
||||||
|
int get_controls_allowed(void){ |
||||||
|
return controls_allowed; |
||||||
|
} |
||||||
|
|
||||||
|
void set_timer(int t){ |
||||||
|
timer.CNT = t; |
||||||
|
} |
||||||
|
|
||||||
|
void set_torque_meas(int min, int max){ |
||||||
|
torque_meas.min = min; |
||||||
|
torque_meas.max = max; |
||||||
|
} |
||||||
|
|
||||||
|
int get_torque_meas_min(void){ |
||||||
|
return torque_meas.min; |
||||||
|
} |
||||||
|
|
||||||
|
int get_torque_meas_max(void){ |
||||||
|
return torque_meas.max; |
||||||
|
} |
||||||
|
|
||||||
|
void set_rt_torque_last(int t){ |
||||||
|
rt_torque_last = t; |
||||||
|
} |
||||||
|
|
||||||
|
void set_desired_torque_last(int t){ |
||||||
|
desired_torque_last = t; |
||||||
|
} |
||||||
|
|
||||||
|
int get_ego_speed(void){ |
||||||
|
return ego_speed; |
||||||
|
} |
||||||
|
|
||||||
|
int get_brake_prev(void){ |
||||||
|
return brake_prev; |
||||||
|
} |
||||||
|
|
||||||
|
int get_gas_prev(void){ |
||||||
|
return gas_prev; |
||||||
|
} |
||||||
|
|
||||||
|
void init_tests_toyota(void){ |
||||||
|
torque_meas.min = 0; |
||||||
|
torque_meas.max = 0; |
||||||
|
desired_torque_last = 0; |
||||||
|
rt_torque_last = 0; |
||||||
|
ts_last = 0; |
||||||
|
set_timer(0); |
||||||
|
} |
||||||
|
|
||||||
|
void init_tests_honda(void){ |
||||||
|
ego_speed = 0; |
||||||
|
gas_interceptor_detected = 0; |
||||||
|
brake_prev = 0; |
||||||
|
gas_prev = 0; |
||||||
|
} |
||||||
@ -0,0 +1,2 @@ |
|||||||
|
#!/usr/bin/env sh |
||||||
|
python -m unittest discover . |
||||||
@ -0,0 +1,148 @@ |
|||||||
|
#!/usr/bin/env python2 |
||||||
|
import unittest |
||||||
|
import numpy as np |
||||||
|
import libpandasafety_py |
||||||
|
|
||||||
|
|
||||||
|
class TestHondaSafety(unittest.TestCase): |
||||||
|
@classmethod |
||||||
|
def setUp(cls): |
||||||
|
cls.safety = libpandasafety_py.libpandasafety |
||||||
|
cls.safety.honda_init(0) |
||||||
|
cls.safety.init_tests_honda() |
||||||
|
|
||||||
|
def _speed_msg(self, speed): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0x158 << 21 |
||||||
|
to_send[0].RDLR = speed |
||||||
|
|
||||||
|
return to_send |
||||||
|
|
||||||
|
def _button_msg(self, buttons): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0x1A6 << 21 |
||||||
|
to_send[0].RDLR = buttons << 5 |
||||||
|
|
||||||
|
return to_send |
||||||
|
|
||||||
|
def _brake_msg(self, brake): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0x17C << 21 |
||||||
|
to_send[0].RDHR = 0x200000 if brake else 0 |
||||||
|
|
||||||
|
return to_send |
||||||
|
|
||||||
|
def _gas_msg(self, gas): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0x17C << 21 |
||||||
|
to_send[0].RDLR = 1 if gas else 0 |
||||||
|
|
||||||
|
return to_send |
||||||
|
|
||||||
|
def _send_brake_msg(self, brake): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0x1FA << 21 |
||||||
|
to_send[0].RDLR = brake |
||||||
|
|
||||||
|
return to_send |
||||||
|
|
||||||
|
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 |
||||||
|
|
||||||
|
return to_send |
||||||
|
|
||||||
|
def _send_steer_msg(self, steer): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0xE4 << 21 |
||||||
|
to_send[0].RDLR = steer |
||||||
|
|
||||||
|
return to_send |
||||||
|
|
||||||
|
def test_default_controls_not_allowed(self): |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_resume_button(self): |
||||||
|
RESUME_BTN = 4 |
||||||
|
self.safety.honda_rx_hook(self._button_msg(RESUME_BTN)) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_set_button(self): |
||||||
|
SET_BTN = 3 |
||||||
|
self.safety.honda_rx_hook(self._button_msg(SET_BTN)) |
||||||
|
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)) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_sample_speed(self): |
||||||
|
self.assertEqual(0, self.safety.get_ego_speed()) |
||||||
|
self.safety.honda_rx_hook(self._speed_msg(100)) |
||||||
|
self.assertEqual(100, self.safety.get_ego_speed()) |
||||||
|
|
||||||
|
def test_prev_brake(self): |
||||||
|
self.assertFalse(self.safety.get_brake_prev()) |
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True)) |
||||||
|
self.assertTrue(self.safety.get_brake_prev()) |
||||||
|
|
||||||
|
def test_disengage_on_brake(self): |
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.safety.honda_rx_hook(self._brake_msg(1)) |
||||||
|
self.assertFalse(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.set_controls_allowed(1) |
||||||
|
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True)) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
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.set_controls_allowed(1) |
||||||
|
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True)) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_prev_gas(self): |
||||||
|
self.assertFalse(self.safety.get_gas_prev()) |
||||||
|
self.safety.honda_rx_hook(self._gas_msg(True)) |
||||||
|
self.assertTrue(self.safety.get_gas_prev()) |
||||||
|
|
||||||
|
def test_disengage_on_gas(self): |
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.safety.honda_rx_hook(self._gas_msg(1)) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_allow_engage_with_gas_pressed(self): |
||||||
|
self.safety.honda_rx_hook(self._gas_msg(1)) |
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.safety.honda_rx_hook(self._gas_msg(1)) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_brake_safety_check(self): |
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000))) |
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) |
||||||
|
|
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) |
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x00F0))) |
||||||
|
|
||||||
|
def test_gas_safety_check(self): |
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000))) |
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) |
||||||
|
|
||||||
|
def test_steer_safety_check(self): |
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000))) |
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000))) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
unittest.main() |
||||||
@ -0,0 +1,411 @@ |
|||||||
|
#!/usr/bin/env python2 |
||||||
|
import unittest |
||||||
|
import numpy as np |
||||||
|
import libpandasafety_py |
||||||
|
|
||||||
|
MAX_RATE_UP = 10 |
||||||
|
MAX_RATE_DOWN = 25 |
||||||
|
MAX_TORQUE = 1500 |
||||||
|
|
||||||
|
MAX_ACCEL = 1500 |
||||||
|
MIN_ACCEL = -3000 |
||||||
|
|
||||||
|
MAX_RT_DELTA = 375 |
||||||
|
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 |
||||||
|
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.toyota_init(100) |
||||||
|
cls.safety.init_tests_toyota() |
||||||
|
|
||||||
|
def _set_prev_torque(self, t): |
||||||
|
self.safety.set_desired_torque_last(t) |
||||||
|
self.safety.set_rt_torque_last(t) |
||||||
|
self.safety.set_torque_meas(t, t) |
||||||
|
|
||||||
|
def _torque_meas_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].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(3): |
||||||
|
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(3): |
||||||
|
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 |
||||||
|
|
||||||
|
t = twos_comp(torque, 16) |
||||||
|
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 |
||||||
|
|
||||||
|
a = twos_comp(accel, 16) |
||||||
|
to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) |
||||||
|
return to_send |
||||||
|
|
||||||
|
def test_default_controls_not_allowed(self): |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_manually_enable_controls_allowed(self): |
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_enable_control_allowed_from_cruise(self): |
||||||
|
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_push[0].RIR = 0x1D2 << 21 |
||||||
|
to_push[0].RDHR = 0xF00000 |
||||||
|
|
||||||
|
self.safety.toyota_rx_hook(to_push) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_disable_control_allowed_from_cruise(self): |
||||||
|
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_push[0].RIR = 0x1D2 << 21 |
||||||
|
to_push[0].RDHR = 0 |
||||||
|
|
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.safety.toyota_rx_hook(to_push) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_accel_actuation_limits(self): |
||||||
|
for accel in np.arange(MIN_ACCEL - 1000, MAX_ACCEL + 1000, 100): |
||||||
|
for controls_allowed in [True, False]: |
||||||
|
self.safety.set_controls_allowed(controls_allowed) |
||||||
|
|
||||||
|
if controls_allowed: |
||||||
|
send = MIN_ACCEL <= accel <= MAX_ACCEL |
||||||
|
else: |
||||||
|
send = accel == 0 |
||||||
|
self.assertEqual(send, self.safety.toyota_tx_hook(self._accel_msg(accel))) |
||||||
|
|
||||||
|
def test_torque_absolute_limits(self): |
||||||
|
for controls_allowed in [True, False]: |
||||||
|
for torque in np.arange(-MAX_TORQUE - 1000, MAX_TORQUE + 1000, MAX_RATE_UP): |
||||||
|
self.safety.set_controls_allowed(controls_allowed) |
||||||
|
self.safety.set_rt_torque_last(torque) |
||||||
|
self.safety.set_torque_meas(torque, torque) |
||||||
|
self.safety.set_desired_torque_last(torque - MAX_RATE_UP) |
||||||
|
|
||||||
|
if controls_allowed: |
||||||
|
send = (-MAX_TORQUE <= torque <= MAX_TORQUE) |
||||||
|
else: |
||||||
|
send = torque == 0 |
||||||
|
|
||||||
|
self.assertEqual(send, self.safety.toyota_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._set_prev_torque(0) |
||||||
|
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) |
||||||
|
|
||||||
|
def test_non_realtime_limit_down(self): |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
self.safety.set_rt_torque_last(1000) |
||||||
|
self.safety.set_torque_meas(500, 500) |
||||||
|
self.safety.set_desired_torque_last(1000) |
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN))) |
||||||
|
|
||||||
|
self.safety.set_rt_torque_last(1000) |
||||||
|
self.safety.set_torque_meas(500, 500) |
||||||
|
self.safety.set_desired_torque_last(1000) |
||||||
|
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1))) |
||||||
|
|
||||||
|
def test_exceed_torque_sensor(self): |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
for sign in [-1, 1]: |
||||||
|
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.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10)))) |
||||||
|
|
||||||
|
def test_realtime_limit_up(self): |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
for sign in [-1, 1]: |
||||||
|
self.safety.init_tests_toyota() |
||||||
|
self._set_prev_torque(0) |
||||||
|
for t in np.arange(0, 380, 10): |
||||||
|
t *= sign |
||||||
|
self.safety.set_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._set_prev_torque(0) |
||||||
|
for t in np.arange(0, 370, 10): |
||||||
|
t *= sign |
||||||
|
self.safety.set_torque_meas(t, t) |
||||||
|
self.assertTrue(self.safety.toyota_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))) |
||||||
|
|
||||||
|
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.assertEqual(-51, self.safety.get_torque_meas_min()) |
||||||
|
self.assertEqual(51, self.safety.get_torque_meas_max()) |
||||||
|
|
||||||
|
self.safety.toyota_rx_hook(self._torque_meas_msg(0)) |
||||||
|
self.assertEqual(-1, self.safety.get_torque_meas_max()) |
||||||
|
self.assertEqual(-51, self.safety.get_torque_meas_min()) |
||||||
|
|
||||||
|
self.safety.toyota_rx_hook(self._torque_meas_msg(0)) |
||||||
|
self.assertEqual(-1, self.safety.get_torque_meas_max()) |
||||||
|
self.assertEqual(-1, self.safety.get_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() |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
unittest.main() |
||||||
Loading…
Reference in new issue