mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 08:14:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

Test lateral limits: fix jerk calculation (#27654)

Browse Source 
* fix test

* add lower jerk limit, lower up tolerance

* add to ignored cars
old-commit-hash: 88ce2f7a52c909c74d935b81ca219d3f8c2854bf
This commit is contained in:
Shane Smiskol
2023-04-13 13:21:30 -07:00
committed by GitHub
parent 6670dee6c2
commit 004da44410
1 changed files with 31 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/tests/test_lateral_limits.py
+31 -15
View File
@@ -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
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
# 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
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}")