Subaru: Add measured steering angle and vehicle speed

board/safety/safety_subaru.h

@@ -106,6 +106,10 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
       torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 16) & 0x7FFU);
       torque_driver_new = -1 * to_signed(torque_driver_new, 11);
       update_sample(&torque_driver, torque_driver_new);
+
+      int angle_meas_new = (GET_BYTES(to_push, 4, 2) & 0xFFFFU);
+      angle_meas_new = ROUND(to_signed(angle_meas_new, 16));
+      update_sample(&angle_meas, angle_meas_new);
     }
 
     // enter controls on rising edge of ACC, exit controls on ACC off
@@ -116,7 +120,15 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
 
     // update vehicle moving with any non-zero wheel speed
     if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_main_bus)) {
-      vehicle_moving = ((GET_BYTES(to_push, 0, 4) >> 12) != 0U) || (GET_BYTES(to_push, 4, 4) != 0U);
+      uint32_t fr = (GET_BYTES(to_push, 1, 3) >> 4) & 0x1FFFU;
+      uint32_t rr = (GET_BYTES(to_push, 3, 3) >> 1) & 0x1FFFU;
+      uint32_t rl = (GET_BYTES(to_push, 4, 3) >> 6) & 0x1FFFU;
+      uint32_t fl = (GET_BYTES(to_push, 6, 2) >> 3) & 0x1FFFU;
+
+      vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
+
+      float speed = (fr + rr + rl + fl) / 4U * 0.057;
+      update_sample(&vehicle_speed, ROUND(speed * VEHICLE_SPEED_FACTOR));
     }
 
     if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_main_bus)) {

tests/safety/common.py

@@ -537,11 +537,46 @@ def test_reset_torque_measurements(self):
     self.assertEqual(self.safety.get_torque_meas_min(), 0)
     self.assertEqual(self.safety.get_torque_meas_max(), 0)
 
+class MeasurementSafetyTest(PandaSafetyTestBase):
+  DEG_TO_CAN: float = 1
 
-class AngleSteeringSafetyTest(PandaSafetyTestBase):
+  @classmethod
+  def setUpClass(cls):
+    if cls.__name__ == "MeasurementSafetyTest":
+      cls.safety = None
+      raise unittest.SkipTest
+
+  @abc.abstractmethod
+  def _angle_meas_msg(self, angle: float):
+    pass
 
-  DEG_TO_CAN: int
+  @abc.abstractmethod
+  def _speed_msg(self, speed):
+    pass
+
+  def common_measurement_test(self, msg_func, min_value, max_value, factor, get_min_func, get_max_func):
+    for val in np.arange(min_value, max_value, 0.5):
+      for i in range(6):
+        self.assertTrue(self._rx(msg_func(val + i * 0.1)))
 
+      # assert close by one decimal place
+      self.assertLessEqual(abs(get_min_func() - val * factor), 1 * abs(factor))
+      self.assertLessEqual(abs(get_max_func() - (val + 0.5) * factor), 1 * abs(factor))
+
+      # reset sample_t by reinitializing the safety mode
+      self._reset_safety_hooks()
+
+      self.assertEqual(get_min_func(), 0)
+      self.assertEqual(get_max_func(), 0)
+
+  def test_vehicle_speed_measurements(self):
+    self.common_measurement_test(self._speed_msg, 0, 80, VEHICLE_SPEED_FACTOR, self.safety.get_vehicle_speed_min, self.safety.get_vehicle_speed_max)
+
+  def test_steering_angle_measurements(self):
+    self.common_measurement_test(self._angle_meas_msg, -180, 180, self.DEG_TO_CAN, self.safety.get_angle_meas_min, self.safety.get_angle_meas_max)
+
+
+class AngleSteeringSafetyTest(MeasurementSafetyTest):
   ANGLE_RATE_BP: List[float]
   ANGLE_RATE_UP: List[float]  # windup limit
   ANGLE_RATE_DOWN: List[float]  # unwind limit
@@ -552,18 +587,10 @@ def setUpClass(cls):
       cls.safety = None
       raise unittest.SkipTest
 
-  @abc.abstractmethod
-  def _speed_msg(self, speed):
-    pass
-
   @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)
@@ -640,37 +667,6 @@ def test_angle_cmd_when_disabled(self):
             should_tx = controls_allowed if steer_control_enabled else angle_cmd == angle_meas
             self.assertEqual(should_tx, self._tx(self._angle_cmd_msg(angle_cmd, steer_control_enabled)))
 
-  def test_reset_angle_measurements(self):
-    # Tests that the angle measurement sample_t is reset on safety mode init
-    for a in np.linspace(-90, 90, 6):
-      self.assertTrue(self._rx(self._angle_meas_msg(a)))
-
-    # reset sample_t by reinitializing the safety mode
-    self._reset_safety_hooks()
-
-    self.assertEqual(self.safety.get_angle_meas_min(), 0)
-    self.assertEqual(self.safety.get_angle_meas_max(), 0)
-
-  def test_vehicle_speed_measurements(self):
-    """
-    Tests:
-     - rx hook correctly parses and rounds the vehicle speed
-     - sample is reset on safety mode init
-    """
-    for speed in np.arange(0, 80, 0.5):
-      for i in range(6):
-        self.assertTrue(self._rx(self._speed_msg(speed + i * 0.1)))
-
-      # assert close by one decimal place
-      self.assertLessEqual(abs(self.safety.get_vehicle_speed_min() - speed * VEHICLE_SPEED_FACTOR), 1)
-      self.assertLessEqual(abs(self.safety.get_vehicle_speed_max() - (speed + 0.5) * VEHICLE_SPEED_FACTOR), 1)
-
-      # reset sample_t by reinitializing the safety mode
-      self._reset_safety_hooks()
-
-      self.assertEqual(self.safety.get_vehicle_speed_min(), 0)
-      self.assertEqual(self.safety.get_vehicle_speed_max(), 0)
-
 
 @add_regen_tests
 class PandaSafetyTest(PandaSafetyTestBase):

tests/safety/test_subaru.py

@@ -3,7 +3,7 @@
 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
+from panda.tests.safety.common import CANPackerPanda, MeasurementSafetyTest
 
 
 MSG_SUBARU_Brake_Status     = 0x13c
@@ -32,7 +32,7 @@ def lkas_tx_msgs(alt_bus):
           [MSG_SUBARU_ES_Infotainment,  SUBARU_MAIN_BUS]]
 
 
-class TestSubaruSafetyBase(common.PandaSafetyTest, common.DriverTorqueSteeringSafetyTest):
+class TestSubaruSafetyBase(common.PandaSafetyTest, common.DriverTorqueSteeringSafetyTest, MeasurementSafetyTest):
   FLAGS = 0
   STANDSTILL_THRESHOLD = 0 # kph
   RELAY_MALFUNCTION_ADDR = MSG_SUBARU_ES_LKAS
@@ -52,6 +52,8 @@ class TestSubaruSafetyBase(common.PandaSafetyTest, common.DriverTorqueSteeringSa
 
   ALT_BUS = SUBARU_MAIN_BUS
 
+  DEG_TO_CAN = -46.08
+
   def setUp(self):
     self.packer = CANPackerPanda("subaru_global_2017_generated")
     self.safety = libpanda_py.libpanda
@@ -68,10 +70,13 @@ def _torque_driver_msg(self, torque):
     return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
 
   def _speed_msg(self, speed):
-    # subaru safety doesn't use the scaled value, so undo the scaling
-    values = {s: speed * 0.057 for s in ["FR", "FL", "RR", "RL"]}
+    values = {s: speed for s in ["FR", "FL", "RR", "RL"]}
     return self.packer.make_can_msg_panda("Wheel_Speeds", self.ALT_BUS, values)
 
+  def _angle_meas_msg(self, angle):
+    values = {"Steering_Angle": angle}
+    return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
+
   def _user_brake_msg(self, brake):
     values = {"Brake": brake}
     return self.packer.make_can_msg_panda("Brake_Status", self.ALT_BUS, values)