Hyundai: add gas disengage and tests (#447)

commaai · Feb 22, 2020 · 1b49d3e · 1b49d3e
1 parent 598074c
commit 1b49d3e
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 1 deletion.
diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h
@@ -9,6 +9,7 @@ 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 = {1057}, .bus = 0, .expected_timestep = 20000U},
 };
@@ -17,6 +18,7 @@ const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_c
 int hyundai_rt_torque_last = 0;
 int hyundai_desired_torque_last = 0;
 int hyundai_cruise_engaged_last = 0;
+bool hyundai_gas_last = false;
 uint32_t hyundai_ts_last = 0;
 struct sample_t hyundai_torque_driver;         // last few driver torques measured
 
@@ -48,7 +50,14 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       hyundai_cruise_engaged_last = cruise_engaged;
     }
 
-    // TODO: check gas pressed
+    // exit controls on rising edge of gas press
+    if ((addr == 608) && (bus == 0)) {
+      bool gas = (GET_BYTE(to_push, 7) >> 6) != 0;
+      if (gas && ! hyundai_gas_last) {
+        controls_allowed = 0;
+      }
+      hyundai_gas_last = gas;
+    }
 
     // check if stock camera ECU is on bus 0
     if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) {

diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py
@@ -41,6 +41,11 @@ def _button_msg(self, buttons):
     to_send[0].RDLR = buttons
     return to_send
 
+  def _gas_msg(self, val):
+    to_send = make_msg(0, 608)
+    to_send[0].RDHR = (val & 0x3) << 30;
+    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)
@@ -89,6 +94,13 @@ def test_disable_control_allowed_from_cruise(self):
     self.safety.safety_rx_hook(to_push)
     self.assertFalse(self.safety.get_controls_allowed())
 
+  def test_disengage_on_gas(self):
+    self.safety.set_controls_allowed(True)
+    self.safety.safety_rx_hook(self._gas_msg(0))
+    self.assertTrue(self.safety.get_controls_allowed())
+    self.safety.safety_rx_hook(self._gas_msg(1))
+    self.assertFalse(self.safety.get_controls_allowed())
+
   def test_non_realtime_limit_up(self):
     self.safety.set_hyundai_torque_driver(0, 0)
     self.safety.set_controls_allowed(True)