Ford: curvature rate limits (commaai#1258)

* ford curvature rate limits draft * make common angle tests work with curvature * comment * no need for this * extra line * fix test * generic curvature test class * more reasonable limits * adjust limits * draft * works * works * clean up * add vehicle speed * works * clean up * clean up * more clean up * more clean up * lower * double * add updated bp * remove * can clean that up * draft * this works! * think that's the correct placement * try this * closer * use min * add/sub one to not falsely trip * remove old angle error safety * i'm not sure if clamp is more readable * fix that * fix * stash * fix these tests * ternary * floats are a pain * draft, works kinda * even better * round that * need tolerance * this should work (adding tol=1 wouldn't let us have multiple rate limits) * test works * clamp breaks if low is higher than high :((( down from 150 blocked msgs to 7! * no blocked msgs!!! * test a whole bunch * stash * stash * clean up test * clean up test to be more like torque (+ speeds) * clean up * cmt * test up * up and down are good * rename and remove * this is tested * uncomment * this is tested by ensuring we move towards error at a minimum rate * not used any more * revert common * clean up test_ford a bit more * some clean up (combine variables where it makes sense) * yeah can't use clamp since min isn't always < max, min(max(.. handles this * clean up * revert that * comments * cmt * another * that's old * misra! * Update board/safety/safety_ford.h * Update board/safety/safety_ford.h * add todo, fix test case * more clear, matches panda * add comment * Update tests/safety/test_ford.py * more fine speed increments * rm comment * better names * this is expected behavior (tested by common checks) * CURVATURE_ERROR_LIMIT_SPEED * better name? * pretty clean! * same for up * only used in one place now * these are now clear * common term * make vehicle_speed a sample_t * need to use values[0] * speed is a float * Revert "speed is a float" This reverts commit 01af02f. * Revert "need to use values[0]" This reverts commit 8f6d683. * Revert "make vehicle_speed a sample_t" This reverts commit ecd8dc8. * safety fixes for new speed sample * test fixes for new speed sample * fix misra and make intermediate variable * this isn't needed
csouers · May 4, 2023 · 94cd9a0 · 94cd9a0
1 parent b44df81
commit 94cd9a0
Show file tree

Hide file tree

Showing 4 changed files with 85 additions and 37 deletions.
diff --git a/board/safety.h b/board/safety.h
@@ -616,7 +616,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
 bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) {
   bool violation = false;
 
-  if (!limits.disable_angle_rate_limits && controls_allowed && steer_control_enabled) {
+  if (controls_allowed && steer_control_enabled) {
     // convert floating point angle rate limits to integers in the scale of the desired angle on CAN,
     // 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
@@ -628,23 +628,29 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
     int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
     int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);
 
-    // check for violation;
-    violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
-  }
-  desired_angle_last = desired_angle;
+    // check that commanded angle value isn't too far from measured, used to limit torque for some safety modes
+    // ensure we start moving in direction of meas while respecting rate limits if error is exceeded
+    if (limits.enforce_angle_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) {
+      // the rate limits above are liberally above openpilot's to avoid false positives.
+      // likewise, allow a lower rate for moving towards meas when error is exceeded
+      int delta_angle_up_lower = interpolate(limits.angle_rate_up_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can;
+      int delta_angle_down_lower = interpolate(limits.angle_rate_down_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can;
+
+      int highest_desired_angle_lower = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up_lower : delta_angle_down_lower);
+      int lowest_desired_angle_lower = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down_lower : delta_angle_up_lower);
 
-  // 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.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);
-      int lowest_allowed = CLAMP(angle_meas.min - limits.max_angle_error - 1, -limits.max_steer, limits.max_steer);
+      lowest_desired_angle = MIN(MAX(lowest_desired_angle, angle_meas.min - limits.max_angle_error - 1), highest_desired_angle_lower);
+      highest_desired_angle = MAX(MIN(highest_desired_angle, angle_meas.max + limits.max_angle_error + 1), lowest_desired_angle_lower);
 
-      // check for violation
-      violation |= max_limit_check(desired_angle, highest_allowed, lowest_allowed);
+      // don't enforce above the max steer
+      lowest_desired_angle = CLAMP(lowest_desired_angle, -limits.max_steer, limits.max_steer);
+      highest_desired_angle = CLAMP(highest_desired_angle, -limits.max_steer, limits.max_steer);
     }
+
+    // check for violation;
+    violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
   }
+  desired_angle_last = desired_angle;
 
   // Angle should either be 0 or same as current angle while not steering
   if (!steer_control_enabled) {

diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h
@@ -138,11 +138,18 @@ 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
+  .angle_rate_up_lookup = {
+    {5., 25., 25.},
+    {0.0002, 0.0001, 0.0001}
+  },
+  .angle_rate_down_lookup = {
+    {5., 25., 25.},
+    {0.000225, 0.00015, 0.00015}
+  },
 
   // no blending at low speed due to lack of torque wind-up and inaccurate current curvature
   .angle_error_min_speed = 10.0,    // m/s
 
-  .disable_angle_rate_limits = true,
   .enforce_angle_error = true,
   .inactive_angle_is_zero = true,
 };

diff --git a/board/safety_declarations.h b/board/safety_declarations.h
@@ -73,7 +73,6 @@ typedef struct {
   const int max_angle_error;             // used to limit error between meas and cmd while enabled
   const float angle_error_min_speed;     // minimum speed to start limiting angle error
 
-  const bool disable_angle_rate_limits;
   const bool enforce_angle_error;        // enables max_angle_error check
   const bool inactive_angle_is_zero;     // if false, enforces angle near meas when disabled (default)
 } SteeringLimits;

diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py
@@ -77,6 +77,10 @@ class TestFordSafety(common.PandaSafetyTest):
   MAX_CURVATURE_ERROR = 0.002
   CURVATURE_ERROR_MIN_SPEED = 10.0  # m/s
 
+  ANGLE_RATE_BP = [5., 25., 25.]
+  ANGLE_RATE_UP = [0.0002, 0.0001, 0.0001]  # windup limit
+  ANGLE_RATE_DOWN = [0.000225, 0.00015, 0.00015]  # unwind limit
+
   cnt_speed = 0
   cnt_speed_2 = 0
   cnt_yaw_rate = 0
@@ -87,9 +91,13 @@ def setUp(self):
     self.safety.set_safety_hooks(Panda.SAFETY_FORD, 0)
     self.safety.init_tests()
 
+  def _set_prev_desired_angle(self, t):
+    t = round(t * self.DEG_TO_CAN)
+    self.safety.set_desired_angle_last(t)
+
   def _reset_curvature_measurement(self, curvature, speed):
-    self._rx(self._speed_msg(speed))
     for _ in range(6):
+      self._rx(self._speed_msg(speed))
       self._rx(self._yaw_rate_msg(curvature, speed))
 
   # Driver brake pedal
@@ -240,6 +248,7 @@ def test_steer_allowed(self):
               for curvature_rate in curvature_rates:
                 for curvature in curvatures:
                   self.safety.set_controls_allowed(controls_allowed)
+                  self._set_prev_desired_angle(curvature)
                   self._reset_curvature_measurement(curvature, speed)
 
                   should_tx = path_offset == 0 and path_angle == 0 and curvature_rate == 0
@@ -251,28 +260,55 @@ def test_steer_allowed(self):
                                     curvature=curvature):
                     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 steer_control_enabled in (True, False):
-      for speed in np.linspace(0, 50, 11):
-        for initial_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 21):
-          self._reset_curvature_measurement(initial_curvature, speed)
+  def test_curvature_rate_limit_up(self):
+    """
+    When the curvature error is exceeded, commanded curvature must start moving towards meas respecting rate limits.
+    Since panda allows higher rate limits to avoid false positives, we need to allow a lower rate to move towards meas.
+    """
+    self.safety.set_controls_allowed(True)
+    small_curvature = 2 / self.DEG_TO_CAN  # significant small amount of curvature to cross boundary
 
-          limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED
-          for new_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 41):
-            too_far_away = round(abs(new_curvature - initial_curvature), 5) > self.MAX_CURVATURE_ERROR
-
-            if steer_control_enabled:
-              should_tx = not limit_command or not too_far_away
-            else:
-              # enforce angle error limit is disabled when steer request bit is 0
-              should_tx = new_curvature == 0
+    for speed in np.arange(0, 40, 0.5):
+      limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED
+      max_delta_up = np.interp(speed, self.ANGLE_RATE_BP, self.ANGLE_RATE_UP)
+      max_delta_up_lower = np.interp(speed + 1, self.ANGLE_RATE_BP, self.ANGLE_RATE_UP)
+
+      cases = [
+        (not limit_command, 0),
+        (not limit_command, max_delta_up_lower - small_curvature),
+        (True, max_delta_up_lower),
+        (True, max_delta_up),
+        (False, max_delta_up + small_curvature),
+      ]
+
+      for sign in (-1, 1):
+        self._reset_curvature_measurement(sign * (self.MAX_CURVATURE_ERROR + 1e-3), speed)
+        for should_tx, curvature in cases:
+          self._set_prev_desired_angle(sign * small_curvature)
+          self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * (small_curvature + curvature), 0)))
+
+  def test_curvature_rate_limit_down(self):
+    self.safety.set_controls_allowed(True)
+    small_curvature = 2 / self.DEG_TO_CAN  # significant small amount of curvature to cross boundary
 
-            with self.subTest(steer_control_enabled=steer_control_enabled, speed=speed,
-                              initial_curvature=initial_curvature, new_curvature=new_curvature):
-              self.assertEqual(should_tx, self._tx(self._tja_command_msg(steer_control_enabled, 0, 0, new_curvature, 0)))
+    for speed in np.arange(0, 40, 0.5):
+      limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED
+      max_delta_down = np.interp(speed, self.ANGLE_RATE_BP, self.ANGLE_RATE_DOWN)
+      max_delta_down_lower = np.interp(speed + 1, self.ANGLE_RATE_BP, self.ANGLE_RATE_DOWN)
+
+      cases = [
+        (not limit_command, self.MAX_CURVATURE),
+        (not limit_command, self.MAX_CURVATURE - max_delta_down_lower + small_curvature),
+        (True, self.MAX_CURVATURE - max_delta_down_lower),
+        (True, self.MAX_CURVATURE - max_delta_down),
+        (False, self.MAX_CURVATURE - max_delta_down - small_curvature),
+      ]
+
+      for sign in (-1, 1):
+        self._reset_curvature_measurement(sign * (self.MAX_CURVATURE - self.MAX_CURVATURE_ERROR - 1e-3), speed)
+        for should_tx, curvature in cases:
+          self._set_prev_desired_angle(sign * self.MAX_CURVATURE)
+          self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * curvature, 0)))
 
   def test_prevent_lkas_action(self):
     self.safety.set_controls_allowed(1)