Apply EU guideline lateral limits (#23974)
* Apply EU limits * Lower bound v_ego * Add curvature limit comment * Remove comment * Update refpull/23426/merge
parent
b57c2f4a14
commit
4a8fffd0f4
|
@ -16,9 +16,8 @@ LON_MPC_N = 32
|
||||||
CONTROL_N = 17
|
CONTROL_N = 17
|
||||||
CAR_ROTATION_RADIUS = 0.0
|
CAR_ROTATION_RADIUS = 0.0
|
||||||
|
|
||||||
# this corresponds to 80deg/s and 20deg/s steering angle in a toyota corolla
|
# EU guidelines
|
||||||
MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992]
|
MAX_LATERAL_JERK = 5.0
|
||||||
MAX_CURVATURE_RATE_SPEEDS = [0, 35]
|
|
||||||
|
|
||||||
CRUISE_LONG_PRESS = 50
|
CRUISE_LONG_PRESS = 50
|
||||||
CRUISE_NEAREST_FUNC = {
|
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
|
curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature
|
||||||
desired_curvature = current_curvature + 2 * curvature_diff_from_psi
|
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,
|
safe_desired_curvature_rate = clip(desired_curvature_rate,
|
||||||
-max_curvature_rate,
|
-max_curvature_rate,
|
||||||
max_curvature_rate)
|
max_curvature_rate)
|
||||||
safe_desired_curvature = clip(desired_curvature,
|
safe_desired_curvature = clip(desired_curvature,
|
||||||
current_curvature - max_curvature_rate * DT_MDL,
|
current_curvature - max_curvature_rate * DT_MDL,
|
||||||
current_curvature + max_curvature_rate * DT_MDL)
|
current_curvature + max_curvature_rate * DT_MDL)
|
||||||
|
|
||||||
return safe_desired_curvature, safe_desired_curvature_rate
|
return safe_desired_curvature, safe_desired_curvature_rate
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
61bac237e228da19e59d456c883f99a92172d217
|
e740c2c793a5e2af19a5d52d7a4a45d77eca9412
|
Loading…
Reference in New Issue