Apply EU guideline lateral limits (commaai#23974)

* Apply EU limits * Lower bound v_ego * Add curvature limit comment * Remove comment * Update ref
budney · Mar 23, 2022 · 4a8fffd · 4a8fffd
1 parent b57c2f4
commit 4a8fffd
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 5 deletions.
diff --git 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
@@ -1 +1 @@
-61bac237e228da19e59d456c883f99a92172d217
+e740c2c793a5e2af19a5d52d7a4a45d77eca9412