Pedal: use avg between tracks (commaai#253)

emmertex · Jul 15, 2019 · e49d0db · e49d0db
1 parent c597dcc
commit e49d0db
Show file tree

Hide file tree

Showing 5 changed files with 33 additions and 24 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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):