From 88ce2f7a52c909c74d935b81ca219d3f8c2854bf Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 13 Apr 2023 13:21:30 -0700 Subject: [PATCH] Test lateral limits: fix jerk calculation (#27654) * fix test * add lower jerk limit, lower up tolerance * add to ignored cars --- selfdrive/car/tests/test_lateral_limits.py | 44 +++++++++++++++------- 1 file changed, 30 insertions(+), 14 deletions(-) diff --git a/selfdrive/car/tests/test_lateral_limits.py b/selfdrive/car/tests/test_lateral_limits.py index d3e4de1798..7ccd5e3c97 100755 --- a/selfdrive/car/tests/test_lateral_limits.py +++ b/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}")