Ford safety: curvature error limit (commaai#1353)

* set ford vehicle speed

* parse yaw rate signals

* misra

* misra

* misra

* misra

* draft

* update module

* already checked

* and set it properly

* some stuff

* draft

* clean up (will fail tests because we don't send yaw rate yet)

* could do something like this

* this is better and less prone to bugs

* match simple op limiting, debugging

* set checksum for messages in tests

* clean up

* fix that

* one m/s fudge

* fix sign of yaw rate

* interpolate detects size

* forgot OP flips the curvature sign. it matches yaw on can

* all my debugging crap

* make replay work for ford

* fix panda blocking messages (array is fixed size so size-1 is 0 rate at high speed)

* uncomment safety test limits

* revert

* round for zero blocked msgs

* fix limits

* meas safety checks that down rate is >=, not <

* test pass

* lots of comments and draft what one meas torque check would look like

* fix that

* add curvature meas

* add debugging stuff

* Revert "add debugging stuff"

This reverts commit 449783f.

* messy but at least one test passes now

* draft

* add max_steer

* some safety clean up

* and that

* start with a test that works

* another test that works (sort of, we need more strict panda safety without false positives)

* no max curvature check (not safety related), allow any rate limits

* add new function

* also need to consider max val here, since OP will send up to that

* and now use the function

* lower to 10

* compilation fixes

* clean up (no rate limiting)

* remove that too

* curvature diff test

* more clean up

* debug

* ?

* better names

* more official

* use _curvature_meas_msg_array here

* bit faster

* no i don't

* revert that

* why not just use angle_meas?

* bb ll

* bb deb

* clean up debug vals

* more

* revert replay drive debugging changes

* Update board/safety.h

* rm line

* only need to round the final thing

* not needed, under 10 ms

* make a class variable

* fix a misra?

* another misra?

better

* ?

* 12.1

* need to explicitly convert

* add one to not false trigger the violation (float rounding)

* not really needed

* rm line

* cmt

* use clamp

* rename

* in struct

* comment

* use max_limit_check

* draft clean up

* Revert "draft clean up"

This reverts commit d1a0e8a.

* make a global

make a global

* this is fine

board/safety.h

@@ -411,7 +411,19 @@ bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
   return (val > MAX_VAL) || (val < MIN_VAL);
 }
 
-// check that commanded value isn't too far from measured
+// check that commanded angle value isn't too far from measured
+bool angle_dist_to_meas_check(int val, struct sample_t *val_meas, const int MAX_ERROR, const int MAX_VAL) {
+
+  // val must always be near val_meas, limited to the maximum value
+  // add 1 to not false trigger the violation
+  int highest_allowed = CLAMP(val_meas->max + MAX_ERROR + 1, -MAX_VAL, MAX_VAL);
+  int lowest_allowed = CLAMP(val_meas->min - MAX_ERROR - 1, -MAX_VAL, MAX_VAL);
+
+  // check for violation
+  return max_limit_check(val, highest_allowed, lowest_allowed);
+}
+
+// check that commanded torque value isn't too far from measured
 bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
                         const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
 

board/safety/safety_ford.h

@@ -124,6 +124,7 @@ static bool ford_get_quality_flag_valid(CANPacket_t *to_push) {
 #define INACTIVE_PATH_OFFSET 512U
 #define INACTIVE_PATH_ANGLE 1000U
 #define FORD_MAX_SPEED_DELTA 2.0  // m/s
+#define FORD_CURVATURE_DELTA_LIMIT_SPEED 10.0  // m/s
 
 static bool ford_lkas_msg_check(int addr) {
   return (addr == MSG_ACCDATA_3)
@@ -132,6 +133,13 @@ static bool ford_lkas_msg_check(int addr) {
       || (addr == MSG_IPMA_Data);
 }
 
+// Curvature rate limits
+const SteeringLimits FORD_STEERING_LIMITS = {
+  .max_steer = 1000,
+  .angle_deg_to_can = 50000,  // 1 / (2e-5) rad to can
+  .max_angle_error = 100,     // 0.002 * FORD_STEERING_LIMITS.angle_deg_to_can
+};
+
 static int ford_rx_hook(CANPacket_t *to_push) {
   bool valid = addr_safety_check(to_push, &ford_rx_checks,
                                  ford_get_checksum, ford_compute_checksum, ford_get_counter, ford_get_quality_flag_valid);
@@ -151,6 +159,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       vehicle_speed = ((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6;
     }
 
+    // Check vehicle speed against a second source
     if (addr == MSG_EngVehicleSpThrottle2) {
       // Disable controls if speeds from ABS and PCM ECUs are too far apart.
       // Signal: Veh_V_ActlEng
@@ -160,6 +169,17 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       }
     }
 
+    // Update vehicle yaw rate
+    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);
+      // 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);
+      update_sample(&angle_meas, current_curvature_can);
+    }
+
     // Update gas pedal
     if (addr == MSG_EngVehicleSpThrottle) {
       // Pedal position: (0.1 * val) in percent
@@ -225,20 +245,29 @@ static int ford_tx_hook(CANPacket_t *to_send) {
   // Safety check for LateralMotionControl action
   if (addr == MSG_LateralMotionControl) {
     // Signal: LatCtl_D_Rq
-    unsigned int steer_control_type = (GET_BYTE(to_send, 4) >> 2) & 0x7U;
-    unsigned int curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5);
-    unsigned int curvature_rate = ((GET_BYTE(to_send, 1) & 0x1FU) << 8) | GET_BYTE(to_send, 2);
-    unsigned int path_angle = (GET_BYTE(to_send, 3) << 3) | (GET_BYTE(to_send, 4) >> 5);
-    unsigned int path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6);
+    bool steer_control_enabled = ((GET_BYTE(to_send, 4) >> 2) & 0x7U) != 0U;
+    unsigned int raw_curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5);
+    unsigned int raw_curvature_rate = ((GET_BYTE(to_send, 1) & 0x1FU) << 8) | GET_BYTE(to_send, 2);
+    unsigned int raw_path_angle = (GET_BYTE(to_send, 3) << 3) | (GET_BYTE(to_send, 4) >> 5);
+    unsigned int raw_path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6);
 
     // These signals are not yet tested with the current safety limits
-    if ((curvature_rate != INACTIVE_CURVATURE_RATE) || (path_angle != INACTIVE_PATH_ANGLE) || (path_offset != INACTIVE_PATH_OFFSET)) {
-      tx = 0;
+    bool violation = (raw_curvature_rate != INACTIVE_CURVATURE_RATE) || (raw_path_angle != INACTIVE_PATH_ANGLE) || (raw_path_offset != INACTIVE_PATH_OFFSET);
+
+    int desired_curvature = raw_curvature - INACTIVE_CURVATURE;  // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature
+    if (controls_allowed) {
+      if (vehicle_speed > FORD_CURVATURE_DELTA_LIMIT_SPEED) {
+        violation |= angle_dist_to_meas_check(desired_curvature, &angle_meas,
+                                              FORD_STEERING_LIMITS.max_angle_error, FORD_STEERING_LIMITS.max_steer);
+      }
     }
 
-    // No steer control allowed when controls are not allowed
-    bool steer_control_enabled = (steer_control_type != 0U) || (curvature != INACTIVE_CURVATURE);
-    if (!controls_allowed && steer_control_enabled) {
+    // No curvature command if controls is not allowed
+    if (!controls_allowed && ((desired_curvature != 0) || steer_control_enabled)) {
+      violation = true;
+    }
+
+    if (violation) {
       tx = 0;
     }
   }

board/safety_declarations.h

@@ -68,6 +68,7 @@ typedef struct {
   const int angle_deg_to_can;
   const struct lookup_t angle_rate_up_lookup;
   const struct lookup_t angle_rate_down_lookup;
+  const int max_angle_error;  // used to limit error between meas and cmd
 } SteeringLimits;
 
 typedef struct {
@@ -124,6 +125,8 @@ uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
 int to_signed(int d, int bits);
 void update_sample(struct sample_t *sample, int sample_new);
 bool max_limit_check(int val, const int MAX, const int MIN);
+bool angle_dist_to_meas_check(int val, struct sample_t *val_meas,
+  const int MAX_ERROR, const int MAX_VAL);
 bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
   const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
 bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
@@ -208,7 +211,7 @@ uint32_t heartbeat_engaged_mismatches = 0;  // count of mismatches between heart
 // for safety modes with angle steering control
 uint32_t ts_angle_last = 0;
 int desired_angle_last = 0;
-struct sample_t angle_meas;         // last 6 steer angles
+struct sample_t angle_meas;         // last 6 steer angles/curvatures
 
 // This can be set with a USB command
 // It enables features that allow alternative experiences, like not disengaging on gas press

tests/safety/test_ford.py

@@ -50,6 +50,11 @@ def checksum(msg):
   return addr, t, ret, bus
 
 
+def round_curvature_can(curvature):
+  # rounds curvature as if it was sent on CAN
+  return round(curvature * 5, 4) / 5
+
+
 class Buttons:
   CANCEL = 0
   RESUME = 1
@@ -68,8 +73,15 @@ class TestFordSafety(common.PandaSafetyTest):
   FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]}
   FWD_BUS_LOOKUP = {0: 2, 2: 0}
 
+  # Max allowed delta between car speeds
   MAX_SPEED_DELTA = 2.0  # m/s
 
+  # Curvature control limits
+  DEG_TO_CAN = 50000  # 1 / (2e-5) rad to can
+  MAX_CURVATURE = 0.02
+  MAX_CURVATURE_DELTA = 0.002
+  CURVATURE_DELTA_LIMIT_SPEED = 10.0  # m/s
+
   cnt_speed = 0
   cnt_speed_2 = 0
   cnt_yaw_rate = 0
@@ -80,6 +92,11 @@ def setUp(self):
     self.safety.set_safety_hooks(Panda.SAFETY_FORD, 0)
     self.safety.init_tests()
 
+  def _reset_curvature_measurements(self, curvature, speed):
+    self._rx(self._speed_msg(speed))
+    for _ in range(6):
+      self._rx(self._yaw_rate_msg(curvature, speed))
+
   # Driver brake pedal
   def _user_brake_msg(self, brake: bool):
     # brake pedal and cruise state share same message, so we have to send
@@ -213,6 +230,20 @@ def test_steer_allowed(self):
                 should_tx = should_tx and (not enabled or controls_allowed)
                 self.assertEqual(should_tx, self._tx(self._tja_command_msg(steer_control_enabled, path_offset, path_angle, curvature, curvature_rate)))
 
+  def test_steer_meas_delta(self):
+    """This safety model enforces a maximum distance from measured and commanded curvature, only above a certain speed"""
+    self.safety.set_controls_allowed(1)
+
+    for speed in np.linspace(0, 50, 11):
+      for initial_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 51):
+        self._reset_curvature_measurements(initial_curvature, speed)
+
+        limit_command = speed > self.CURVATURE_DELTA_LIMIT_SPEED
+        for new_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 51):
+          too_far_away = round_curvature_can(abs(new_curvature - initial_curvature)) > self.MAX_CURVATURE_DELTA
+          should_tx = not limit_command or not too_far_away
+          self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, new_curvature, 0)))
+
   def test_prevent_lkas_action(self):
     self.safety.set_controls_allowed(1)
     self.assertFalse(self._tx(self._lkas_command_msg(1)))