Safety code testing (#104)
* inital infrastructure for panda safety testing * add test for toyota acceleration * test for non real time torque rate limits and refactoring * add test for cruise disable * fix toyota limit down * add tests for realtime limits * test for torque measurements * fix toyota test setup * honda button logic * test for brake logic * tests for gas logic * test steer, gas and brake message contents * add test script * fix hardcoded limitsmaster
parent
f4efd1f29d
commit
1c88caf917
|
@ -2,6 +2,8 @@
|
|||
.*.swp
|
||||
.*.swo
|
||||
*.o
|
||||
*.so
|
||||
*.d
|
||||
a.out
|
||||
*~
|
||||
.#*
|
||||
|
|
|
@ -0,0 +1,18 @@
|
|||
CC = clang
|
||||
CCFLAGS = -O3 -fPIC -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,56 @@
|
|||
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);
|
||||
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);
|
||||
|
||||
""")
|
||||
|
||||
libpandasafety = ffi.dlopen(libpandasafety_fn)
|
|
@ -0,0 +1,101 @@
|
|||
#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;
|
||||
|
||||
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 static
|
||||
#include "safety.h"
|
||||
|
||||
void set_controls_allowed(int c){
|
||||
controls_allowed = c;
|
||||
}
|
||||
|
||||
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,185 @@
|
|||
#!/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
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
return val
|
||||
else:
|
||||
return (2**bits) + val
|
||||
|
||||
|
||||
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_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 _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())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
Loading…
Reference in New Issue