diff --git a/tests/safety/common.py b/tests/safety/common.py index 51745a8f38..c07f12a8a2 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -12,6 +12,10 @@ MAX_WRONG_COUNTERS = 5 +def sign_of(a): + return 1 if a > 0 else -1 + + def make_msg(bus, addr, length=8): return libpanda_py.make_CANPacket(addr, bus, b'\x00' * length) @@ -475,6 +479,91 @@ def test_torque_measurements(self): self.assertTrue(self.safety.get_torque_meas_max() in max_range) +class AngleSteeringSafetyTest(PandaSafetyTestBase): + + DEG_TO_CAN: int + + ANGLE_DELTA_BP: List[float] + ANGLE_DELTA_V: List[float] # windup limit + ANGLE_DELTA_VU: List[float] # unwind limit + + @classmethod + def setUpClass(cls): + if cls.__name__ == "AngleSteeringSafetyTest": + cls.safety = None + raise unittest.SkipTest + + @abc.abstractmethod + def _angle_cmd_msg(self, angle: float, enabled: bool): + pass + + @abc.abstractmethod + def _angle_meas_msg(self, angle: float): + pass + + def _set_prev_desired_angle(self, t): + t = int(t * self.DEG_TO_CAN) + self.safety.set_desired_angle_last(t) + + def _angle_meas_msg_array(self, angle): + for _ in range(6): + self._rx(self._angle_meas_msg(angle)) + + def test_angle_cmd_when_enabled(self): + # when controls are allowed, angle cmd rate limit is enforced + speeds = [0., 1., 5., 10., 15., 50.] + angles = [-300, -100, -10, 0, 10, 100, 300] + for a in angles: + for s in speeds: + max_delta_up = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_V) + max_delta_down = np.interp(s, self.ANGLE_DELTA_BP, self.ANGLE_DELTA_VU) + + # first test against false positives + self._angle_meas_msg_array(a) + self._rx(self._speed_msg(s)) # pylint: disable=no-member + + self._set_prev_desired_angle(a) + self.safety.set_controls_allowed(1) + + # Stay within limits + # Up + self.assertTrue(self._tx(self._angle_cmd_msg(a + sign_of(a) * max_delta_up, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Don't change + self.assertTrue(self._tx(self._angle_cmd_msg(a, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Down + self.assertTrue(self._tx(self._angle_cmd_msg(a - sign_of(a) * max_delta_down, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Inject too high rates + # Up + self.assertFalse(self._tx(self._angle_cmd_msg(a + sign_of(a) * (max_delta_up + 1.1), True))) + + # Don't change + self.safety.set_controls_allowed(1) + self._set_prev_desired_angle(a) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertTrue(self._tx(self._angle_cmd_msg(a, True))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Down + self.assertFalse(self._tx(self._angle_cmd_msg(a - sign_of(a) * (max_delta_down + 1.1), True))) + + # Check desired steer should be the same as steer angle when controls are off + self.safety.set_controls_allowed(0) + self.assertTrue(self._tx(self._angle_cmd_msg(a, False))) + + def test_angle_cmd_when_disabled(self): + self.safety.set_controls_allowed(0) + + self._set_prev_desired_angle(0) + self.assertFalse(self._tx(self._angle_cmd_msg(0, True))) + self.assertFalse(self.safety.get_controls_allowed()) + + @add_regen_tests class PandaSafetyTest(PandaSafetyTestBase): TX_MSGS: Optional[List[List[int]]] = None diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index 74c34c6d39..24e266fd14 100755 --- a/tests/safety/test_nissan.py +++ b/tests/safety/test_nissan.py @@ -1,21 +1,12 @@ #!/usr/bin/env python3 import unittest -import numpy as np from panda import Panda from panda.tests.libpanda import libpanda_py import panda.tests.safety.common as common from panda.tests.safety.common import CANPackerPanda -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit - -def sign(a): - return 1 if a > 0 else -1 - - -class TestNissanSafety(common.PandaSafetyTest): +class TestNissanSafety(common.PandaSafetyTest, common.AngleSteeringSafetyTest): TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]] STANDSTILL_THRESHOLD = 0 @@ -25,32 +16,30 @@ class TestNissanSafety(common.PandaSafetyTest): FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2b1, 0x4cc]} FWD_BUS_LOOKUP = {0: 2, 2: 0} + DEG_TO_CAN = -100 + + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + def setUp(self): self.packer = CANPackerPanda("nissan_x_trail_2017") self.safety = libpanda_py.libpanda self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) self.safety.init_tests() + def _angle_cmd_msg(self, angle, state): + values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": state} + return self.packer.make_can_msg_panda("LKAS", 0, values) + def _angle_meas_msg(self, angle): values = {"STEER_ANGLE": angle} return self.packer.make_can_msg_panda("STEER_ANGLE_SENSOR", 0, values) - def _set_prev_angle(self, t): - t = int(t * -100) - self.safety.set_desired_angle_last(t) - - def _angle_meas_msg_array(self, angle): - for _ in range(6): - self._rx(self._angle_meas_msg(angle)) - def _pcm_status_msg(self, enable): values = {"CRUISE_ENABLED": enable} return self.packer.make_can_msg_panda("CRUISE_STATE", 2, values) - def _lkas_control_msg(self, angle, state): - values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": state} - return self.packer.make_can_msg_panda("LKAS", 0, values) - def _speed_msg(self, speed): # TODO: why the 3.6? m/s to kph? not in dbc values = {"WHEEL_SPEED_%s" % s: speed * 3.6 for s in ["RR", "RL"]} @@ -71,60 +60,6 @@ def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0): "RES_BUTTON": res, "NO_BUTTON_PRESSED": no_button} return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 2, values) - def test_angle_cmd_when_enabled(self): - # when controls are allowed, angle cmd rate limit is enforced - speeds = [0., 1., 5., 10., 15., 50.] - angles = [-300, -100, -10, 0, 10, 100, 300] - for a in angles: - for s in speeds: - max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) - max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) - - # first test against false positives - self._angle_meas_msg_array(a) - self._rx(self._speed_msg(s)) - - self._set_prev_angle(a) - self.safety.set_controls_allowed(1) - - # Stay within limits - # Up - self.assertEqual(True, self._tx(self._lkas_control_msg(a + sign(a) * max_delta_up, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Don't change - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Inject too high rates - # Up - self.assertEqual(False, self._tx(self._lkas_control_msg(a + sign(a) * (max_delta_up + 1), 1))) - - # Don't change - self.safety.set_controls_allowed(1) - self._set_prev_angle(a) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(False, self._tx(self._lkas_control_msg(a - sign(a) * (max_delta_down + 1), 1))) - - # Check desired steer should be the same as steer angle when controls are off - self.safety.set_controls_allowed(0) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 0))) - - def test_angle_cmd_when_disabled(self): - self.safety.set_controls_allowed(0) - - self._set_prev_angle(0) - self.assertFalse(self._tx(self._lkas_control_msg(0, 1))) - self.assertFalse(self.safety.get_controls_allowed()) - def test_acc_buttons(self): btns = [ ("cancel", True), @@ -141,6 +76,7 @@ def test_acc_buttons(self): tx = self._tx(self._acc_button_cmd(**args)) self.assertEqual(tx, should_tx) + class TestNissanLeafSafety(TestNissanSafety): def setUp(self): @@ -161,5 +97,6 @@ def _user_gas_msg(self, gas): def test_acc_buttons(self): pass + if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_tesla.py b/tests/safety/test_tesla.py index 370cfda5c5..cdca23bb19 100755 --- a/tests/safety/test_tesla.py +++ b/tests/safety/test_tesla.py @@ -6,13 +6,10 @@ from panda.tests.libpanda import libpanda_py from panda.tests.safety.common import CANPackerPanda -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit - MAX_ACCEL = 2.0 MIN_ACCEL = -3.5 + class CONTROL_LEVER_STATE: DN_1ST = 32 UP_1ST = 16 @@ -22,8 +19,6 @@ class CONTROL_LEVER_STATE: FWD = 1 IDLE = 0 -def sign(a): - return 1 if a > 0 else -1 class TestTeslaSafety(common.PandaSafetyTest): STANDSTILL_THRESHOLD = 0 @@ -35,22 +30,6 @@ def setUp(self): self.packer = None raise unittest.SkipTest - def _angle_meas_msg(self, angle): - values = {"EPAS_internalSAS": angle} - return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) - - def _set_prev_angle(self, t): - t = int(t * 10) - self.safety.set_desired_angle_last(t) - - def _angle_meas_msg_array(self, angle): - for _ in range(6): - self._rx(self._angle_meas_msg(angle)) - - def _lkas_control_msg(self, angle, enabled): - values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0} - return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values) - def _speed_msg(self, speed): values = {"DI_vehicleSpeed": speed / 0.447} return self.packer.make_can_msg_panda("DI_torque2", 0, values) @@ -83,70 +62,31 @@ def _long_control_msg(self, set_speed, acc_val=0, jerk_limits=(0, 0), accel_limi } return self.packer.make_can_msg_panda("DAS_control", 0, values) -class TestTeslaSteeringSafety(TestTeslaSafety): + +class TestTeslaSteeringSafety(TestTeslaSafety, common.AngleSteeringSafetyTest): TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2]] RELAY_MALFUNCTION_ADDR = 0x488 FWD_BLACKLISTED_ADDRS = {2: [0x488]} + DEG_TO_CAN = 10 + + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + def setUp(self): self.packer = CANPackerPanda("tesla_can") self.safety = libpanda_py.libpanda self.safety.set_safety_hooks(Panda.SAFETY_TESLA, 0) self.safety.init_tests() - def test_angle_cmd_when_enabled(self): - # when controls are allowed, angle cmd rate limit is enforced - speeds = [0., 1., 5., 10., 15., 50.] - angles = [-300, -100, -10, 0, 10, 100, 300] - for a in angles: - for s in speeds: - max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) - max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) - - # first test against false positives - self._angle_meas_msg_array(a) - self._rx(self._speed_msg(s)) - - self._set_prev_angle(a) - self.safety.set_controls_allowed(1) - - # Stay within limits - # Up - self.assertEqual(True, self._tx(self._lkas_control_msg(a + sign(a) * max_delta_up, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Don't change - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Inject too high rates - # Up - self.assertEqual(False, self._tx(self._lkas_control_msg(a + sign(a) * (max_delta_up + 1.1), 1))) - - # Don't change - self.safety.set_controls_allowed(1) - self._set_prev_angle(a) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # Down - self.assertEqual(False, self._tx(self._lkas_control_msg(a - sign(a) * (max_delta_down + 1.1), 1))) - - # Check desired steer should be the same as steer angle when controls are off - self.safety.set_controls_allowed(0) - self.assertEqual(True, self._tx(self._lkas_control_msg(a, 0))) - - def test_angle_cmd_when_disabled(self): - self.safety.set_controls_allowed(0) + def _angle_meas_msg(self, angle): + values = {"EPAS_internalSAS": angle} + return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values) - self._set_prev_angle(0) - self.assertFalse(self._tx(self._lkas_control_msg(0, 1))) - self.assertFalse(self.safety.get_controls_allowed()) + def _angle_cmd_msg(self, angle, enabled): + values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0} + return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values) def test_acc_buttons(self): """ @@ -205,6 +145,7 @@ def test_acc_accel_limits(self): send = np.all(np.isclose([min_accel, max_accel], 0, atol=0.0001)) self.assertEqual(send, self._tx(self._long_control_msg(10, acc_val=4, accel_limits=[min_accel, max_accel]))) + class TestTeslaChassisLongitudinalSafety(TestTeslaLongitudinalSafety): TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2], [0x2B9, 0]] RELAY_MALFUNCTION_ADDR = 0x488 @@ -216,6 +157,7 @@ def setUp(self): self.safety.set_safety_hooks(Panda.SAFETY_TESLA, Panda.FLAG_TESLA_LONG_CONTROL) self.safety.init_tests() + class TestTeslaPTLongitudinalSafety(TestTeslaLongitudinalSafety): TX_MSGS = [[0x2BF, 0]] RELAY_MALFUNCTION_ADDR = 0x2BF @@ -227,5 +169,6 @@ def setUp(self): self.safety.set_safety_hooks(Panda.SAFETY_TESLA, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN) self.safety.init_tests() + if __name__ == "__main__": unittest.main()