Test lateral limits: fix jerk calculation (#27654)

* fix test

* add lower jerk limit, lower up tolerance

* add to ignored cars
old-commit-hash: 88ce2f7a52

selfdrive/car/tests/test_lateral_limits.py

@@ -10,16 +10,24 @@ from common.realtime import DT_CTRL
 from selfdrive.car.car_helpers import interfaces
 from selfdrive.car.fingerprints import all_known_cars
 from selfdrive.car.interfaces import get_torque_params
+from selfdrive.car.subaru.values import CAR as SUBARU
 
 CAR_MODELS = all_known_cars()
 
-# ISO 11270
-MAX_LAT_JERK = 2.5             # m/s^3
-MAX_LAT_JERK_TOLERANCE = 0.75  # m/s^3
-MAX_LAT_ACCEL = 3.0            # m/s^2
+# ISO 11270 - allowed up jerk is strictly lower than recommended limits
+MAX_LAT_ACCEL = 3.0              # m/s^2
+MAX_LAT_JERK_UP = 2.5            # m/s^3
+MAX_LAT_JERK_DOWN = 5.0          # m/s^3
+MAX_LAT_JERK_UP_TOLERANCE = 0.5  # m/s^3
 
 # jerk is measured over half a second
-JERK_MEAS_FRAMES = 0.5 / DT_CTRL
+JERK_MEAS_T = 0.5
+
+# TODO: put these cars within limits
+ABOVE_LIMITS_CARS = [
+  SUBARU.LEGACY,
+  SUBARU.OUTBACK,
+]
 
 car_model_jerks: DefaultDict[str, Dict[str, float]] = defaultdict(dict)
 
@@ -43,6 +51,9 @@ class TestLateralLimits(unittest.TestCase):
     if CP.notCar:
       raise unittest.SkipTest
 
+    if CP.carFingerprint in ABOVE_LIMITS_CARS:
+      raise unittest.SkipTest
+
     CarControllerParams = importlib.import_module(f'selfdrive.car.{CP.carName}.values').CarControllerParams
     cls.control_params = CarControllerParams(CP)
     cls.torque_params = get_torque_params(cls.car_model)
@@ -50,20 +61,24 @@ class TestLateralLimits(unittest.TestCase):
   @staticmethod
   def calculate_0_5s_jerk(control_params, torque_params):
     steer_step = control_params.STEER_STEP
-    steer_up_per_frame = (control_params.STEER_DELTA_UP / control_params.STEER_MAX) / steer_step
-    steer_down_per_frame = (control_params.STEER_DELTA_DOWN / control_params.STEER_MAX) / steer_step
+    max_lat_accel = torque_params['MAX_LAT_ACCEL_MEASURED']
 
-    steer_up_0_5_sec = min(steer_up_per_frame * JERK_MEAS_FRAMES, 1.0)
-    steer_down_0_5_sec = min(steer_down_per_frame * JERK_MEAS_FRAMES, 1.0)
+    # Steer up/down delta per 10ms frame, in percentage of max torque
+    steer_up_per_frame = control_params.STEER_DELTA_UP / control_params.STEER_MAX / steer_step
+    steer_down_per_frame = control_params.STEER_DELTA_DOWN / control_params.STEER_MAX / steer_step
 
-    max_lat_accel = torque_params['MAX_LAT_ACCEL_MEASURED']
-    return steer_up_0_5_sec * max_lat_accel, steer_down_0_5_sec * max_lat_accel
+    # Lateral acceleration reached in 0.5 seconds, clipping to max torque
+    accel_up_0_5_sec = min(steer_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel
+    accel_down_0_5_sec = min(steer_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel
+
+    # Convert to m/s^3
+    return accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T
 
   def test_jerk_limits(self):
     up_jerk, down_jerk = self.calculate_0_5s_jerk(self.control_params, self.torque_params)
     car_model_jerks[self.car_model] = {"up_jerk": up_jerk, "down_jerk": down_jerk}
-    self.assertLessEqual(up_jerk, MAX_LAT_JERK + MAX_LAT_JERK_TOLERANCE)
-    self.assertLessEqual(down_jerk, MAX_LAT_JERK + MAX_LAT_JERK_TOLERANCE)
+    self.assertLessEqual(up_jerk, MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE)
+    self.assertLessEqual(down_jerk, MAX_LAT_JERK_DOWN)
 
   def test_max_lateral_accel(self):
     self.assertLessEqual(self.torque_params["MAX_LAT_ACCEL_MEASURED"], MAX_LAT_ACCEL)
@@ -76,7 +91,8 @@ if __name__ == "__main__":
 
   max_car_model_len = max([len(car_model) for car_model in car_model_jerks])
   for car_model, _jerks in sorted(car_model_jerks.items(), key=lambda i: i[1]['up_jerk'], reverse=True):
-    violation = any([_jerk >= MAX_LAT_JERK + MAX_LAT_JERK_TOLERANCE for _jerk in _jerks.values()])
+    violation = _jerks["up_jerk"] > MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE or \
+                _jerks["down_jerk"] > MAX_LAT_JERK_DOWN
     violation_str = " - VIOLATION" if violation else ""
 
     print(f"{car_model:{max_car_model_len}} - up jerk: {round(_jerks['up_jerk'], 2):5} m/s^3, down jerk: {round(_jerks['down_jerk'], 2):5} m/s^3{violation_str}")