spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Apply EU guideline lateral limits (#23974) 

* Apply EU limits

* Lower bound v_ego

* Add curvature limit comment

* Remove comment

* Update ref
pull/23426/merge
HaraldSchafer 2022-03-22 19:46:04 -07:00 committed by GitHub
parent b57c2f4a14
commit 4a8fffd0f4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
selfdrive/controls/lib/drive_helpers.py
View File

@ -16,9 +16,8 @@ LON_MPC_N = 32
CONTROL_N = 17
CAR_ROTATION_RADIUS = 0.0
# this corresponds to 80deg/s and 20deg/s steering angle in a toyota corolla
MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992]
MAX_CURVATURE_RATE_SPEEDS = [0, 35]
# EU guidelines
MAX_LATERAL_JERK = 5.0
CRUISE_LONG_PRESS = 50
CRUISE_NEAREST_FUNC = {
@ -107,11 +106,13 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature
desired_curvature = current_curvature + 2 * curvature_diff_from_psi
max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES)
v_ego = max(v_ego, 0.1)
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2)
safe_desired_curvature_rate = clip(desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
safe_desired_curvature = clip(desired_curvature,
current_curvature - max_curvature_rate * DT_MDL,
current_curvature + max_curvature_rate * DT_MDL)
return safe_desired_curvature, safe_desired_curvature_rate

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
61bac237e228da19e59d456c883f99a92172d217
e740c2c793a5e2af19a5d52d7a4a45d77eca9412