From 1c88caf917ce5d7fe569675bc445fe526cb798fc Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 2 Apr 2018 20:41:52 +0200 Subject: [PATCH] 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 limits --- .gitignore | 2 + tests/safety/Makefile | 18 +++ tests/safety/libpandasafety_py.py | 56 +++++++++ tests/safety/test.c | 101 ++++++++++++++++ tests/safety/test.sh | 2 + tests/safety/test_honda.py | 148 ++++++++++++++++++++++++ tests/safety/test_toyota.py | 185 ++++++++++++++++++++++++++++++ 7 files changed, 512 insertions(+) create mode 100644 tests/safety/Makefile create mode 100644 tests/safety/libpandasafety_py.py create mode 100644 tests/safety/test.c create mode 100755 tests/safety/test.sh create mode 100755 tests/safety/test_honda.py create mode 100755 tests/safety/test_toyota.py diff --git a/.gitignore b/.gitignore index c589cb1..7b7762e 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,8 @@ .*.swp .*.swo *.o +*.so +*.d a.out *~ .#* diff --git a/tests/safety/Makefile b/tests/safety/Makefile new file mode 100644 index 0000000..2a01774 --- /dev/null +++ b/tests/safety/Makefile @@ -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 diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py new file mode 100644 index 0000000..138e53d --- /dev/null +++ b/tests/safety/libpandasafety_py.py @@ -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) diff --git a/tests/safety/test.c b/tests/safety/test.c new file mode 100644 index 0000000..84278b7 --- /dev/null +++ b/tests/safety/test.c @@ -0,0 +1,101 @@ +#include +#include + +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; +} diff --git a/tests/safety/test.sh b/tests/safety/test.sh new file mode 100755 index 0000000..83d8f5b --- /dev/null +++ b/tests/safety/test.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env sh +python -m unittest discover . diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py new file mode 100755 index 0000000..fb169af --- /dev/null +++ b/tests/safety/test_honda.py @@ -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() diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py new file mode 100755 index 0000000..c3e389c --- /dev/null +++ b/tests/safety/test_toyota.py @@ -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()