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,6 +2,8 @@
 .*.swp
 .*.swo
 *.o
+*.so
+*.d
 a.out
 *~
 .#*

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

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)

tests/safety/test.c

@@ -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;
+}

tests/safety/test.sh

@@ -0,0 +1,2 @@
+#!/usr/bin/env sh
+python -m unittest discover .

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()