Lateral jerk unit test (#25759)

* stash

* more test

* less test

* clean test

* no angle

* add tolerance

* fix gm

fix gm

* test both

* lower some rates on hkg

* stash

* simpler

* bump panda

* Revert "bump panda"

This reverts commit f2137c2211.

* only torque

* make kona pass

* duplicate __init__

* move

* half clean up

* half clean up

* more clean up

* more clean up

* fix static analysis

* calculate over 0.5 seconds

* limit to max steer

* type annotation

* calc once

selfdrive/car/body/values.py

@@ -17,6 +17,9 @@ class CarControllerParams:
   LKAS_MAX_TORQUE = 1               # A value of 1 is easy to overpower
   STEER_THRESHOLD = 1.0
 
+  def __init__(self, CP):
+    pass
+
 
 class CAR:
   BODY = "COMMA BODY"

selfdrive/car/chrysler/carcontroller.py

@@ -45,7 +45,7 @@ class CarController:
         self.hud_count += 1
 
     # steering
-    if self.frame % 2 == 0:
+    if self.frame % self.params.STEER_STEP == 0:
 
       # TODO: can we make this more sane? why is it different for all the cars?
       lkas_control_bit = self.lkas_control_bit_prev

selfdrive/car/chrysler/values.py

@@ -35,6 +35,7 @@ class CAR:
 
 class CarControllerParams:
   def __init__(self, CP):
+    self.STEER_STEP = 2  # 50 Hz
     self.STEER_ERROR_MAX = 80
     if CP.carFingerprint in RAM_HD:
       self.STEER_DELTA_UP = 14

selfdrive/car/ford/carcontroller.py

@@ -66,7 +66,7 @@ class CarController:
       apply_angle = 0.
 
     # send steering commands at 20Hz
-    if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
+    if (self.frame % CarControllerParams.STEER_STEP) == 0:
       # use LatCtlPath_An_Actl to actuate steering
       path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
 

selfdrive/car/ford/values.py

@@ -17,7 +17,7 @@ AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_p
 
 class CarControllerParams:
   # Messages: Lane_Assist_Data1, LateralMotionControl
-  LKAS_STEER_STEP = 5
+  STEER_STEP = 5
   # Message: IPMA_Data
   LKAS_UI_STEP = 100
   # Message: ACCDATA_3
@@ -32,6 +32,9 @@ class CarControllerParams:
   RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
   RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
 
+  def __init__(self, CP):
+    pass
+
 
 class CANBUS:
   main = 0

selfdrive/car/gm/carcontroller.py

@@ -53,7 +53,7 @@ class CarController:
     init_lka_counter = not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera
     steer_step = self.params.INACTIVE_STEER_STEP
     if CC.latActive or init_lka_counter:
-      steer_step = self.params.ACTIVE_STEER_STEP
+      steer_step = self.params.STEER_STEP
 
     # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the
     # next Panda loopback confirmation in the current CS frame.

selfdrive/car/gm/values.py

@@ -11,7 +11,7 @@ Ecu = car.CarParams.Ecu
 
 class CarControllerParams:
   STEER_MAX = 300  # GM limit is 3Nm. Used by carcontroller to generate LKA output
-  ACTIVE_STEER_STEP = 2  # Active control frames per command (50hz)
+  STEER_STEP = 2  # Active control frames per command (50hz)
   INACTIVE_STEER_STEP = 10  # Inactive control frames per command (10hz)
   STEER_DELTA_UP = 7  # Delta rates require review due to observed EPS weakness
   STEER_DELTA_DOWN = 17

selfdrive/car/hyundai/carcontroller.py

@@ -59,8 +59,7 @@ class CarController:
     hud_control = CC.hudControl
 
     # steering torque
-    steer = actuators.steer
+    new_steer = int(round(actuators.steer * self.params.STEER_MAX))
-    new_steer = int(round(steer * self.params.STEER_MAX))
     apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
 
     if not CC.latActive:

selfdrive/car/hyundai/values.py

@@ -23,6 +23,7 @@ class CarControllerParams:
     self.STEER_DRIVER_MULTIPLIER = 2
     self.STEER_DRIVER_FACTOR = 1
     self.STEER_THRESHOLD = 150
+    self.STEER_STEP = 1  # 100 Hz
 
     if CP.carFingerprint in CANFD_CAR:
       self.STEER_MAX = 270

selfdrive/car/mazda/values.py

@@ -20,6 +20,10 @@ class CarControllerParams:
   STEER_DRIVER_MULTIPLIER = 1     # weight driver torque
   STEER_DRIVER_FACTOR = 1         # from dbc
   STEER_ERROR_MAX = 350           # max delta between torque cmd and torque motor
+  STEER_STEP = 1  # 100 Hz
+
+  def __init__(self, CP):
+    pass
 
 
 class CAR:

selfdrive/car/nissan/values.py

@@ -17,6 +17,9 @@ class CarControllerParams:
   LKAS_MAX_TORQUE = 1               # A value of 1 is easy to overpower
   STEER_THRESHOLD = 1.0
 
+  def __init__(self, CP):
+    pass
+
 
 class CAR:
   XTRAIL = "NISSAN X-TRAIL 2017"

selfdrive/car/tesla/values.py

@@ -108,3 +108,6 @@ class CarControllerParams:
   JERK_LIMIT_MAX = 8
   JERK_LIMIT_MIN = -8
   ACCEL_TO_SPEED_MULTIPLIER = 3
+
+  def __init__(self, CP):
+    pass

selfdrive/car/tests/test_lateral_limits.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+from collections import defaultdict
+import importlib
+from parameterized import parameterized_class
+import sys
+from typing import DefaultDict, Dict
+import unittest
+
+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.hyundai.values import CAR as HYUNDAI
+
+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
+
+# jerk is measured over half a second
+JERK_MEAS_FRAMES = 0.5 / DT_CTRL
+
+# TODO: update the max measured lateral accel for these cars
+ABOVE_LIMITS_CARS = [
+  HYUNDAI.KONA_EV,
+  HYUNDAI.KONA_HEV,
+  HYUNDAI.KONA,
+  HYUNDAI.KONA_EV_2022,
+]
+
+car_model_jerks: DefaultDict[str, Dict[str, float]] = defaultdict(dict)
+
+
+@parameterized_class('car_model', [(c,) for c in CAR_MODELS])
+class TestLateralLimits(unittest.TestCase):
+  car_model: str
+
+  @classmethod
+  def setUpClass(cls):
+    CarInterface, _, _ = interfaces[cls.car_model]
+    CP = CarInterface.get_params(cls.car_model)
+
+    if CP.dashcamOnly:
+      raise unittest.SkipTest("Platform is behind dashcamOnly")
+
+    # TODO: test all platforms
+    if CP.lateralTuning.which() != 'torque':
+      raise unittest.SkipTest
+
+    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)
+
+  @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
+
+    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)
+
+    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
+
+  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)
+
+  def test_max_lateral_accel(self):
+    self.assertLessEqual(self.torque_params["MAX_LAT_ACCEL_MEASURED"], MAX_LAT_ACCEL)
+
+
+if __name__ == "__main__":
+  result = unittest.main(exit=False)
+
+  print(f"\n\n---- Lateral limit report ({len(CAR_MODELS)} cars) ----\n")
+
+  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_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}")
+
+  # exit with test result
+  sys.exit(not result.result.wasSuccessful())

selfdrive/car/toyota/values.py

@@ -18,6 +18,7 @@ class CarControllerParams:
   ACCEL_MAX = 1.5  # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
   ACCEL_MIN = -3.5  # m/s2
 
+  STEER_STEP = 1
   STEER_MAX = 1500
   STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
 

selfdrive/car/volkswagen/carcontroller.py

@@ -31,7 +31,7 @@ class CarController:
 
     # **** Steering Controls ************************************************ #
 
-    if self.frame % self.CCP.HCA_STEP == 0:
+    if self.frame % self.CCP.STEER_STEP == 0:
       # Logic to avoid HCA state 4 "refused":
       #   * Don't steer unless HCA is in state 3 "ready" or 5 "active"
       #   * Don't steer at standstill
@@ -50,14 +50,14 @@ class CarController:
           self.hcaEnabledFrameCount = 0
         else:
           self.hcaEnabledFrameCount += 1
-          if self.hcaEnabledFrameCount >= 118 * (100 / self.CCP.HCA_STEP):  # 118s
+          if self.hcaEnabledFrameCount >= 118 * (100 / self.CCP.STEER_STEP):  # 118s
             hcaEnabled = False
             self.hcaEnabledFrameCount = 0
           else:
             hcaEnabled = True
             if self.apply_steer_last == apply_steer:
               self.hcaSameTorqueCount += 1
-              if self.hcaSameTorqueCount > 1.9 * (100 / self.CCP.HCA_STEP):  # 1.9s
+              if self.hcaSameTorqueCount > 1.9 * (100 / self.CCP.STEER_STEP):  # 1.9s
                 apply_steer -= (1, -1)[apply_steer < 0]
                 self.hcaSameTorqueCount = 0
             else:

selfdrive/car/volkswagen/values.py

@@ -18,7 +18,7 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
 
 
 class CarControllerParams:
-  HCA_STEP = 2                            # HCA_01/HCA_1 message frequency 50Hz
+  STEER_STEP = 2                          # HCA_01/HCA_1 message frequency 50Hz
   ACC_CONTROL_STEP = 2                    # ACC_06/ACC_07/ACC_System frequency 50Hz
 
   ACCEL_MAX = 2.0                         # 2.0 m/s max acceleration