diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h index f930c4ff40a242..dfc15d0a358be8 100644 --- a/board/safety/safety_cadillac.h +++ b/board/safety/safety_cadillac.h @@ -1,4 +1,4 @@ -const int CADILLAC_STEER_MAX = 150; // 1s +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 @@ -14,8 +14,7 @@ int cadillac_rt_torque_last = 0; int cadillac_desired_torque_last[4] = {0}; // 4 torque messages uint32_t cadillac_ts_last = 0; int cadillac_supercruise_on = 0; - -struct sample_t cadillac_torque_driver; // last 3 driver torques measured +struct sample_t cadillac_torque_driver; // last few driver torques measured int cadillac_get_torque_idx(uint32_t addr) { if (addr==0x151) return 0; @@ -60,7 +59,7 @@ 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; - // block steering cmd above 150 + // steer cmd checks 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; @@ -71,12 +70,12 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (controls_allowed) { // *** global torque limit check *** - violation |= max_limit_check(desired_torque, CADILLAC_STEER_MAX); + violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER); // *** torque rate limit check *** int desired_torque_last = cadillac_desired_torque_last[idx]; violation |= driver_limit_check(desired_torque, desired_torque_last, &cadillac_torque_driver, - CADILLAC_STEER_MAX, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN, + CADILLAC_MAX_STEER, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN, CADILLAC_DRIVER_TORQUE_ALLOWANCE, CADILLAC_DRIVER_TORQUE_FACTOR); // used next time @@ -87,7 +86,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // every RT_INTERVAL set the new limits uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last); - if (ts_elapsed > RT_INTERVAL) { + if (ts_elapsed > CADILLAC_RT_INTERVAL) { cadillac_rt_torque_last = desired_torque; cadillac_ts_last = ts; } @@ -98,8 +97,8 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation = 1; } - // reset to 0 if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !controls_allowed) { cadillac_desired_torque_last[idx] = 0; cadillac_rt_torque_last = 0; cadillac_ts_last = ts; diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 4d213ce67e0d73..21bd61dda37068 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -8,15 +8,24 @@ // brake rising edge // brake > 0mph -// gm_: poor man's namespacing +const int GM_MAX_STEER = 255; +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 int GM_MAX_RATE_UP = 7; +const int GM_MAX_RATE_DOWN = 17; +const int GM_DRIVER_TORQUE_ALLOWANCE = 50; +const int GM_DRIVER_TORQUE_FACTOR = 4; + int gm_brake_prev = 0; int gm_gas_prev = 0; int gm_speed = 0; - // silence everything if stock ECUs are still online int gm_ascm_detected = 0; - int 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; @@ -31,6 +40,13 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { addr = to_push->RIR >> 21; } + if (addr == 388) { + int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF); + torque_driver_new = to_signed(torque_driver_new, 11); + // update array of samples + update_sample(&gm_torque_driver, torque_driver_new); + } + if (addr == 0x1f1 && bus_number == 0) { //Bit 5 should be ignition "on" //Backup plan is Bit 2 (accessory power) @@ -136,13 +152,49 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // LKA STEER: safety check if (addr == 384) { int rdlr = to_send->RDLR; - int steer = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8); - steer = to_signed(steer, 11); - int max_steer = 255; + int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8); + uint32_t ts = TIM2->CNT; + int violation = 0; + desired_torque = to_signed(desired_torque, 11); + if (current_controls_allowed) { - if ((steer > max_steer) || (steer < -max_steer)) return 0; - } else { - if (steer != 0) return 0; + + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, GM_MAX_STEER); + + // *** torque rate limit check *** + violation |= driver_limit_check(desired_torque, gm_desired_torque_last, &gm_torque_driver, + GM_MAX_STEER, GM_MAX_RATE_UP, GM_MAX_RATE_DOWN, + GM_DRIVER_TORQUE_ALLOWANCE, GM_DRIVER_TORQUE_FACTOR); + + // used next time + gm_desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + violation |= rt_rate_limit_check(desired_torque, gm_rt_torque_last, GM_MAX_RT_DELTA); + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, gm_ts_last); + if (ts_elapsed > GM_RT_INTERVAL) { + gm_rt_torque_last = desired_torque; + gm_ts_last = ts; + } + } + + // no torque if controls is not allowed + if (!current_controls_allowed && (desired_torque != 0)) { + violation = 1; + } + + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !current_controls_allowed) { + gm_desired_torque_last = 0; + gm_rt_torque_last = 0; + gm_ts_last = ts; + } + + if (violation) { + return false; } } diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index a8065255ed8ed7..8840c66f315dd1 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -40,6 +40,7 @@ void set_timer(int t); void set_torque_meas(int min, int max); void set_cadillac_torque_driver(int min, int max); +void set_gm_torque_driver(int min, int max); void set_rt_torque_last(int t); void set_desired_torque_last(int t); int get_torque_meas_min(void); @@ -62,6 +63,13 @@ void set_cadillac_desired_torque_last(int t); void set_cadillac_rt_torque_last(int t); +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 toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); diff --git a/tests/safety/test.c b/tests/safety/test.c index 66fc8ba8a9f2f7..f0f2af506da58b 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -24,6 +24,7 @@ typedef struct struct sample_t torque_meas; struct sample_t cadillac_torque_driver; +struct sample_t gm_torque_driver; TIM_TypeDef timer; TIM_TypeDef *TIM2 = &timer; @@ -69,6 +70,11 @@ void set_cadillac_torque_driver(int min, int max){ cadillac_torque_driver.max = max; } +void set_gm_torque_driver(int min, int max){ + gm_torque_driver.min = min; + gm_torque_driver.max = max; +} + int get_torque_meas_min(void){ return torque_meas.min; } @@ -85,6 +91,10 @@ void set_cadillac_rt_torque_last(int t){ cadillac_rt_torque_last = t; } +void set_gm_rt_torque_last(int t){ + gm_rt_torque_last = t; +} + void set_desired_torque_last(int t){ desired_torque_last = t; } @@ -93,6 +103,11 @@ void set_cadillac_desired_torque_last(int t){ for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = t; } +void set_gm_desired_torque_last(int t){ + gm_desired_torque_last = t; +} + + int get_ego_speed(void){ return ego_speed; } @@ -131,6 +146,15 @@ void init_tests_cadillac(void){ set_timer(0); } +void init_tests_gm(void){ + gm_torque_driver.min = 0; + gm_torque_driver.max = 0; + gm_desired_torque_last = 0; + gm_rt_torque_last = 0; + gm_ts_last = 0; + set_timer(0); +} + void init_tests_honda(void){ ego_speed = 0; gas_interceptor_detected = 0; diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py new file mode 100644 index 00000000000000..8efba5e3183ecc --- /dev/null +++ b/tests/safety/test_gm.py @@ -0,0 +1,270 @@ +#!/usr/bin/env python2 +import unittest +import numpy as np +import libpandasafety_py + +MAX_RATE_UP = 7 +MAX_RATE_DOWN = 17 +MAX_STEER = 255 +MAX_BRAKE = 255 +MAX_GAS = 3072 +MAX_REGEN = 1404 + +MAX_RT_DELTA = 128 +RT_INTERVAL = 250000 + +DRIVER_TORQUE_ALLOWANCE = 50; +DRIVER_TORQUE_FACTOR = 4; + +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 TestGmSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.gm_init(0) + cls.safety.init_tests_gm() + + def _speed_msg(self, speed): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 842 << 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 = 481 << 21 + to_send[0].RDHR = buttons << 12 + return to_send + + def _brake_msg(self, brake): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 241 << 21 + to_send[0].RDLR = 0xa00 if brake else 0x900 + return to_send + + def _gas_msg(self, gas): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 417 << 21 + to_send[0].RDHR = (1 << 16) 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 = 789 << 21 + brake = (-brake) & 0xfff + to_send[0].RDLR = (brake >> 8) | ((brake &0xff) << 8) + return to_send + + def _send_gas_msg(self, gas): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 715 << 21 + to_send[0].RDLR = ((gas & 0x1f) << 27) | ((gas & 0xfe0) << 11) + return to_send + + def _set_prev_torque(self, t): + self.safety.set_gm_desired_torque_last(t) + self.safety.set_gm_rt_torque_last(t) + + def _torque_driver_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 388 << 21 + + t = twos_comp(torque, 11) + to_send[0].RDHR = (((t >> 8) & 0x7) << 16) | ((t & 0xFF) << 24) + return to_send + + def _torque_driver_msg_array(self, torque): + for i in range(3): + self.safety.gm_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 = 384 << 21 + + t = twos_comp(torque, 11) + to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8) + return to_send + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + 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.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.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.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.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.set_controls_allowed(1) + + self.safety.gm_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.gm_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.set_controls_allowed(1) + + self.safety.gm_rx_hook(self._brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.gm_rx_hook(self._brake_msg(False)) + + def test_disengage_on_gas(self): + self.safety.set_controls_allowed(1) + self.safety.gm_rx_hook(self._gas_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.gm_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.set_controls_allowed(1) + self.safety.gm_rx_hook(self._gas_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.gm_rx_hook(self._gas_msg(False)) + + def test_brake_safety_check(self): + for enabled in [0, 1]: + for b in range(0, 500): + self.safety.set_controls_allowed(enabled) + if abs(b) > MAX_BRAKE or (not enabled and b != 0): + self.assertFalse(self.safety.gm_tx_hook(self._send_brake_msg(b))) + else: + self.assertTrue(self.safety.gm_tx_hook(self._send_brake_msg(b))) + + def test_gas_safety_check(self): + for enabled in [0, 1]: + for g in range(0, 2**12-1): + self.safety.set_controls_allowed(enabled) + if abs(g) > MAX_GAS or (not enabled and g != MAX_REGEN): + self.assertFalse(self.safety.gm_tx_hook(self._send_gas_msg(g))) + else: + self.assertTrue(self.safety.gm_tx_hook(self._send_gas_msg(g))) + + def test_steer_safety_check(self): + for enabled in [0, 1]: + for t in range(-0x200, 0x200): + 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))) + else: + self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t))) + + def test_manually_enable_controls_allowed(self): + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(0) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_non_realtime_limit_up(self): + self.safety.set_gm_torque_driver(0, 0) + 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._set_prev_torque(0) + self.assertTrue(self.safety.gm_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.safety.set_controls_allowed(True) + self._set_prev_torque(0) + self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_gm_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + def test_against_torque_driver(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): + 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.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))) + + # spot check some individual cases + for sign in [-1, 1]: + driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign + torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign + 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._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._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._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._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))) + + + def test_realtime_limits(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self.safety.init_tests_gm() + self._set_prev_torque(0) + 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._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))) + + # 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)))) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index b7e0612a799119..143916d609ec05 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -72,11 +72,13 @@ def test_default_controls_not_allowed(self): 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.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.assertTrue(self.safety.get_controls_allowed()) @@ -119,6 +121,7 @@ def test_allow_brake_at_zero_speed(self): self.safety.honda_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 def test_not_allow_brake_when_moving(self): # Brake was already pressed @@ -154,10 +157,12 @@ def test_brake_safety_check(self): 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))) + self.safety.set_controls_allowed(0) + self.assertTrue(self.safety.honda_tx_hook(self._send_gas_msg(0x0000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_gas_msg(0x1000))) 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)))