From f56bee86eb48a2e4d33cae2dd2ae86b9f04a2c22 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Sep 2024 14:51:38 -0700 Subject: [PATCH 1/3] test more models! --- opendbc/car/honda/carcontroller.py | 8 +------- opendbc/car/honda/values.py | 4 ++++ opendbc/car/tests/test_lateral_limits.py | 2 +- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/opendbc/car/honda/carcontroller.py b/opendbc/car/honda/carcontroller.py index 20358c67fb..f00906dac4 100644 --- a/opendbc/car/honda/carcontroller.py +++ b/opendbc/car/honda/carcontroller.py @@ -97,12 +97,6 @@ def process_hud_alert(hud_alert): "lanes_visible", "fcw", "acc_alert", "steer_required", "lead_distance_bars"]) -def rate_limit_steer(new_steer, last_steer): - # TODO just hardcoded ramp to min/max in 0.33s for all Honda - MAX_DELTA = 3 * DT_CTRL - return clip(new_steer, last_steer - MAX_DELTA, last_steer + MAX_DELTA) - - class CarController(CarControllerBase): def __init__(self, dbc_name, CP): super().__init__(dbc_name, CP) @@ -138,7 +132,7 @@ def update(self, CC, CS, now_nanos): gas, brake = 0.0, 0.0 # *** rate limit steer *** - limited_steer = rate_limit_steer(actuators.steer, self.last_steer) + limited_steer = rate_limit(actuators.steer, self.last_steer, -self.params.STEER_DELTA_DOWN, self.params.STEER_DELTA_UP) self.last_steer = limited_steer # *** apply brake hysteresis *** diff --git a/opendbc/car/honda/values.py b/opendbc/car/honda/values.py index 70ccecb0ed..1200f69cca 100644 --- a/opendbc/car/honda/values.py +++ b/opendbc/car/honda/values.py @@ -35,6 +35,10 @@ class CarControllerParams: BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0] # 2m/s^2 BOSCH_GAS_LOOKUP_V = [0, 1600] + STEER_STEP = 1 # 100 Hz + STEER_DELTA_UP = 3 # min/max in 0.33s for all Honda + STEER_DELTA_DOWN = 3 + def __init__(self, CP): self.STEER_MAX = CP.lateralParams.torqueBP[-1] # mirror of list (assuming first item is zero) for interp of signed request values diff --git a/opendbc/car/tests/test_lateral_limits.py b/opendbc/car/tests/test_lateral_limits.py index 585289968d..2a0b7eb90a 100755 --- a/opendbc/car/tests/test_lateral_limits.py +++ b/opendbc/car/tests/test_lateral_limits.py @@ -35,7 +35,7 @@ def setup_class(cls): pytest.skip("Platform is behind dashcamOnly") # TODO: test all platforms - if CP.lateralTuning.which() != 'torque': + if CP.steerControlType != 'torque': pytest.skip() if CP.notCar: From 42801dfaf5d66b61489b91bcd7a8a8e3350c6905 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Sep 2024 15:08:12 -0700 Subject: [PATCH 2/3] Impreza 2020: reach max in ~0.8s instead of ~0.6 --- opendbc/car/subaru/values.py | 1 + 1 file changed, 1 insertion(+) diff --git a/opendbc/car/subaru/values.py b/opendbc/car/subaru/values.py index 15b8a8b8ec..bc586778b0 100644 --- a/opendbc/car/subaru/values.py +++ b/opendbc/car/subaru/values.py @@ -24,6 +24,7 @@ def __init__(self, CP): self.STEER_DELTA_UP = 40 self.STEER_DELTA_DOWN = 40 elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020: + self.STEER_DELTA_UP = 35 self.STEER_MAX = 1439 else: self.STEER_MAX = 2047 From 6b0761454db6b91f94e8ebfe3339660c58cf4695 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Sep 2024 19:00:13 -0700 Subject: [PATCH 3/3] stash so far --- opendbc/car/nissan/values.py | 1 + opendbc/car/tests/test_lateral_limits.py | 136 +++++++++++++++++++---- 2 files changed, 115 insertions(+), 22 deletions(-) diff --git a/opendbc/car/nissan/values.py b/opendbc/car/nissan/values.py index da396e655e..7a5a6452b3 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 2a0b7eb90a..28ed51424e 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