diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 885e273..505b4b8 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 4bf0a5d..16900f0 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 e18d751..0f0dbcb 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -52,6 +52,11 @@ class TestSubaruSafety(unittest.TestCase): 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 @@ class TestSubaruSafety(unittest.TestCase): 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):