1.75x curvature to reduce slowdown

ErichMoraga · May 13, 2020 · 65eddca · 65eddca
1 parent e38726e
commit 65eddca
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
@@ -142,7 +142,7 @@ def update(self, sm, pm, CP, VM, PP):
       curv = y_pp / (1. + y_p**2)**1.5
 
       a_y_max = 2.975 - v_ego * 0.0375  # ~1.85 @ 75mph, ~2.6 @ 25mph
-      v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
+      v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv)**1.75, 1e-4, None))
       model_speed = np.min(v_curvature)
       model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
     else: