From b2dbb504dcae0f68842bf14b7293dbf89bf3d546 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Wed, 4 Mar 2020 10:54:13 -0800 Subject: [PATCH] remove toyota ipas safety code and tests (#460) --- board/safety.h | 3 - board/safety/safety_toyota_ipas.h | 169 --------------------- python/__init__.py | 1 - tests/safety/libpandasafety_py.py | 1 - tests/safety/test.c | 4 - tests/safety/test_toyota_ipas.py | 243 ------------------------------ 6 files changed, 421 deletions(-) delete mode 100644 board/safety/safety_toyota_ipas.h delete mode 100644 tests/safety/test_toyota_ipas.py diff --git a/board/safety.h b/board/safety.h index b52171e30e4f1a..407f33f6695ee8 100644 --- a/board/safety.h +++ b/board/safety.h @@ -4,7 +4,6 @@ #include "safety/safety_defaults.h" #include "safety/safety_honda.h" #include "safety/safety_toyota.h" -#include "safety/safety_toyota_ipas.h" #include "safety/safety_tesla.h" #include "safety/safety_gm_ascm.h" #include "safety/safety_gm.h" @@ -34,7 +33,6 @@ #define SAFETY_MAZDA 13U #define SAFETY_NISSAN 14U #define SAFETY_VOLKSWAGEN_MQB 15U -#define SAFETY_TOYOTA_IPAS 16U #define SAFETY_ALLOUTPUT 17U #define SAFETY_GM_ASCM 18U #define SAFETY_NOOUTPUT 19U @@ -208,7 +206,6 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_NOOUTPUT, &nooutput_hooks}, #ifdef ALLOW_DEBUG {SAFETY_CADILLAC, &cadillac_hooks}, - {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, {SAFETY_TESLA, &tesla_hooks}, {SAFETY_NISSAN, &nissan_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h deleted file mode 100644 index e9515658cdded0..00000000000000 --- a/board/safety/safety_toyota_ipas.h +++ /dev/null @@ -1,169 +0,0 @@ -// uses tons from safety_toyota -// TODO: refactor to repeat less code - -// IPAS override -const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value - -// 2m/s are added to be less restrictive -const struct lookup_t LOOKUP_ANGLE_RATE_UP = { - {2., 7., 17.}, - {5., .8, .15}}; - -const struct lookup_t LOOKUP_ANGLE_RATE_DOWN = { - {2., 7., 17.}, - {5., 3.5, .4}}; - -const float RT_ANGLE_FUDGE = 1.5; // for RT checks allow 50% more angle change -const float CAN_TO_DEG = 2. / 3.; // convert angles from CAN unit to degrees - -int ipas_state = 1; // 1 disabled, 3 executing angle control, 5 override -int angle_control = 0; // 1 if direct angle control packets are seen -float speed = 0.; - -struct sample_t angle_meas; // last 3 steer angles -struct sample_t torque_driver; // last 3 driver steering torque - -// state of angle limits -int16_t desired_angle_last = 0; // last desired steer angle -int16_t rt_angle_last = 0; // last desired torque for real time check -uint32_t ts_angle_last = 0; - -int controls_allowed_last = 0; - - -static int toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - // check standard toyota stuff as well - bool valid = toyota_rx_hook(to_push); - - int addr = GET_ADDR(to_push); - - if (addr == 0x260) { - // get driver steering torque - int16_t torque_driver_new = (GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 2); - - // update array of samples - update_sample(&torque_driver, torque_driver_new); - } - - // get steer angle - if (addr == 0x25) { - int angle_meas_new = ((GET_BYTE(to_push, 0) & 0xF) << 8) | GET_BYTE(to_push, 1); - uint32_t ts = TIM2->CNT; - - angle_meas_new = to_signed(angle_meas_new, 12); - - // update array of samples - update_sample(&angle_meas, angle_meas_new); - - // *** angle real time check - // add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz - int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG) + 1.))); - int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG) + 1.))); - int highest_rt_angle = rt_angle_last + ((rt_angle_last > 0) ? rt_delta_angle_up : rt_delta_angle_down); - int lowest_rt_angle = rt_angle_last - ((rt_angle_last > 0) ? rt_delta_angle_down : rt_delta_angle_up); - - // every RT_INTERVAL or when controls are turned on, set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); - if ((ts_elapsed > TOYOTA_RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) { - rt_angle_last = angle_meas_new; - ts_angle_last = ts; - } - - // check for violation - if (angle_control && - ((angle_meas_new < lowest_rt_angle) || - (angle_meas_new > highest_rt_angle))) { - controls_allowed = 0; - } - - controls_allowed_last = controls_allowed; - } - - // get speed - if (addr == 0xb4) { - speed = ((float)((GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6))) * 0.01 / 3.6; - } - - // get ipas state - if (addr == 0x262) { - ipas_state = GET_BYTE(to_push, 0) & 0xf; - } - - // exit controls on high steering override - if (angle_control && ((torque_driver.min > TOYOTA_IPAS_OVERRIDE_THRESHOLD) || - (torque_driver.max < -TOYOTA_IPAS_OVERRIDE_THRESHOLD) || - (ipas_state==5))) { - controls_allowed = 0; - } - return valid; -} - -static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - - int tx = 1; - int bypass_standard_tx_hook = 0; - int bus = GET_BUS(to_send); - int addr = GET_ADDR(to_send); - - // Check if msg is sent on BUS 0 - if (bus == 0) { - - // STEER ANGLE - if ((addr == 0x266) || (addr == 0x167)) { - - angle_control = 1; // we are in angle control mode - int desired_angle = ((GET_BYTE(to_send, 0) & 0xF) << 8) | GET_BYTE(to_send, 1); - int ipas_state_cmd = GET_BYTE(to_send, 0) >> 4; - bool violation = 0; - - desired_angle = to_signed(desired_angle, 12); - - if (controls_allowed) { - // add 1 to not false trigger the violation - float delta_angle_float; - delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG) + 1.; - int delta_angle_up = (int) (delta_angle_float); - delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG) + 1.; - int delta_angle_down = (int) (delta_angle_float); - - int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); - int lowest_desired_angle = desired_angle_last - ((desired_angle_last > 0) ? delta_angle_down : delta_angle_up); - if ((desired_angle > highest_desired_angle) || - (desired_angle < lowest_desired_angle)){ - violation = 1; - controls_allowed = 0; - } - } - - // desired steer angle should be the same as steer angle measured when controls are off - if ((!controls_allowed) && - ((desired_angle < (angle_meas.min - 1)) || - (desired_angle > (angle_meas.max + 1)) || - (ipas_state_cmd != 1))) { - violation = 1; - } - - desired_angle_last = desired_angle; - - if (violation) { - tx = 0; - } - bypass_standard_tx_hook = 1; - } - } - - // check standard toyota stuff as well if addr isn't IPAS related - if (!bypass_standard_tx_hook) { - tx &= toyota_tx_hook(to_send); - } - - return tx; -} - -const safety_hooks toyota_ipas_hooks = { - .init = toyota_init, - .rx = toyota_ipas_rx_hook, - .tx = toyota_ipas_tx_hook, - .tx_lin = nooutput_tx_lin_hook, - .fwd = toyota_fwd_hook, -}; diff --git a/python/__init__.py b/python/__init__.py index a137174276a66b..c0e27e8d6a775e 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -125,7 +125,6 @@ class Panda(object): SAFETY_MAZDA = 13 SAFETY_NISSAN = 14 SAFETY_VOLKSWAGEN_MQB = 15 - SAFETY_TOYOTA_IPAS = 16 SAFETY_ALLOUTPUT = 17 SAFETY_GM_ASCM = 18 SAFETY_NOOUTPUT = 19 diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index b289e4c5fc2ea8..4830d2366f66f1 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -39,7 +39,6 @@ int get_gas_interceptor_prev(void); int get_hw_type(void); void set_timer(uint32_t t); -void reset_angle_control(void); int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send); int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push); diff --git a/tests/safety/test.c b/tests/safety/test.c index 48b94dec6cb734..980bf18d548e87 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -85,10 +85,6 @@ void set_gas_interceptor_detected(bool c){ gas_interceptor_detected = c; } -void reset_angle_control(void){ - angle_control = 0; -} - bool get_controls_allowed(void){ return controls_allowed; } diff --git a/tests/safety/test_toyota_ipas.py b/tests/safety/test_toyota_ipas.py deleted file mode 100644 index 3fcca95f1ca53c..00000000000000 --- a/tests/safety/test_toyota_ipas.py +++ /dev/null @@ -1,243 +0,0 @@ -#!/usr/bin/env python3 -import unittest -import numpy as np -from panda import Panda -from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import make_msg -from panda.tests.safety.test_toyota import toyota_checksum - -IPAS_OVERRIDE_THRESHOLD = 200 - -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit - -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 TestToyotaSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA_IPAS, 66) - cls.safety.init_tests_toyota() - - def _torque_driver_msg(self, torque): - to_send = make_msg(0, 0x260) - t = twos_comp(torque, 16) - to_send[0].RDLR = t | ((t & 0xFF) << 16) - to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24) - return to_send - - def _torque_driver_msg_array(self, torque): - for i in range(6): - self.safety.safety_rx_hook(self._torque_driver_msg(torque)) - - def _angle_meas_msg(self, angle): - to_send = make_msg(0, 0x25) - t = twos_comp(angle, 12) - to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) - return to_send - - def _angle_meas_msg_array(self, angle): - for i in range(6): - self.safety.safety_rx_hook(self._angle_meas_msg(angle)) - - def _ipas_state_msg(self, state): - to_send = make_msg(0, 0x262) - to_send[0].RDLR = state & 0xF - return to_send - - def _ipas_control_msg(self, angle, state): - # note: we command 2/3 of the angle due to CAN conversion - to_send = make_msg(0, 0x266) - t = twos_comp(angle, 12) - to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) - to_send[0].RDLR |= ((state & 0xf) << 4) - - return to_send - - def _speed_msg(self, speed): - to_send = make_msg(0, 0xB4) - speed = int(speed * 100 * 3.6) - - to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00) - return to_send - - def test_ipas_override(self): - - ## angle control is not active - self.safety.set_controls_allowed(1) - - # 3 consecutive msgs where driver exceeds threshold but angle_control isn't active - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) - self.assertTrue(self.safety.get_controls_allowed()) - - self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) - self.assertTrue(self.safety.get_controls_allowed()) - - # ipas state is override - self.safety.safety_rx_hook(self._ipas_state_msg(5)) - self.assertTrue(self.safety.get_controls_allowed()) - - ## now angle control is active - self.safety.safety_tx_hook(self._ipas_control_msg(0, 0)) - self.safety.safety_rx_hook(self._ipas_state_msg(0)) - - # 3 consecutive msgs where driver does exceed threshold - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) - self.assertFalse(self.safety.get_controls_allowed()) - - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) - self.assertFalse(self.safety.get_controls_allowed()) - - # ipas state is override and torque isn't overriding any more - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(0) - self.safety.safety_rx_hook(self._ipas_state_msg(5)) - self.assertFalse(self.safety.get_controls_allowed()) - - # 3 consecutive msgs where driver does not exceed threshold and - # ipas state is not override - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._ipas_state_msg(0)) - self.assertTrue(self.safety.get_controls_allowed()) - - self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD) - self.assertTrue(self.safety.get_controls_allowed()) - - self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD) - self.assertTrue(self.safety.get_controls_allowed()) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_when_disabled(self): - - self.safety.set_controls_allowed(0) - - # test angle cmd too far from actual - angle_refs = [-10, 10] - deltas = list(range(-2, 3)) - expected_results = [False, True, True, True, False] - - for a in angle_refs: - self._angle_meas_msg_array(a) - for i, d in enumerate(deltas): - self.assertEqual(expected_results[i], self.safety.safety_tx_hook(self._ipas_control_msg(a + d, 1))) - - # test ipas state cmd enabled - self._angle_meas_msg_array(0) - self.assertEqual(0, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3))) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_when_enabled(self): - - # ipas angle cmd should pass through when controls are enabled - - self.safety.set_controls_allowed(1) - self._angle_meas_msg_array(0) - self.safety.safety_rx_hook(self._speed_msg(0.1)) - - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(4, 1))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-4, 3))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-8, 3))) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_rate_when_disabled(self): - - # as long as the command is close to the measured, no rate limit is enforced when - # controls are disabled - self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._angle_meas_msg(0)) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1))) - self.safety.safety_rx_hook(self._angle_meas_msg(100)) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(100, 1))) - self.safety.safety_rx_hook(self._angle_meas_msg(-100)) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-100, 1))) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_rate_when_enabled(self): - - # when controls are allowed, angle cmd rate limit is enforced - # test 1: no limitations if we stay within limits - speeds = [0., 1., 5., 10., 15., 100.] - angles = [-300, -100, -10, 0, 10, 100, 300] - for a in angles: - for s in speeds: - - # first test against false positives - self._angle_meas_msg_array(a) - self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._speed_msg(s)) - max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) - max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # now inject too high rates - self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * - (max_delta_up + 1), 1))) - self.assertFalse(self.safety.get_controls_allowed()) - self.safety.set_controls_allowed(1) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) * - (max_delta_down + 1), 1))) - self.assertFalse(self.safety.get_controls_allowed()) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_measured_rate(self): - - speeds = [0., 1., 5., 10., 15., 100.] - angles = [-300, -100, -10, 0, 10, 100, 300] - angles = [10] - for a in angles: - for s in speeds: - self._angle_meas_msg_array(a) - self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._speed_msg(s)) - #max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) - #max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) - self.safety.safety_rx_hook(self._angle_meas_msg(a)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._angle_meas_msg(a + 150)) - self.assertFalse(self.safety.get_controls_allowed()) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - -if __name__ == "__main__": - unittest.main()