diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h index 50f9a2281b2869..5dd85a7fb774eb 100644 --- a/board/safety/safety_cadillac.h +++ b/board/safety/safety_cadillac.h @@ -1,18 +1,39 @@ const int CADILLAC_STEER_MAX = 150; // 1s const int CADILLAC_IGNITION_TIMEOUT = 1000000; // 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 +const int32_t CADILLAC_RT_INTERVAL = 250000; // 250ms between real time checks +const int CADILLAC_MAX_RATE_UP = 2; +const int CADILLAC_MAX_RATE_DOWN = 5; +const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50; +const int CADILLAC_DRIVER_TORQUE_FACTOR = 4; int cadillac_ign = 0; int cadillac_cruise_engaged_last = 0; +uint32_t cadillac_ts_ign_last = 0; +int cadillac_rt_torque_last = 0; +int cadillac_desired_torque_last = 0; uint32_t cadillac_ts_last = 0; +struct sample_t cadillac_torque_driver; // last 3 driver torques measured + static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus_number = (to_push->RDTR >> 4) & 0xFF; uint32_t addr = to_push->RIR >> 21; + if (addr == 356) { + int torque_driver_new = ((to_push->RDLR & 0x3) << 8) | ((to_push->RDLR >> 8) & 0xFF); + torque_driver_new = to_signed(torque_driver_new, 11); + + // update array of sample + update_sample(&cadillac_torque_driver, torque_driver_new); + } + // this message isn't all zeros when ignition is on if ((addr == 0x160) && (bus_number == 0) && to_push->RDLR) { cadillac_ign = 1; - cadillac_ts_last = TIM2->CNT; // reset timer when ign is received + cadillac_ts_ign_last = TIM2->CNT; // reset timer when ign is received } // enter controls on rising edge of ACC, exit controls on ACC off @@ -32,14 +53,79 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // block steering cmd above 150 if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) { - int lkas_cmd = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8); - lkas_cmd = to_signed(lkas_cmd, 14); - // block message is controls are allowed and lkas command exceeds max, or - // if controls aren't allowed and lkas cmd isn't 0 - if (controls_allowed && - ((lkas_cmd > CADILLAC_STEER_MAX) || (lkas_cmd < -CADILLAC_STEER_MAX))) { - return 0; - } else if (!controls_allowed && lkas_cmd) return 0; + int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8); + int violation = 0; + uint32_t ts = TIM2->CNT; + desired_torque = to_signed(desired_torque, 14); + + if (controls_allowed) { + + // *** global torque limit check *** + if ((desired_torque > CADILLAC_STEER_MAX) || (desired_torque < -CADILLAC_STEER_MAX)) { + violation = 1; + } + + // *** torque rate limit check *** + int highest_allowed_torque = max(cadillac_desired_torque_last, 0) + CADILLAC_MAX_RATE_UP; + int lowest_allowed_torque = min(cadillac_desired_torque_last, 0) - CADILLAC_MAX_RATE_UP; + + int driver_torque_max_limit = CADILLAC_STEER_MAX + + (CADILLAC_DRIVER_TORQUE_ALLOWANCE + cadillac_torque_driver.max) * + CADILLAC_DRIVER_TORQUE_FACTOR; + int driver_torque_min_limit = -CADILLAC_STEER_MAX + + (-CADILLAC_DRIVER_TORQUE_ALLOWANCE + cadillac_torque_driver.max) * + CADILLAC_DRIVER_TORQUE_FACTOR; + + // if we've exceeded the applied torque, we must start moving toward 0 + highest_allowed_torque = min(highest_allowed_torque, + max(cadillac_desired_torque_last - CADILLAC_MAX_RATE_DOWN, + max(driver_torque_max_limit, 0))); + lowest_allowed_torque = max(lowest_allowed_torque, + min(cadillac_desired_torque_last + CADILLAC_MAX_RATE_DOWN, + min(driver_torque_min_limit, 0))); + + // check for violation + if ((desired_torque < lowest_allowed_torque) || (desired_torque > highest_allowed_torque)) { + violation = 1; + } + + //// used next time + cadillac_desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + int highest_rt_torque = max(cadillac_rt_torque_last, 0) + CADILLAC_MAX_RT_DELTA; + int lowest_rt_torque = min(cadillac_rt_torque_last, 0) - CADILLAC_MAX_RT_DELTA; + + + // check for violation + if ((desired_torque < lowest_rt_torque) || (desired_torque > highest_rt_torque)) { + violation = 1; + } + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last); + if (ts_elapsed > RT_INTERVAL) { + cadillac_rt_torque_last = desired_torque; + cadillac_ts_last = ts; + } + } + + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = 1; + } + + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !controls_allowed) { + cadillac_desired_torque_last = 0; + cadillac_rt_torque_last = 0; + cadillac_ts_last = ts; + } + + if (violation) { + return false; + } + } return true; } @@ -50,14 +136,13 @@ static void cadillac_init(int16_t param) { static int cadillac_ign_hook() { uint32_t ts = TIM2->CNT; - uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_ign_last); if (ts_elapsed > CADILLAC_IGNITION_TIMEOUT) { cadillac_ign = 0; } return cadillac_ign; } -// Placeholder file, actual safety is TODO. const safety_hooks cadillac_hooks = { .init = cadillac_init, .rx = cadillac_rx_hook, diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 2d908488a1ce81..bfb1f890751686 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -39,6 +39,7 @@ void init_tests_toyota(void); void set_timer(int t); void set_torque_meas(int min, int max); +void set_cadillac_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); @@ -52,6 +53,13 @@ int get_brake_prev(void); int get_gas_prev(void); +void init_tests_cadillac(void); +void cadillac_init(int16_t param); +void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +void set_cadillac_desired_torque_last(int t); +void set_cadillac_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 eb19ab28c08b88..353b4fab57dec2 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -23,6 +23,7 @@ typedef struct } TIM_TypeDef; struct sample_t torque_meas; +struct sample_t cadillac_torque_driver; TIM_TypeDef timer; TIM_TypeDef *TIM2 = &timer; @@ -63,6 +64,11 @@ void set_torque_meas(int min, int max){ torque_meas.max = max; } +void set_cadillac_torque_driver(int min, int max){ + cadillac_torque_driver.min = min; + cadillac_torque_driver.max = max; +} + int get_torque_meas_min(void){ return torque_meas.min; } @@ -75,10 +81,18 @@ void set_rt_torque_last(int t){ rt_torque_last = t; } +void set_cadillac_rt_torque_last(int t){ + cadillac_rt_torque_last = t; +} + void set_desired_torque_last(int t){ desired_torque_last = t; } +void set_cadillac_desired_torque_last(int t){ + cadillac_desired_torque_last = t; +} + int get_ego_speed(void){ return ego_speed; } @@ -100,6 +114,15 @@ void init_tests_toyota(void){ set_timer(0); } +void init_tests_cadillac(void){ + cadillac_torque_driver.min = 0; + cadillac_torque_driver.max = 0; + cadillac_desired_torque_last = 0; + cadillac_rt_torque_last = 0; + cadillac_ts_last = 0; + set_timer(0); +} + void init_tests_honda(void){ ego_speed = 0; gas_interceptor_detected = 0; diff --git a/tests/safety/test_cadillac.py b/tests/safety/test_cadillac.py new file mode 100644 index 00000000000000..d9f70ff111520d --- /dev/null +++ b/tests/safety/test_cadillac.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python2 +import unittest +import numpy as np +import libpandasafety_py + +MAX_RATE_UP = 2 +MAX_RATE_DOWN = 5 +MAX_TORQUE = 150 + +MAX_RT_DELTA = 75 +RT_INTERVAL = 250000 + +DRIVER_TORQUE_ALLOWANCE = 50; +DRIVER_TORQUE_FACTOR = 4; + +IPAS_OVERRIDE_THRESHOLD = 200 + +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 TestCadillacSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.cadillac_init(0) + cls.safety.init_tests_cadillac() + + def _set_prev_torque(self, t): + self.safety.set_cadillac_desired_torque_last(t) + self.safety.set_cadillac_rt_torque_last(t) + + def _torque_driver_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x164 << 21 + + t = twos_comp(torque, 11) + to_send[0].RDLR = ((t >> 8) & 0x3) | ((t & 0xFF) << 8) + return to_send + + def _torque_driver_msg_array(self, torque): + for i in range(3): + self.safety.cadillac_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 = 0x151 << 21 + + t = twos_comp(torque, 14) + to_send[0].RDLR = ((t >> 8) & 0x3F) | ((t & 0xFF) << 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()) + self.safety.set_controls_allowed(0) + + def test_enable_control_allowed_from_cruise(self): + to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_push[0].RIR = 0x370 << 21 + to_push[0].RDLR = 0x800000 + to_push[0].RDTR = 0 + + self.safety.cadillac_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 = 0x370 << 21 + to_push[0].RDLR = 0 + to_push[0].RDTR = 0 + + self.safety.set_controls_allowed(1) + self.safety.cadillac_rx_hook(to_push) + self.assertFalse(self.safety.get_controls_allowed()) + + 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_cadillac_rt_torque_last(torque) + self.safety.set_cadillac_torque_driver(0, 0) + self.safety.set_cadillac_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.cadillac_tx_hook(self._torque_msg(torque))) + + def test_non_realtime_limit_up(self): + self.safety.set_cadillac_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + self._set_prev_torque(0) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(MAX_RATE_UP))) + self._set_prev_torque(0) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_RATE_UP))) + + self._set_prev_torque(0) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) + self.safety.set_controls_allowed(True) + self._set_prev_torque(0) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_cadillac_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + def test_exceed_torque_sensor(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_cadillac_torque_driver(t, t) + self._set_prev_torque(MAX_TORQUE * sign) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(MAX_TORQUE * sign))) + + self.safety.set_cadillac_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_TORQUE))) + + # spot check some individual cases + for sign in [-1, 1]: + driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign + torque_desired = (MAX_TORQUE - 10 * DRIVER_TORQUE_FACTOR) * sign + delta = 1 * sign + self._set_prev_torque(torque_desired) + self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired))) + self._set_prev_torque(torque_desired + delta) + self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired + delta))) + + self._set_prev_torque(MAX_TORQUE * sign) + self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN) * sign))) + self._set_prev_torque(MAX_TORQUE * sign) + self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(0))) + self._set_prev_torque(MAX_TORQUE * sign) + self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - 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_cadillac() + self._set_prev_torque(0) + self.safety.set_cadillac_torque_driver(0, 0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.cadillac_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.cadillac_tx_hook(self._torque_msg(t))) + + # Increase timer to update rt_torque_last + self.safety.set_timer(RT_INTERVAL + 1) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + + +if __name__ == "__main__": + unittest.main()