From 2ebbe3616b5c9a35e803cf45b2a2cb2fc2895c18 Mon Sep 17 00:00:00 2001
From: rbiasini <riccardo.biasini@gmail.com>
Date: Wed, 19 Feb 2020 19:55:05 -0800
Subject: [PATCH] Subaru: disengage on gas press (#446)

* Subaru: check for gas pressed

* added tests

* rx freq check on throttle

* also support for the not yet supported pre-global platform
---
 board/safety/safety_subaru.h | 12 +++++++++++-
 board/safety/safety_toyota.h |  2 +-
 tests/safety/test_subaru.py  | 12 ++++++++++++
 3 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index 885e2731d523c2..505b4b8568e35d 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -12,6 +12,7 @@ const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x164, 0}, {0x221, 0}, {0x322, 0}
 
 // TODO: do checksum and counter checks after adding the signals to the outback dbc file
 AddrCheckStruct subaru_rx_checks[] = {
+  {.addr = { 0x40, 0x140}, .bus = 0, .expected_timestep = 10000U},
   {.addr = {0x119, 0x371}, .bus = 0, .expected_timestep = 20000U},
   {.addr = {0x240, 0x144}, .bus = 0, .expected_timestep = 50000U},
 };
@@ -21,6 +22,7 @@ int subaru_cruise_engaged_last = 0;
 int subaru_rt_torque_last = 0;
 int subaru_desired_torque_last = 0;
 uint32_t subaru_ts_last = 0;
+bool subaru_gas_last = false;
 struct sample_t subaru_torque_driver;         // last few driver torques measured
 
 static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
@@ -53,7 +55,15 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       subaru_cruise_engaged_last = cruise_engaged;
     }
 
-    // TODO: enforce cancellation on gas pressed
+    // exit controls on rising edge of gas press
+    if (((addr == 0x40) || (addr == 0x140)) && (bus == 0)) {
+      int byte = (addr == 0x40) ? 4 : 0;
+      bool gas = GET_BYTE(to_push, byte) != 0;
+      if (gas && !subaru_gas_last) {
+        controls_allowed = 0;
+      }
+      subaru_gas_last = gas;
+    }
 
     if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) {
       relay_malfunction = true;
diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
index 4bf0a5de43d067..16900f00d01131 100644
--- a/board/safety/safety_toyota.h
+++ b/board/safety/safety_toyota.h
@@ -106,7 +106,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
     // exit controls on rising edge of gas press
     if (addr == 0x2C1) {
-      int gas = GET_BYTE(to_push, 6) & 0xFF;
+      int gas = GET_BYTE(to_push, 6);
       if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) {
         controls_allowed = 0;
       }
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index e18d7515cb1345..0f0dbcb1e21ac1 100644
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -52,6 +52,11 @@ def _torque_msg(self, torque):
     to_send[0].RDLR = (t << 16)
     return to_send
 
+  def _gas_msg(self, gas):
+    to_send = make_msg(0, 0x40)
+    to_send[0].RDHR = gas & 0xFF
+    return to_send
+
   def test_spam_can_buses(self):
     test_spam_can_buses(self, TX_MSGS)
 
@@ -74,6 +79,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_steer_safety_check(self):
     for enabled in [0, 1]:
       for t in range(-3000, 3000):