Hyundai brake check (#464)

* WIP: adding brake check * Added Hyundai brake tests
commaai · Mar 8, 2020 · 04809e1 · 04809e1
1 parent 74c8ee0
commit 04809e1
Show file tree

Hide file tree

Showing 2 changed files with 60 additions and 1 deletion.
diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h
@@ -1,24 +1,29 @@
 const int HYUNDAI_MAX_STEER = 255;             // like stock
 const int HYUNDAI_MAX_RT_DELTA = 112;          // max delta torque allowed for real time checks
-const uint32_t HYUNDAI_RT_INTERVAL = 250000;    // 250ms between real time checks
+const uint32_t HYUNDAI_RT_INTERVAL = 250000;   // 250ms between real time checks
 const int HYUNDAI_MAX_RATE_UP = 3;
 const int HYUNDAI_MAX_RATE_DOWN = 7;
 const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
 const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
+const int HYUNDAI_STANDSTILL_THRSLD = 30;  // ~1kph
 const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}};
 
 // TODO: do checksum and counter checks
 AddrCheckStruct hyundai_rx_checks[] = {
   {.addr = {608}, .bus = 0, .expected_timestep = 10000U},
   {.addr = {897}, .bus = 0, .expected_timestep = 10000U},
+  {.addr = {902}, .bus = 0, .expected_timestep = 10000U},
+  {.addr = {916}, .bus = 0, .expected_timestep = 10000U},
   {.addr = {1057}, .bus = 0, .expected_timestep = 20000U},
 };
 const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]);
 
 int hyundai_rt_torque_last = 0;
 int hyundai_desired_torque_last = 0;
 int hyundai_cruise_engaged_last = 0;
+int hyundai_speed = 0;
 bool hyundai_gas_last = false;
+bool hyundai_brake_last = false;
 uint32_t hyundai_ts_last = 0;
 struct sample_t hyundai_torque_driver;         // last few driver torques measured
 
@@ -58,6 +63,22 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       hyundai_gas_last = gas;
     }
 
+    // sample subaru wheel speed, averaging opposite corners
+    if (addr == 902) {
+      hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF;  // FL
+      hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF;  // RL
+      hyundai_speed /= 2;
+    }
+
+    // exit controls on rising edge of brake press
+    if (addr == 916) {
+      bool brake = (GET_BYTE(to_push, 6) >> 7) != 0;
+      if (brake && (!hyundai_brake_last || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
+        controls_allowed = 0;
+      }
+      hyundai_brake_last = brake;
+    }
+
     // check if stock camera ECU is on bus 0
     if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) {
       relay_malfunction = true;

diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py
@@ -15,6 +15,8 @@
 DRIVER_TORQUE_ALLOWANCE = 50;
 DRIVER_TORQUE_FACTOR = 2;
 
+SPEED_THRESHOLD = 30  # ~1kph
+
 TX_MSGS = [[832, 0], [1265, 0]]
 
 def twos_comp(val, bits):
@@ -46,6 +48,17 @@ def _gas_msg(self, val):
     to_send[0].RDHR = (val & 0x3) << 30;
     return to_send
 
+  def _brake_msg(self, brake):
+    to_send = make_msg(0, 916)
+    to_send[0].RDHR = brake << 23;
+    return to_send
+
+  def _speed_msg(self, speed):
+    to_send = make_msg(0, 902)
+    to_send[0].RDLR = speed & 0x3FFF;
+    to_send[0].RDHR = (speed & 0x3FFF) << 16;
+    return to_send
+
   def _set_prev_torque(self, t):
     self.safety.set_hyundai_desired_torque_last(t)
     self.safety.set_hyundai_rt_torque_last(t)
@@ -101,6 +114,31 @@ 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
+    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
+    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_non_realtime_limit_up(self):
     self.safety.set_hyundai_torque_driver(0, 0)
     self.safety.set_controls_allowed(True)