safety tests: common angle tests (commaai#1185)

* basic formatting * common tests * assert truthfulness * ignore single pylint exception * wya too many lines
budney · Dec 3, 2022 · 6e8a4fc · 6e8a4fc
1 parent 616450c
commit 6e8a4fc
Show file tree

Hide file tree

Showing 3 changed files with 120 additions and 151 deletions.
diff --git 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
@@ -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
@@ -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()