safety: make vehicle_speed a sample (#1391)

* convert vehicle_speed into sample_t, change no behavior

* draft

* round

* test

* clean up

* round

* round all

* use min

* remove round macro from this PR

* reset speed measurement

* debug

* bbd

* rm

* revert

* test above and below

* need this now

* misra pt 1

* misra pt 2

* misra pt 3

* i don't understand this one, not different from other cases

* fix test

* test

* revert that

* draft

* test the sample_t works properly for safety modes that use it (angle only)

* can combine these tests

* test decimals

* global

* misra

comment

* suggestions

* fix

* use new helper

board/safety.h

@@ -332,7 +332,6 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   regen_braking = false;
   regen_braking_prev = false;
   cruise_engaged_prev = false;
-  vehicle_speed = 0;
   vehicle_moving = false;
   acc_main_on = false;
   cruise_button_prev = 0;
@@ -345,6 +344,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   valid_steer_req_count = 0;
   invalid_steer_req_count = 0;
 
+  vehicle_speed.min = 0;
+  vehicle_speed.max = 0;
   torque_meas.min = 0;
   torque_meas.max = 0;
   torque_driver.min = 0;
@@ -620,8 +621,8 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
     // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are
     // always slightly above openpilot's in case we read an updated speed in between angle commands
     // TODO: this speed fudge can be much lower, look at data to determine the lowest reasonable offset
-    int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, vehicle_speed - 1.) * limits.angle_deg_to_can) + 1.;
-    int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, vehicle_speed - 1.) * limits.angle_deg_to_can) + 1.;
+    int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.;
+    int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.;
 
     // allow down limits at zero since small floats will be rounded to 0
     int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
@@ -634,7 +635,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
 
   // check that commanded angle value isn't too far from measured, used to limit torque for some safety modes
   if (limits.enforce_angle_error && controls_allowed && steer_control_enabled) {
-    if (vehicle_speed > limits.angle_error_min_speed) {
+    if ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed) {
       // val must always be near angle_meas, limited to the maximum value
       // add 1 to not false trigger the violation
       int highest_allowed = CLAMP(angle_meas.max + limits.max_angle_error + 1, -limits.max_steer, limits.max_steer);

board/safety/safety_ford.h

@@ -163,7 +163,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
     // Update vehicle speed
     if (addr == MSG_BrakeSysFeatures) {
       // Signal: Veh_V_ActlBrk
-      vehicle_speed = ((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6;
+      update_sample(&vehicle_speed, (((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6 * VEHICLE_SPEED_FACTOR) + 0.5);
     }
 
     // Check vehicle speed against a second source
@@ -171,7 +171,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       // Disable controls if speeds from ABS and PCM ECUs are too far apart.
       // Signal: Veh_V_ActlEng
       float filtered_pcm_speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01 / 3.6;
-      if (ABS(filtered_pcm_speed - vehicle_speed) > FORD_MAX_SPEED_DELTA) {
+      if (ABS(filtered_pcm_speed - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > FORD_MAX_SPEED_DELTA) {
         controls_allowed = 0;
       }
     }
@@ -180,7 +180,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
     if (addr == MSG_Yaw_Data_FD1) {
       // Signal: VehYaw_W_Actl
       float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5;
-      float current_curvature = ford_yaw_rate / MAX(vehicle_speed, 0.1);
+      float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
       // convert current curvature into units on CAN for comparison with desired curvature
       int current_curvature_can = current_curvature * (float)FORD_STEERING_LIMITS.angle_deg_to_can +
                                   ((current_curvature > 0.) ? 0.5 : -0.5);

board/safety/safety_nissan.h

@@ -65,7 +65,7 @@ static int nissan_rx_hook(CANPacket_t *to_push) {
         uint16_t right_rear = (GET_BYTE(to_push, 0) << 8) | (GET_BYTE(to_push, 1));
         uint16_t left_rear = (GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3));
         vehicle_moving = (right_rear | left_rear) != 0U;
-        vehicle_speed = (right_rear + left_rear) / 2.0 * 0.005 / 3.6;
+        update_sample(&vehicle_speed, ((right_rear + left_rear) / 2.0 * 0.005 / 3.6 * VEHICLE_SPEED_FACTOR) + 0.5);
       }
 
       // X-Trail 0x15c, Leaf 0x239

board/safety/safety_tesla.h

@@ -80,8 +80,9 @@ static int tesla_rx_hook(CANPacket_t *to_push) {
 
       if(addr == (tesla_powertrain ? 0x116 : 0x118)) {
         // Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS
-        vehicle_speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
-        vehicle_moving = ABS(vehicle_speed) > 0.1;
+        float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
+        vehicle_moving = ABS(speed) > 0.1;
+        update_sample(&vehicle_speed, (speed * VEHICLE_SPEED_FACTOR) + 0.5);
       }
 
       if(addr == (tesla_powertrain ? 0x106 : 0x108)) {

board/safety_declarations.h

@@ -16,6 +16,8 @@ uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
 const int MAX_WRONG_COUNTERS = 5;
 const uint8_t MAX_MISSED_MSGS = 10U;
 #define MAX_ADDR_CHECK_MSGS 3U
+// used to represent floating point vehicle speed in a sample_t
+#define VEHICLE_SPEED_FACTOR 100.0
 
 // sample struct that keeps 6 samples in memory
 struct sample_t {
@@ -193,7 +195,7 @@ bool brake_pressed_prev = false;
 bool regen_braking = false;
 bool regen_braking_prev = false;
 bool cruise_engaged_prev = false;
-float vehicle_speed = 0;
+struct sample_t vehicle_speed;
 bool vehicle_moving = false;
 bool acc_main_on = false;  // referred to as "ACC off" in ISO 15622:2018
 int cruise_button_prev = 0;

tests/libpanda/safety_helpers.h

@@ -79,6 +79,14 @@ bool get_acc_main_on(void){
   return acc_main_on;
 }
 
+int get_vehicle_speed_min(void){
+  return vehicle_speed.min;
+}
+
+int get_vehicle_speed_max(void){
+  return vehicle_speed.max;
+}
+
 int get_current_safety_mode(void){
   return current_safety_mode;
 }

tests/libpanda/safety_helpers.py

@@ -16,6 +16,8 @@ def setup_safety_helpers(ffi):
   bool get_brake_pressed_prev(void);
   bool get_regen_braking_prev(void);
   bool get_acc_main_on(void);
+  int get_vehicle_speed_min(void);
+  int get_vehicle_speed_max(void);
   int get_current_safety_mode(void);
   int get_current_safety_param(void);
 
@@ -62,6 +64,8 @@ class PandaSafety:
   def get_brake_pressed_prev(self) -> bool: ...
   def get_regen_braking_prev(self) -> bool: ...
   def get_acc_main_on(self) -> bool: ...
+  def get_vehicle_speed_min(self) -> int: ...
+  def get_vehicle_speed_max(self) -> int: ...
   def get_current_safety_mode(self) -> int: ...
   def get_current_safety_param(self) -> int: ...
 

tests/safety/common.py

@@ -10,6 +10,7 @@ from panda import ALTERNATIVE_EXPERIENCE
 from panda.tests.libpanda import libpanda_py
 
 MAX_WRONG_COUNTERS = 5
+VEHICLE_SPEED_FACTOR = 100
 
 
 def sign_of(a):
@@ -550,6 +551,10 @@ class AngleSteeringSafetyTest(PandaSafetyTestBase):
       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
@@ -566,6 +571,10 @@ class AngleSteeringSafetyTest(PandaSafetyTestBase):
     for _ in range(6):
       self._rx(self._angle_meas_msg(angle))
 
+  def _reset_speed_measurement(self, speed):
+    for _ in range(6):
+      self._rx(self._speed_msg(speed))
+
   def test_angle_cmd_when_enabled(self):
     # when controls are allowed, angle cmd rate limit is enforced
     speeds = [0., 1., 5., 10., 15., 50.]
@@ -577,7 +586,7 @@ class AngleSteeringSafetyTest(PandaSafetyTestBase):
 
         # first test against false positives
         self._reset_angle_measurement(a)
-        self._rx(self._speed_msg(s))  # pylint: disable=no-member
+        self._reset_speed_measurement(s)
 
         self._set_prev_desired_angle(a)
         self.safety.set_controls_allowed(1)
@@ -641,6 +650,26 @@ class AngleSteeringSafetyTest(PandaSafetyTestBase):
     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):