diff --git a/opendbc/car/nissan/values.py b/opendbc/car/nissan/values.py index da396e655e9..7a5a6452b38 100644 --- a/opendbc/car/nissan/values.py +++ b/opendbc/car/nissan/values.py @@ -14,6 +14,7 @@ class CarControllerParams: ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4]) LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower STEER_THRESHOLD = 1.0 + STEER_STEP = 1 # 100 Hz def __init__(self, CP): pass diff --git a/opendbc/car/tests/test_lateral_limits.py b/opendbc/car/tests/test_lateral_limits.py index 2a0b7eb90a3..28ed51424ea 100755 --- a/opendbc/car/tests/test_lateral_limits.py +++ b/opendbc/car/tests/test_lateral_limits.py @@ -1,14 +1,17 @@ #!/usr/bin/env python3 from collections import defaultdict import importlib +import math from parameterized import parameterized_class import pytest import sys from opendbc.car import DT_CTRL +from opendbc.car.common.numpy_fast import interp from opendbc.car.car_helpers import interfaces from opendbc.car.fingerprints import all_known_cars from opendbc.car.interfaces import get_torque_params +from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel # TODO: no VM in opendbc CAR_MODELS = all_known_cars() @@ -29,42 +32,130 @@ class TestLateralLimits: @classmethod def setup_class(cls): CarInterface, _, _, _ = interfaces[cls.car_model] - CP = CarInterface.get_non_essential_params(cls.car_model) + cls.CP = CarInterface.get_non_essential_params(cls.car_model) + cls.VM = VehicleModel(cls.CP) - if CP.dashcamOnly: + if cls.CP.dashcamOnly: pytest.skip("Platform is behind dashcamOnly") - # TODO: test all platforms - if CP.steerControlType != 'torque': - pytest.skip() + # # TODO: test all platforms + # if CP.steerControlType != 'torque': + # pytest.skip() - if CP.notCar: + if cls.CP.notCar: pytest.skip() - CarControllerParams = importlib.import_module(f'opendbc.car.{CP.carName}.values').CarControllerParams - cls.control_params = CarControllerParams(CP) + CarControllerParams = importlib.import_module(f'opendbc.car.{cls.CP.carName}.values').CarControllerParams + cls.control_params = CarControllerParams(cls.CP) cls.torque_params = get_torque_params()[cls.car_model] @staticmethod - def calculate_0_5s_jerk(control_params, torque_params): + def calculate_0_5s_jerk(CP, VM, control_params, torque_params): steer_step = control_params.STEER_STEP max_lat_accel = torque_params['MAX_LAT_ACCEL_MEASURED'] - # 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 - - # 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 + if CP.steerControlType == 'torque': + # 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 + + # 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 + else: + # We need to test the entire speed range for angle cars as jerk is speed dependent + print(CP.carFingerprint, control_params.ANGLE_RATE_LIMIT_UP, steer_step) + + up_jerks = [] + down_jerks = [] + + speeds = [1e-3, 5., 15., 35.] # m/s + for speed in speeds: + up_rate = interp(speed, control_params.ANGLE_RATE_LIMIT_UP.speed_bp, control_params.ANGLE_RATE_LIMIT_UP.angle_v) + down_rate = interp(speed, control_params.ANGLE_RATE_LIMIT_DOWN.speed_bp, control_params.ANGLE_RATE_LIMIT_DOWN.angle_v) + + # Ford is already curvature + if CP.carName != 'ford': + up_rate = VM.calc_curvature(math.radians(up_rate), speed, 0) + down_rate = VM.calc_curvature(math.radians(down_rate), speed, 0) + print(up_rate, down_rate) + continue + + accel_up_per_frame = up_rate * speed ** 2 / steer_step + accel_down_per_frame = down_rate * speed ** 2 / steer_step + + accel_up_0_5_sec = min(accel_up_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel) + accel_down_0_5_sec = min(accel_down_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel) + + print('speed', speed) + print('accel_up_per_frame', accel_up_per_frame, 'accel_down_per_frame', accel_down_per_frame) + print('accel_up_0_5_sec', accel_up_0_5_sec, 'accel_down_0_5_sec', accel_down_0_5_sec) + up_jerks.append(accel_up_0_5_sec / JERK_MEAS_T) + down_jerks.append(accel_down_0_5_sec / JERK_MEAS_T) + # continue + # + # accel_up_0_5_sec = min(curvature_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL + # accel_down_0_5_sec = min(curvature_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL + # jerk_up, jerk_down = accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T + # print(up_rate, down_rate) + # print('jerk_up', jerk_up, 'jerk_down', jerk_down) + # + # # curvature_up = self.VM.calc_curvature(math.radians(1.6) * (100 / steer_step), speed, 0) + # # jerk_up = curvature * speed ** 2 + # # print('speed', speed, 'curvature', curvature, 'jerk', jerk) + return max(up_jerks), max(down_jerks) def test_jerk_limits(self): - up_jerk, down_jerk = self.calculate_0_5s_jerk(self.control_params, self.torque_params) + # print(self.CP.carFingerprint, self.CP.steerControlType) + # if self.CP.steerControlType == 'torque': + up_jerk, down_jerk = self.calculate_0_5s_jerk(self.CP, self.VM, self.control_params, self.torque_params) assert up_jerk <= MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE assert down_jerk <= MAX_LAT_JERK_DOWN + # else: + # steer_step = self.control_params.STEER_STEP + # max_lat_accel = self.torque_params['MAX_LAT_ACCEL_MEASURED'] + # + # + # # We need to test the entire speed range for angle cars as jerk is speed dependent + # print(self.CP.carFingerprint, self.control_params.ANGLE_RATE_LIMIT_UP, self.control_params.STEER_STEP) + # speeds = [1e-3, 5., 15., 35.] # m/s + # for speed in speeds: + # up_rate = interp(speed, self.control_params.ANGLE_RATE_LIMIT_UP.speed_bp, self.control_params.ANGLE_RATE_LIMIT_UP.angle_v) + # down_rate = interp(speed, self.control_params.ANGLE_RATE_LIMIT_DOWN.speed_bp, self.control_params.ANGLE_RATE_LIMIT_DOWN.angle_v) + # + # # Ford is already curvature + # if self.CP.carName != 'ford': + # up_rate = self.VM.calc_curvature(math.radians(up_rate), speed, 0) + # down_rate = self.VM.calc_curvature(math.radians(down_rate), speed, 0) + # + # accel_up_per_frame = up_rate * speed ** 2 / self.control_params.STEER_STEP + # accel_down_per_frame = down_rate * speed ** 2 / self.control_params.STEER_STEP + # + # accel_up_0_5_sec = min(accel_up_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel) + # accel_down_0_5_sec = min(accel_down_per_frame * JERK_MEAS_T / DT_CTRL, max_lat_accel) + # + # print('speed', speed) + # print('accel_up_per_frame', accel_up_per_frame, 'accel_down_per_frame', accel_down_per_frame) + # print('accel_up_0_5_sec', accel_up_0_5_sec, 'accel_down_0_5_sec', accel_down_0_5_sec) + # continue + # + # accel_up_0_5_sec = min(curvature_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL + # accel_down_0_5_sec = min(curvature_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * MAX_LAT_ACCEL + # jerk_up, jerk_down = accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T + # print(up_rate, down_rate) + # print('jerk_up', jerk_up, 'jerk_down', jerk_down) + # + # + # + # # curvature_up = self.VM.calc_curvature(math.radians(1.6) * (100 / self.control_params.STEER_STEP), speed, 0) + # # jerk_up = curvature * speed ** 2 + # # print('speed', speed, 'curvature', curvature, 'jerk', jerk) + # + # if self.CP.carFingerprint != 'TOYOTA_RAV4_TSS2_2023': + # return def test_max_lateral_accel(self): assert self.torque_params["MAX_LAT_ACCEL_MEASURED"] <= MAX_LAT_ACCEL @@ -90,9 +181,10 @@ def class_setup(self, request): yield cls = request.cls if hasattr(cls, "control_params"): - up_jerk, down_jerk = TestLateralLimits.calculate_0_5s_jerk(cls.control_params, cls.torque_params) + up_jerk, down_jerk = TestLateralLimits.calculate_0_5s_jerk(cls.CP, cls.VM, cls.control_params, cls.torque_params) self.car_model_jerks[cls.car_model] = {"up_jerk": up_jerk, "down_jerk": down_jerk} if __name__ == '__main__': - sys.exit(pytest.main([__file__, '-n0', '--no-summary'], plugins=[LatAccelReport()])) # noqa: TID251 + sys.exit(pytest.main([__file__, '-n0', '--no-summary'],)) + # plugins=[LatAccelReport()])) # noqa: TID251