diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f34467081b1a3e..483889a460c74b 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -16,9 +16,8 @@ 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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 4802acb9fcc1b3..5ab2c510e0914b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -61bac237e228da19e59d456c883f99a92172d217 \ No newline at end of file +e740c2c793a5e2af19a5d52d7a4a45d77eca9412 \ No newline at end of file