Follow plan sooner after engaging (commaai#545)

* Set accel 0 when not active

* Smooth in a_ego

* conditional accel constraint no longer needed

comment

comment

* tune to 0.5

* Make a_desired and v_desired FirstOrderFilters

* update releases

README.md

@@ -1,4 +1,4 @@
-# Stock Additions - [2021-12-28](/SA_RELEASES.md) (0.8.13)
+# Stock Additions - [2021-12-31](/SA_RELEASES.md) (0.8.13)
 
 Stock Additions is a fork of openpilot designed to be minimal in design while boasting various feature additions and behavior improvements over stock. I have a 2017 Toyota Corolla with comma pedal, so most of my changes are designed to improve the longitudinal performance.
 

SA_RELEASES.md

@@ -1,5 +1,9 @@
 Stock Additions 0.8.13
 ===
+## - 2021-12-31, 5:00pm MST Notes
+ * A few controls improvements related to right after engaging:
+   * Car no longer brakes for a second after engaging at low speeds if there's no lead
+   * Follows plan sooner: After you take your foot off the gas pedal to accelerate while engaged, the car now brakes much earlier. Previously the car would continue to accelerate for a second even if we're above the set speed or close to a car.
 ## - 2021-12-28, 3:00am MST Notes
  * Use correct path offset for comma three, may reduce slight left hugging
 ## - 2021-12-24, 3:23am MST Notes

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -323,10 +323,9 @@ def set_desired_TR(self, desired_TR):
     self.desired_TR = desired_TR
     self.set_weights()
 
-  def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
+  def update(self, carstate, radarstate, v_cruise):
     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)
@@ -357,10 +356,7 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
     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)
-    if prev_accel_constraint:
-      self.params[:,3] = np.copy(self.prev_a)
-    else:
-      self.params[:,3] = a_ego
+    self.params[:,3] = np.copy(self.prev_a)
     self.params[:, 4] = self.desired_TR
 
     self.run()

selfdrive/controls/lib/longitudinal_planner.py

@@ -4,6 +4,7 @@
 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
@@ -48,9 +49,8 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
 
     self.fcw = False
 
-    self.v_desired = init_v
-    self.a_desired = init_a
-    self.alpha = np.exp(-DT_MDL / 2.0)
+    self.v_desired = FirstOrderFilter(init_v, 2.0, DT_MDL)
+    self.a_desired = FirstOrderFilter(init_a, 0.5, DT_MDL)
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -67,16 +67,14 @@ 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 = v_ego
-      self.a_desired = a_ego
-      # Smoothly changing between accel trajectory is only relevant when OP is driving
-      prev_accel_constraint = False
+      self.v_desired.x = v_ego
+      # Smooth in current a_ego so we always have an accurate plan
+      self.a_desired.update(a_ego)
 
     # Prevent divergence, smooth in current v_ego
-    self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
-    self.v_desired = max(0.0, self.v_desired)
+    self.v_desired.update(v_ego)
+    self.v_desired.x = max(0.0, self.v_desired.x)
 
     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)
@@ -85,11 +83,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 + 0.05)
-    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
+    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)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
-    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.mpc.set_cur_state(self.v_desired.x, self.a_desired.x)
+    self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
     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)
@@ -100,9 +98,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
-    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
+    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
 
   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
-    self.acceleration = self.planner.a_desired
+    self.speed = self.planner.v_desired.x
+    self.acceleration = self.planner.a_desired.x
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts