From e49d0dbbd64467eb7cf1997364ea6610060b0334 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Mon, 15 Jul 2019 13:15:22 -0700 Subject: [PATCH] Pedal: use avg between tracks (#253) --- board/safety/safety_honda.h | 2 +- board/safety/safety_toyota.h | 2 +- board/safety_declarations.h | 3 +++ tests/safety/test_honda.py | 25 ++++++++++++++----------- tests/safety/test_toyota.py | 25 ++++++++++++++----------- 5 files changed, 33 insertions(+), 24 deletions(-) diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index c36c60fe6aa763..6179a5e37450d5 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -62,7 +62,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // length check because bosch hardware also uses this id (0x201 w/ len = 8) if ((addr == 0x201) && (len == 6)) { gas_interceptor_detected = 1; - int gas_interceptor = (GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1); + int gas_interceptor = GET_INTERCEPTOR(to_push); if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD) && long_controls_allowed) { diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index ac3184d39737b5..c1ce996058611b 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -69,7 +69,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of interceptor gas press if (addr == 0x201) { gas_interceptor_detected = 1; - int gas_interceptor = (GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1); + int gas_interceptor = GET_INTERCEPTOR(to_push); if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && long_controls_allowed) { diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 2029a22593aa37..7e0a54d73e8231 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -50,3 +50,6 @@ int gas_interceptor_prev = 0; // This is set by USB command 0xdf bool long_controls_allowed = 1; + +// avg between 2 tracks +#define GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2) diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index f0562b8d4c1351..5833d96e3f89f0 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -5,6 +5,8 @@ MAX_BRAKE = 255 +INTERCEPTOR_THRESHOLD = 328 + class TestHondaSafety(unittest.TestCase): @classmethod def setUp(cls): @@ -66,7 +68,9 @@ def _send_interceptor_msg(self, gas, addr): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = addr << 21 to_send[0].RDTR = 6 - to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) + gas2 = gas * 2 + to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ + ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) return to_send @@ -176,16 +180,15 @@ def test_allow_engage_with_gas_pressed(self): def test_disengage_on_gas_interceptor(self): for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - if long_controls_allowed: - self.assertFalse(self.safety.get_controls_allowed()) - else: - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_gas_interceptor_detected(False) + for g in range(0, 0x1000): + self.safety.set_long_controls_allowed(long_controls_allowed) + self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201)) + remain_enabled = (not long_controls_allowed or g <= INTERCEPTOR_THRESHOLD) + self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self.safety.set_gas_interceptor_detected(False) self.safety.set_long_controls_allowed(True) def test_allow_engage_with_gas_interceptor_pressed(self): diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index 7dd1601d7712c3..dc5b21ac8f7b42 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -15,6 +15,8 @@ MAX_TORQUE_ERROR = 350 +INTERCEPTOR_THRESHOLD = 475 + def twos_comp(val, bits): if val >= 0: return val @@ -81,7 +83,9 @@ def _send_interceptor_msg(self, gas, addr): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = addr << 21 to_send[0].RDTR = 6 - to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) + gas2 = gas * 2 + to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ + ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) return to_send @@ -145,16 +149,15 @@ def test_allow_engage_with_gas_pressed(self): def test_disengage_on_gas_interceptor(self): for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - if long_controls_allowed: - self.assertFalse(self.safety.get_controls_allowed()) - else: - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_gas_interceptor_detected(False) + for g in range(0, 0x1000): + self.safety.set_long_controls_allowed(long_controls_allowed) + self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201)) + remain_enabled = (not long_controls_allowed or g <= INTERCEPTOR_THRESHOLD) + self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self.safety.set_gas_interceptor_detected(False) self.safety.set_long_controls_allowed(True) def test_allow_engage_with_gas_interceptor_pressed(self):