toyota: use universal gas pressed bit (#488)

* toyota: use universal gas pressed bit

* fix tests

board/drivers/llcan.h

@@ -11,7 +11,7 @@
 #define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
 #define GET_LEN(msg) ((msg)->RDTR & 0xF)
 #define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
-#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0XFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
+#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
 #define GET_BYTES_04(msg) ((msg)->RDLR)
 #define GET_BYTES_48(msg) ((msg)->RDHR)
 

board/safety/safety_toyota.h

@@ -88,6 +88,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     }
 
     // enter controls on rising edge of ACC, exit controls on ACC off
+    // exit controls on rising edge of gas press
     if (addr == 0x1D2) {
       // 5th bit is CRUISE_ACTIVE
       int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
@@ -98,6 +99,13 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
         controls_allowed = 1;
       }
       toyota_cruise_engaged_last = cruise_engaged;
+
+      // handle gas_pressed
+      bool gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0;
+      if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
+        controls_allowed = 0;
+      }
+      gas_pressed_prev = gas_pressed;
     }
 
     // sample speed
@@ -133,15 +141,6 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       gas_interceptor_prev = gas_interceptor;
     }
 
-    // exit controls on rising edge of gas press
-    if (addr == 0x2C1) {
-      bool gas_pressed = GET_BYTE(to_push, 6) != 0;
-      if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
-        controls_allowed = 0;
-      }
-      gas_pressed_prev = gas_pressed;
-    }
-
     // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
     if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) {
       relay_malfunction = true;

tests/safety/test_toyota.py

@@ -97,9 +97,10 @@ def _brake_msg(self, brake):
     to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24
     return to_send
 
-  def _send_gas_msg(self, gas):
-    to_send = make_msg(0, 0x2C1)
-    to_send[0].RDHR = (gas & 0xFF) << 16
+  def _gas_pressed_msg(self, pressed, enable_cruise=False):
+    to_send = make_msg(0, 0x1D2)
+    to_send[0].RDLR = ((1*(not pressed)) << 4) | (1*enable_cruise << 5)
+    to_send[0].RDHR = (toyota_checksum(to_send[0], 0x1D2, 8) << 24)
     return to_send
 
   def _send_interceptor_msg(self, gas, addr):
@@ -139,9 +140,9 @@ def test_disable_control_allowed_from_cruise(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
   def test_prev_gas(self):
-    for g in range(0, 256):
-      self.safety.safety_rx_hook(self._send_gas_msg(g))
-      self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
+    for pressed in [True, False]:
+      self.safety.safety_rx_hook(self._gas_pressed_msg(pressed))
+      self.assertEqual(pressed, self.safety.get_gas_pressed_prev())
 
   def test_prev_gas_interceptor(self):
     self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
@@ -152,26 +153,27 @@ def test_prev_gas_interceptor(self):
     self.safety.set_gas_interceptor_detected(False)
 
   def test_disengage_on_gas(self):
-    self.safety.safety_rx_hook(self._send_gas_msg(0))
+    self.safety.safety_rx_hook(self._gas_pressed_msg(False))
     self.safety.set_controls_allowed(True)
-    self.safety.safety_rx_hook(self._send_gas_msg(1))
+    self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
     self.assertFalse(self.safety.get_controls_allowed())
 
   def test_unsafe_mode_no_disengage_on_gas(self):
-    self.safety.safety_rx_hook(self._send_gas_msg(0))
+    self.safety.safety_rx_hook(self._gas_pressed_msg(False))
     self.safety.set_controls_allowed(True)
     self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS)
-    self.safety.safety_rx_hook(self._send_gas_msg(1))
+    self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
     self.assertTrue(self.safety.get_controls_allowed())
     self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
 
   def test_allow_engage_with_gas_pressed(self):
-    self.safety.safety_rx_hook(self._send_gas_msg(1))
+    self.safety.safety_rx_hook(self._gas_pressed_msg(True))
     self.safety.set_controls_allowed(True)
-    self.safety.safety_rx_hook(self._send_gas_msg(1))
-    self.assertTrue(self.safety.get_controls_allowed())
-    self.safety.safety_rx_hook(self._send_gas_msg(1))
-    self.assertTrue(self.safety.get_controls_allowed())
+    for _ in range(2):
+      # since cruise msg is used for gas pedal state, cruise bit must
+      # also be set for this test or else it will set controls_allowed
+      self.safety.safety_rx_hook(self._gas_pressed_msg(True, enable_cruise=True))
+      self.assertTrue(self.safety.get_controls_allowed())
 
   def test_disengage_on_gas_interceptor(self):
     for g in range(0, 0x1000):