Subaru brake check (commaai#463)

* WIP: subaru brake check

* FInalized logic and added tests to Subaru brake check

board/safety/safety_subaru.h

@@ -7,6 +7,7 @@ const int SUBARU_MAX_RATE_UP = 50;
 const int SUBARU_MAX_RATE_DOWN = 70;
 const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60;
 const int SUBARU_DRIVER_TORQUE_FACTOR = 10;
+const int SUBARU_STANDSTILL_THRSLD = 20;  // about 1kph
 
 const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x221, 0}, {0x322, 0}};
 const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}};
@@ -16,6 +17,8 @@ const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_M
 AddrCheckStruct subaru_rx_checks[] = {
   {.addr = { 0x40}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U},
   {.addr = {0x119}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
+  {.addr = {0x139}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
+  {.addr = {0x13a}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
   {.addr = {0x240}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U},
 };
 // TODO: do checksum and counter checks after adding the signals to the outback dbc file
@@ -30,8 +33,10 @@ const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_r
 int subaru_cruise_engaged_last = 0;
 int subaru_rt_torque_last = 0;
 int subaru_desired_torque_last = 0;
+int subaru_speed = 0;
 uint32_t subaru_ts_last = 0;
 bool subaru_gas_last = false;
+bool subaru_brake_last = false;
 bool subaru_global = false;
 struct sample_t subaru_torque_driver;         // last few driver torques measured
 
@@ -92,6 +97,22 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       subaru_cruise_engaged_last = cruise_engaged;
     }
 
+    // sample subaru wheel speed, averaging opposite corners
+    if ((addr == 0x13a) && subaru_global) {
+      subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF;  // FR
+      subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF;  // RL
+      subaru_speed /= 2;
+    }
+
+    // exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models)
+    if ((addr == 0x139) && subaru_global) {
+      bool brake = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
+      if (brake && (!subaru_brake_last || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
+        controls_allowed = 0;
+      }
+      subaru_brake_last = brake;
+    }
+
     // exit controls on rising edge of gas press
     if (((addr == 0x40) && subaru_global) ||
         ((addr == 0x140) && !subaru_global)) {

tests/safety/test_subaru.py

@@ -15,6 +15,8 @@
 DRIVER_TORQUE_ALLOWANCE = 60;
 DRIVER_TORQUE_FACTOR = 10;
 
+SPEED_THRESHOLD = 20  # 1kph (see dbc file)
+
 TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]]
 TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]]
 
@@ -44,6 +46,8 @@ class TestSubaruSafety(unittest.TestCase):
   cnt_gas = 0
   cnt_torque_driver = 0
   cnt_cruise = 0
+  cnt_speed = 0
+  cnt_brake = 0
 
   @classmethod
   def setUp(cls):
@@ -69,6 +73,24 @@ def _torque_driver_msg(self, torque):
       to_send[0].RDHR = (t >> 3) & 0xFF
     return to_send
 
+  def _speed_msg(self, speed):
+    speed &= 0x1FFF
+    to_send = make_msg(0, 0x13a)
+    to_send[0].RDLR = speed << 12
+    to_send[0].RDHR = speed << 6
+    to_send[0].RDLR |= (self.cnt_speed & 0xF) << 8
+    to_send[0].RDLR |= subaru_checksum(to_send, 0x13a, 8)
+    self.__class__.cnt_speed += 1
+    return to_send
+
+  def _brake_msg(self, brake):
+    to_send = make_msg(0, 0x139)
+    to_send[0].RDHR = (brake << 4) & 0xFFF
+    to_send[0].RDLR |= (self.cnt_brake & 0xF) << 8
+    to_send[0].RDLR |= subaru_checksum(to_send, 0x139, 8)
+    self.__class__.cnt_brake += 1
+    return to_send
+
   def _torque_msg(self, torque):
     t = twos_comp(torque, 13)
     if self.safety.get_subaru_global():
@@ -133,6 +155,33 @@ def test_disengage_on_gas(self):
     self.safety.safety_rx_hook(self._gas_msg(1))
     self.assertFalse(self.safety.get_controls_allowed())
 
+  def test_allow_brake_at_zero_speed(self):
+    # Brake was already pressed
+    if (self.safety.get_subaru_global()):
+      self.safety.safety_rx_hook(self._brake_msg(1))
+      self.safety.set_controls_allowed(1)
+      self.safety.safety_rx_hook(self._brake_msg(1))
+      self.assertTrue(self.safety.get_controls_allowed())
+      self.safety.safety_rx_hook(self._brake_msg(0))
+      self.assertTrue(self.safety.get_controls_allowed())
+      # rising edge of brake should disengage
+      self.safety.safety_rx_hook(self._brake_msg(1))
+      self.assertFalse(self.safety.get_controls_allowed())
+      self.safety.safety_rx_hook(self._brake_msg(0))  # reset no brakes
+
+  def test_not_allow_brake_when_moving(self):
+    # Brake was already pressed
+    if (self.safety.get_subaru_global()):
+      self.safety.safety_rx_hook(self._brake_msg(1))
+      self.safety.set_controls_allowed(1)
+      self.safety.safety_rx_hook(self._speed_msg(SPEED_THRESHOLD))
+      self.safety.safety_rx_hook(self._brake_msg(1))
+      self.assertTrue(self.safety.get_controls_allowed())
+      self.safety.safety_rx_hook(self._speed_msg(SPEED_THRESHOLD + 1))
+      self.safety.safety_rx_hook(self._brake_msg(1))
+      self.assertFalse(self.safety.get_controls_allowed())
+      self.safety.safety_rx_hook(self._speed_msg(0))
+
   def test_steer_safety_check(self):
     for enabled in [0, 1]:
       for t in range(-3000, 3000):