From 7d21acbcccb67ac555d3633221b5a1ecbf0cb463 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Thu, 5 Apr 2018 13:30:44 -0700 Subject: [PATCH] added steer override check when IPAS is in control (#106) * added steer override check when IPAS is in control * same override threshold as carController * added initial safety tests for angle control * cleaned up safety tests and added ipas state override check * ipas_override is an unnecessary global variable * bump panda version --- VERSION | 2 +- board/safety/safety_toyota.h | 56 ++++++++++++++++++++++++--- tests/safety/test_toyota.py | 75 ++++++++++++++++++++++++++++++++++++ 3 files changed, 127 insertions(+), 6 deletions(-) diff --git a/VERSION b/VERSION index 992977ad209ac1..9cb4db99be24f8 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.1.0 \ No newline at end of file +v1.1.1 \ No newline at end of file diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 78aaa36973c6fa..cd9e06cdaad980 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -1,6 +1,15 @@ +int cruise_engaged_last = 0; // cruise state +int ipas_state = 0; + // track the torque measured for limiting int16_t torque_meas[3] = {0, 0, 0}; // last 3 motor torques produced by the eps int16_t torque_meas_min = 0, torque_meas_max = 0; +int16_t torque_driver[3] = {0, 0, 0}; // last 3 driver steering torque +int16_t torque_driver_min = 0, torque_driver_max = 0; + +// IPAS override +const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value +int angle_control = 0; // 1 if direct angle control packets are seen // global torque limit const int32_t MAX_TORQUE = 1500; // max torque cmd allowed ever @@ -30,8 +39,10 @@ int16_t rt_torque_last = 0; // last desired torque for real time chec uint32_t ts_last = 0; static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - // get eps motor torque (0.66 factor in dbc) + + // EPS torque sensor if ((to_push->RIR>>21) == 0x260) { + // get eps motor torque (see dbc_eps_torque_factor in dbc) int16_t torque_meas_new_16 = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF)); // increase torque_meas by 1 to be conservative on rounding @@ -49,16 +60,46 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (torque_meas[i] < torque_meas_min) torque_meas_min = torque_meas[i]; if (torque_meas[i] > torque_meas_max) torque_meas_max = torque_meas[i]; } + + // get driver steering torque + int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF)); + + // shift the array + for (int i = sizeof(torque_driver)/sizeof(torque_driver[0]) - 1; i > 0; i--) { + torque_driver[i] = torque_driver[i-1]; + } + torque_driver[0] = torque_driver_new; + + // get the minimum and maximum driver torque over the last 3 frames + torque_driver_min = torque_driver_max = torque_driver[0]; + for (int i = 1; i < sizeof(torque_driver)/sizeof(torque_driver[0]); i++) { + if (torque_driver[i] < torque_driver_min) torque_driver_min = torque_driver[i]; + if (torque_driver[i] > torque_driver_max) torque_driver_max = torque_driver[i]; + } } - // exit controls on ACC off + // enter controls on rising edge of ACC, exit controls on ACC off if ((to_push->RIR>>21) == 0x1D2) { // 4 bits: 55-52 - if (to_push->RDHR & 0xF00000) { + int cruise_engaged = to_push->RDHR & 0xF00000; + if (cruise_engaged && (!cruise_engaged_last)) { controls_allowed = 1; - } else { + } else if (!cruise_engaged) { controls_allowed = 0; } + cruise_engaged_last = cruise_engaged; + } + + // get ipas state + if ((to_push->RIR>>21) == 0x262) { + ipas_state = (to_push->RDLR & 0xf); + } + + // exit controls on high steering override + if (angle_control && ((torque_driver_min > IPAS_OVERRIDE_THRESHOLD) || + (torque_driver_max < -IPAS_OVERRIDE_THRESHOLD) || + (ipas_state==5))) { + controls_allowed = 0; } } @@ -79,7 +120,12 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } } - // STEER: safety check on bytes 2-3 + // STEER ANGLE + if ((to_send->RIR>>21) == 0x266) { + angle_control = 1; + } + + // STEER TORQUE: safety check on bytes 2-3 if ((to_send->RIR>>21) == 0x2E4) { int16_t desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF); int16_t violation = 0; diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index c3e389c2a55f5c..09a24e00a33d9f 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -15,6 +15,8 @@ MAX_TORQUE_ERROR = 350 +IPAS_OVERRIDE_THRESHOLD = 200 + def twos_comp(val, bits): if val >= 0: return val @@ -42,6 +44,18 @@ def _torque_meas_msg(self, torque): to_send[0].RDHR = t | ((t & 0xFF) << 16) return to_send + def _torque_driver_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x260 << 21 + + t = twos_comp(torque, 16) + to_send[0].RDLR = t | ((t & 0xFF) << 16) + return to_send + + def _torque_driver_msg_array(self, torque): + for i in range(3): + self.safety.toyota_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 = 0x2E4 << 21 @@ -50,6 +64,19 @@ def _torque_msg(self, torque): to_send[0].RDLR = t | ((t & 0xFF) << 16) return to_send + def _ipas_state_msg(self, state): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x262 << 21 + + to_send[0].RDLR = state & 0xF + return to_send + + def _ipas_control_msg(self): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x266 << 21 + + return to_send + def _accel_msg(self, accel): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 0x343 << 21 @@ -180,6 +207,54 @@ def test_torque_measurements(self): self.assertEqual(-1, self.safety.get_torque_meas_max()) self.assertEqual(-1, self.safety.get_torque_meas_min()) + 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.toyota_rx_hook(self._ipas_state_msg(5)) + self.assertTrue(self.safety.get_controls_allowed()) + + ## now angle control is active + self.safety.toyota_tx_hook(self._ipas_control_msg()) + self.safety.toyota_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.toyota_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.toyota_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()) + if __name__ == "__main__": unittest.main()