Slowdown for curves (commaai#274)

* Update accel limits

* not as much deceleration

* Display slowdown img when either model-related solution are slowing down

* tone down deceleration

* fix missing var

SA_RELEASES.md

@@ -5,6 +5,7 @@ Stock Additions v0.6 - 2020-11-09 (0.7.10)
  * Raise max limit of global_df_mod to 1.5x
  * Continued white panda support (untested, grey gives no warning)
  * Reduced DM uncertain alert at night with EON
+ * Add slow down for curves from openpilot 0.6.3
 
 Stock Additions v0.5.4 - 2020-10-15 (0.7.7)
 ===

cereal/log.capnp

@@ -840,6 +840,7 @@ struct Plan {
     mpc3 @3;
     model @4;
     dynamicSpeed @5;
+    curveSlowdown @6;
   }
 }
 

selfdrive/controls/controlsd.py

@@ -572,7 +572,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
     controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
     controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
     controlsState.vCurvature = self.sm['plan'].vCurvature
-    controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model
+    controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource in [LongitudinalPlanSource.model, LongitudinalPlanSource.curveSlowdown]
     controlsState.cumLagMs = -self.rk.remaining * 1000.
     controlsState.startMonoTime = int(start_time * 1e9)
     controlsState.mapValid = self.sm['plan'].mapValid

selfdrive/controls/lib/planner.py

@@ -26,12 +26,12 @@
 
 # need fast accel at very low speed for stop and go
 # make sure these accelerations are smaller than mpc limits
-_A_CRUISE_MAX_V = [1.55, 1.4, 0.7, .4]
+_A_CRUISE_MAX_V = [1.6, 1.4, 0.7, .4]
 _A_CRUISE_MAX_V_FOLLOWING = [1.7, 1.65, 0.7, .5]
 _A_CRUISE_MAX_BP = [0.,  6.4, 22.5, 40.]
 
 # Lookup table for turns
-_A_TOTAL_MAX_V = [2.2, 4.35]
+_A_TOTAL_MAX_V = [2.2, 4.15]
 _A_TOTAL_MAX_BP = [20., 40.]
 
 
@@ -74,6 +74,8 @@ def __init__(self, CP):
     self.a_acc = 0.0
     self.v_cruise = 0.0
     self.a_cruise = 0.0
+    self.v_model = 0.0
+    self.a_model = 0.0
 
     self.longitudinalPlanSource = 'cruise'
     self.fcw_checker = FCWChecker()
@@ -85,7 +87,7 @@ def __init__(self, CP):
   def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
     possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]
     if enabled:
-      solutions = {'cruise': self.v_cruise}
+      solutions = {'cruise': self.v_cruise, 'curveSlowdown': self.v_model}
       if self.mpc1.prev_lead_status:
         solutions['mpc1'] = self.mpc1.v_mpc
       if self.mpc2.prev_lead_status:
@@ -110,6 +112,9 @@ def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
       elif slowest == 'model':
         self.v_acc = self.mpc_model.v_mpc
         self.a_acc = self.mpc_model.a_mpc
+      elif slowest == 'curveSlowdown':
+        self.v_acc = self.v_model
+        self.a_acc = self.a_model
 
     self.v_acc_future = min(possible_futures)
 
@@ -131,6 +136,24 @@ def update(self, sm, pm, CP, VM, PP):
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
     following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
 
+    if len(sm['model'].path.poly):  # old slowdown for curves code
+      path = list(sm['model'].path.poly)
+
+      # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
+      # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
+      # k = y'' / (1 + y'^2)^1.5
+      # TODO: compute max speed without using a list of points and without numpy
+      y_p = 3 * path[0] * self.path_x ** 2 + 2 * path[1] * self.path_x + path[2]
+      y_pp = 6 * path[0] * self.path_x + 2 * path[1]
+      curv = y_pp / (1. + y_p ** 2) ** 1.5
+
+      a_y_max = 3.1 - v_ego * 0.032
+      v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 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:
+      model_speed = 255.  # (MAX_SPEED)
+
     # Calculate speed for normal cruise control
     if enabled and not self.first_loop and not sm['carState'].gasPressed:
       accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
@@ -148,6 +171,12 @@ def update(self, sm, pm, CP, VM, PP):
                                                     jerk_limits[1], jerk_limits[0],
                                                     LON_MPC_STEP)
 
+      self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
+                                                  model_speed,
+                                                  2 * accel_limits[1], accel_limits[0],
+                                                  2 * jerk_limits[1], jerk_limits[0],
+                                                  LON_MPC_STEP)
+
       # cruise speed can't be negative even is user is distracted
       self.v_cruise = max(self.v_cruise, 0.)
     else: