forked from IQ.Lvbs/IQ.Pilot
83 lines
2.9 KiB
Python
83 lines
2.9 KiB
Python
"""
|
|
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)))
|