Revert "Follow plan sooner after engaging (#545)"

This reverts commit d86d8ea.

Author: sshane

SA_RELEASES.md

@@ -1,5 +1,7 @@
 Stock Additions 0.8.13
 ===
+## - 2022-01-03
+ * Revert "follow longitudinal plan earlier" change as it had some minor bugs. Re-do in progress
 ## - 2022-01-01
  * Improve lateral performance by interpolating lateral plan to 100hz (from 20hz)
 ## - 2021-12-31, 5:00pm MST Notes

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -323,9 +323,10 @@ def set_desired_TR(self, desired_TR):
     self.desired_TR = desired_TR
     self.set_weights()
 
-  def update(self, carstate, radarstate, v_cruise):
+  def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
     self.v_ego = carstate.vEgo
     v_ego = self.x0[1]
+    a_ego = self.x0[2]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne, 0)
@@ -356,7 +357,10 @@ def update(self, carstate, radarstate, v_cruise):
     x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
     self.source = SOURCES[np.argmin(x_obstacles[0])]
     self.params[:,2] = np.min(x_obstacles, axis=1)
-    self.params[:,3] = np.copy(self.prev_a)
+    if prev_accel_constraint:
+      self.params[:,3] = np.copy(self.prev_a)
+    else:
+      self.params[:,3] = a_ego
     self.params[:, 4] = self.desired_TR
 
     self.run()

selfdrive/controls/lib/longitudinal_planner.py

@@ -4,7 +4,6 @@
 from common.numpy_fast import interp
 
 import cereal.messaging as messaging
-from common.filter_simple import FirstOrderFilter
 from common.realtime import DT_MDL
 from selfdrive.modeld.constants import T_IDXS
 from selfdrive.config import Conversions as CV
@@ -49,8 +48,9 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
 
     self.fcw = False
 
-    self.v_desired = FirstOrderFilter(init_v, 2.0, DT_MDL)
-    self.a_desired = FirstOrderFilter(init_a, 0.5, DT_MDL)
+    self.v_desired = init_v
+    self.a_desired = init_a
+    self.alpha = np.exp(-DT_MDL / 2.0)
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -67,14 +67,16 @@ def update(self, sm):
     long_control_state = sm['controlsState'].longControlState
     force_slow_decel = sm['controlsState'].forceDecel
 
+    prev_accel_constraint = True
     if long_control_state == LongCtrlState.off or sm['carState'].gasPressed:
-      self.v_desired.x = v_ego
-      # Smooth in current a_ego so we always have an accurate plan
-      self.a_desired.update(a_ego)
+      self.v_desired = v_ego
+      self.a_desired = a_ego
+      # Smoothly changing between accel trajectory is only relevant when OP is driving
+      prev_accel_constraint = False
 
     # Prevent divergence, smooth in current v_ego
-    self.v_desired.update(v_ego)
-    self.v_desired.x = max(0.0, self.v_desired.x)
+    self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
+    self.v_desired = max(0.0, self.v_desired)
 
     accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
     accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
@@ -83,11 +85,11 @@ def update(self, sm):
       accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
       accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
     # clip limits, cannot init MPC outside of bounds
-    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired.x + 0.05)
-    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired.x - 0.05)
+    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
+    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
-    self.mpc.set_cur_state(self.v_desired.x, self.a_desired.x)
-    self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
+    self.mpc.set_cur_state(self.v_desired, self.a_desired)
+    self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint)
     self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
     self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
@@ -98,9 +100,9 @@ def update(self, sm):
       cloudlog.info("FCW triggered")
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
-    a_prev = self.a_desired.x
-    self.a_desired.x = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
-    self.v_desired.x = self.v_desired.x + DT_MDL * (self.a_desired.x + a_prev) / 2.0
+    a_prev = self.a_desired
+    self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
+    self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0
 
   def publish(self, sm, pm):
     plan_send = messaging.new_message('longitudinalPlan')

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -97,8 +97,8 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
           'carState': car_state.carState,
           'controlsState': control.controlsState}
     self.planner.update(sm)
-    self.speed = self.planner.v_desired.x
-    self.acceleration = self.planner.a_desired.x
+    self.speed = self.planner.v_desired
+    self.acceleration = self.planner.a_desired
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts