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

Author: rbiasini

VERSION

@@ -1 +1 @@
-v1.1.0
+v1.1.1

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;

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()