IQ.Pilot Release Commit @ fddcfba

iqdbc_repo/iqdbc/safety/tests/gas_interceptor_common.py

@@ -0,0 +1,82 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import numpy as np
+import unittest
+
+from iqdbc.safety.tests.common import CANPackerSafety, CarSafetyTest
+
+
+class GasInterceptorSafetyTest(CarSafetyTest):
+
+  INTERCEPTOR_THRESHOLD = 0
+
+  cnt_gas_cmd = 0
+  cnt_user_gas = 0
+
+  packer: CANPackerSafety
+
+  @classmethod
+  def setUpClass(cls):
+    if cls.__name__ == "GasInterceptorSafetyTest" or cls.__name__.endswith("Base"):
+      cls.safety = None
+      raise unittest.SkipTest
+
+  def _interceptor_gas_cmd(self, gas: int):
+    values: dict[str, float | int] = {"PEDAL_COUNTER": self.__class__.cnt_gas_cmd & 0xF}
+    if gas > 0:
+      values["GAS_COMMAND"] = gas * 255.
+      values["GAS_COMMAND2"] = gas * 255.
+    self.__class__.cnt_gas_cmd += 1
+    return self.packer.make_can_msg_safety("GAS_COMMAND", 0, values)
+
+  def _interceptor_user_gas(self, gas: int):
+    values = {"INTERCEPTOR_GAS": gas, "INTERCEPTOR_GAS2": gas,
+              "PEDAL_COUNTER": self.__class__.cnt_user_gas}
+    self.__class__.cnt_user_gas += 1
+    return self.packer.make_can_msg_safety("GAS_SENSOR", 0, values)
+
+  # Skip non-interceptor user gas tests
+  def test_prev_gas(self):
+    pass
+
+  def test_no_disengage_on_gas(self):
+    pass
+
+  def test_prev_gas_interceptor(self):
+    self._rx(self._interceptor_user_gas(0x0))
+    self.assertFalse(self.safety.get_gas_interceptor_prev())
+    self._rx(self._interceptor_user_gas(0x1000))
+    self.assertTrue(self.safety.get_gas_interceptor_prev())
+    self._rx(self._interceptor_user_gas(0x0))
+
+  def test_no_disengage_on_gas_interceptor(self):
+    self.safety.set_controls_allowed(True)
+    for g in range(0x1000):
+      self._rx(self._interceptor_user_gas(g))
+      # Test we allow lateral, but not longitudinal
+      self.assertTrue(self.safety.get_controls_allowed())
+      self.assertEqual(g <= self.INTERCEPTOR_THRESHOLD, self.safety.get_longitudinal_brake_allowed())
+      self.assertTrue(self.safety.get_longitudinal_gas_allowed())
+      # Make sure we can re-gain longitudinal actuation
+      self._rx(self._interceptor_user_gas(0))
+      self.assertTrue(self.safety.get_longitudinal_brake_allowed())
+      self.assertTrue(self.safety.get_longitudinal_gas_allowed())
+
+  def test_allow_engage_with_gas_interceptor_pressed(self):
+    self._rx(self._interceptor_user_gas(0x1000))
+    self.safety.set_controls_allowed(True)
+    self._rx(self._interceptor_user_gas(0x1000))
+    self.assertTrue(self.safety.get_controls_allowed())
+    self._rx(self._interceptor_user_gas(0))
+
+  def test_gas_interceptor_safety_check(self):
+    for gas in np.arange(0, 4000, 100):
+      for controls_allowed in [True, False]:
+        self.safety.set_controls_allowed(controls_allowed)
+        if controls_allowed:
+          send = True
+        else:
+          send = gas == 0
+        self.assertEqual(send, self._tx(self._interceptor_gas_cmd(gas)))