diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index 548039bc70..4fef966374 100644 --- a/selfdrive/car/body/values.py +++ b/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" diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 31cd01b654..ba6aaf8250 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/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 diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 02261a0b63..7629a2f086 100644 --- a/selfdrive/car/chrysler/values.py +++ b/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 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index f18014601c..8c391bb276 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/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 diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 5114f8d065..0e6ef464b3 100644 --- a/selfdrive/car/ford/values.py +++ b/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 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index d380abf8e3..b4a79d10a6 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/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. diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 84fa36a994..ece128c253 100644 --- a/selfdrive/car/gm/values.py +++ b/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 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 5582499f25..b81c5e3f7d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -59,8 +59,7 @@ class CarController: hud_control = CC.hudControl # steering torque - steer = actuators.steer - new_steer = int(round(steer * self.params.STEER_MAX)) + new_steer = int(round(actuators.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: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index a90797ae18..4d3f44d65f 100644 --- a/selfdrive/car/hyundai/values.py +++ b/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 diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index e6e9b3aee8..598b598a16 100644 --- a/selfdrive/car/mazda/values.py +++ b/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: diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index a6ee27a4a3..6371b56be8 100644 --- a/selfdrive/car/nissan/values.py +++ b/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" diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 52f2aedf98..bac025a9c5 100644 --- a/selfdrive/car/tesla/values.py +++ b/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 diff --git a/selfdrive/car/tests/test_lateral_limits.py b/selfdrive/car/tests/test_lateral_limits.py new file mode 100755 index 0000000000..a82c20ce01 --- /dev/null +++ b/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()) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 6a92540f00..8ff06a591a 100644 --- a/selfdrive/car/toyota/values.py +++ b/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 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 628962de75..4b19f4d13c 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/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: diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 208d146e2f..d1f6bfb02c 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/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