From 43687488518a870625ee907723d74549729d7241 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Thu, 5 Mar 2020 00:16:03 -0800 Subject: [PATCH] WIP: Toyota brake check. (#459) * Toyota brake check with safety tests --- board/drivers/llcan.h | 2 +- board/safety/safety_toyota.h | 51 +++++++++++++++++++++++-------- tests/safety/libpandasafety_py.py | 2 +- tests/safety/test.c | 2 +- tests/safety/test_toyota.py | 39 +++++++++++++++++++++-- 5 files changed, 79 insertions(+), 17 deletions(-) diff --git a/board/drivers/llcan.h b/board/drivers/llcan.h index 4cd9b4b..5e9f276 100644 --- a/board/drivers/llcan.h +++ b/board/drivers/llcan.h @@ -52,7 +52,7 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s void llcan_init(CAN_TypeDef *CAN_obj) { // Enter init mode register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); - + // Wait for INAK bit to be set while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {} diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 16900f0..14eb1bb 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -16,7 +16,8 @@ const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2 const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 -const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file +const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph +const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 475; // ratio between offset and gain from dbc file const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}, {0x344, 0}, {0x365, 0}, {0x366, 0}, {0x4CB, 0}, // DSU bus 0 {0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1}, // DSU bus 1 @@ -24,8 +25,10 @@ const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0} {0x200, 0}, {0x750, 0}}; // interceptor + Blindspot monitor AddrCheckStruct toyota_rx_checks[] = { - {.addr = {0x260}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 20000U}, - {.addr = {0x1D2}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 30000U}, + {.addr = { 0xaa}, .bus = 0, .check_checksum = false, .expected_timestep = 12000U}, + {.addr = {0x260}, .bus = 0, .check_checksum = true, .expected_timestep = 20000U}, + {.addr = {0x1D2}, .bus = 0, .check_checksum = true, .expected_timestep = 30000U}, + {.addr = {0x224, 0x226}, .bus = 0, .check_checksum = false, .expected_timestep = 25000U}, }; const int TOYOTA_RX_CHECKS_LEN = sizeof(toyota_rx_checks) / sizeof(toyota_rx_checks[0]); @@ -37,7 +40,9 @@ int toyota_desired_torque_last = 0; // last desired steer torque int toyota_rt_torque_last = 0; // last desired torque for real time check uint32_t toyota_ts_last = 0; int toyota_cruise_engaged_last = 0; // cruise state -int toyota_gas_prev = 0; +bool toyota_gas_prev = false; +bool toyota_brake_prev = false; +bool toyota_moving = false; struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps @@ -65,7 +70,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); // get eps motor torque (0.66 factor in dbc) - if (addr == 0x260) { + if ((addr == 0x260) && (bus == 0)) { int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6); torque_meas_new = to_signed(torque_meas_new, 16); @@ -81,7 +86,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1D2) { + if ((addr == 0x1D2) && (bus == 0)) { // 5th bit is CRUISE_ACTIVE int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; if (!cruise_engaged) { @@ -93,21 +98,43 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { toyota_cruise_engaged_last = cruise_engaged; } + // sample speed + if ((addr == 0xaa) && (bus == 0)) { + int speed = 0; + // sum 4 wheel speeds + for (int i=0; i<8; i+=2) { + int next_byte = i + 1; // hack to deal with misra 10.8 + speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte); + } + toyota_moving = (speed / 4) > TOYOTA_STANDSTILL_THRSLD; + } + + // exit controls on rising edge of brake pedal + // most cars have brake_pressed on 0x226, corolla and rav4 on 0x224 + if (((addr == 0x224) || (addr == 0x226)) && (bus == 0)) { + int byte = (addr == 0x224) ? 0 : 4; + bool brake = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0; + if (brake && (!toyota_brake_prev || toyota_moving)) { + controls_allowed = 0; + } + toyota_brake_prev = brake; + } + // exit controls on rising edge of interceptor gas press - if (addr == 0x201) { + if ((addr == 0x201) && (bus == 0)) { gas_interceptor_detected = 1; int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && - (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) { + if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && + (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) { controls_allowed = 0; } gas_interceptor_prev = gas_interceptor; } // exit controls on rising edge of gas press - if (addr == 0x2C1) { - int gas = GET_BYTE(to_push, 6); - if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) { + if ((addr == 0x2C1) && (bus == 0)) { + bool gas = GET_BYTE(to_push, 6) != 0; + if (gas && !toyota_gas_prev && !gas_interceptor_detected) { controls_allowed = 0; } toyota_gas_prev = gas; diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 4830d23..8b099fd 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -48,7 +48,7 @@ int set_safety_hooks(uint16_t mode, int16_t param); void init_tests_toyota(void); int get_toyota_torque_meas_min(void); int get_toyota_torque_meas_max(void); -int get_toyota_gas_prev(void); +bool get_toyota_gas_prev(void); void set_toyota_torque_meas(int min, int max); void set_toyota_desired_torque_last(int t); void set_toyota_rt_torque_last(int t); diff --git a/tests/safety/test.c b/tests/safety/test.c index 980bf18..af3d096 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -159,7 +159,7 @@ int get_chrysler_torque_meas_max(void){ return chrysler_torque_meas.max; } -int get_toyota_gas_prev(void){ +bool get_toyota_gas_prev(void){ return toyota_gas_prev; } diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index 493fb1f..a7506c0 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -15,6 +15,8 @@ MIN_ACCEL = -3000 MAX_RT_DELTA = 375 RT_INTERVAL = 250000 +STANDSTILL_THRESHOLD = 100 # 1kph + MAX_TORQUE_ERROR = 350 INTERCEPTOR_THRESHOLD = 475 @@ -62,7 +64,7 @@ class TestToyotaSafety(unittest.TestCase): t = twos_comp(torque, 16) to_send = make_msg(0, 0x260) to_send[0].RDHR = (t & 0xff00) | ((t & 0xFF) << 16) - to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24) + to_send[0].RDHR |= toyota_checksum(to_send[0], 0x260, 8) << 24 return to_send def _torque_msg(self, torque): @@ -77,6 +79,20 @@ class TestToyotaSafety(unittest.TestCase): to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) return to_send + def _speed_msg(self, s): + to_send = make_msg(0, 0xaa) + to_send[0].RDLR = (s & 0xFF) << 8 | (s >> 8) + to_send[0].RDLR += (s & 0xFF) << 24 | ((s >> 8) << 16) + to_send[0].RDHR = (s & 0xFF) << 8 | (s >> 8) + to_send[0].RDHR += (s & 0xFF) << 24 | ((s >> 8) << 16) + return to_send + + def _brake_msg(self, brake): + to_send = make_msg(0, 0x226) + to_send[0].RDHR = brake << 5 + to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24 + return to_send + def _send_gas_msg(self, gas): to_send = make_msg(0, 0x2C1) to_send[0].RDHR = (gas & 0xFF) << 16 @@ -121,7 +137,7 @@ class TestToyotaSafety(unittest.TestCase): def test_prev_gas(self): for g in range(0, 256): self.safety.safety_rx_hook(self._send_gas_msg(g)) - self.assertEqual(g, self.safety.get_toyota_gas_prev()) + self.assertEqual(True if g > 0 else False, self.safety.get_toyota_gas_prev()) def test_prev_gas_interceptor(self): self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) @@ -155,6 +171,25 @@ class TestToyotaSafety(unittest.TestCase): self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) self.safety.set_gas_interceptor_detected(False) + def test_allow_brake_at_zero_speed(self): + # Brake was already pressed + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.set_controls_allowed(1) + + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._brake_msg(False)) # reset no brakes + + def test_not_allow_brake_when_moving(self): + # Brake was already pressed + self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.safety_rx_hook(self._speed_msg(STANDSTILL_THRESHOLD + 1)) + self.safety.set_controls_allowed(1) + + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + def test_allow_engage_with_gas_interceptor_pressed(self): self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) self.safety.set_controls_allowed(1)