Fix low-speed allow_throttle behavior in long planner (#33894)

* Misc fixes

* don't check allow_throttle slowdown for e2e

* Removed unused variable

* believe this clip is still necessary

* Update process replay refs

---------

Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com>

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -347,7 +347,8 @@ class LongitudinalMpc:
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
     self.params[:,0] = ACCEL_MIN
-    self.params[:,1] = self.max_a
+    # negative accel constraint causes problems because negative speed is not allowed
+    self.params[:,1] = max(0.0, self.max_a)
 
     # Update in ACC mode or ACC/e2e blend
     if self.mode == 'acc':
@@ -356,6 +357,7 @@ class LongitudinalMpc:
       # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
       # when the leads are no factor.
       v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
+      # TODO does this make sense when max_a is negative?
       v_upper = v_ego + (T_IDXS * self.max_a * 1.05)
       v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
                                  v_lower,

selfdrive/controls/lib/longitudinal_planner.py

@@ -22,7 +22,7 @@ A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 ALLOW_THROTTLE_THRESHOLD = 0.5
-ACCEL_LIMIT_MARGIN = 0.05
+MIN_ALLOW_THROTTLE_SPEED = 2.5
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
@@ -151,12 +151,12 @@ class LongitudinalPlanner:
     self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
     x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error)
     # Don't clip at low speeds since throttle_prob doesn't account for creep
-    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= 5.0
+    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
 
     if not self.allow_throttle:
-      # MPC breaks when accel limits would cause negative velocity within the MPC horizon, so we clip the max accel limit at vEgo/T_MAX plus a bit of margin
-      clipped_accel_coast = max(accel_coast, accel_limits_turns[0], -v_ego / T_IDXS_MPC[-1] + ACCEL_LIMIT_MARGIN)
-      accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast)
+      clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
+      clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
+      accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp)
 
     if force_slow_decel:
       v_cruise = 0.0

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -108,18 +108,6 @@ def create_maneuvers(kwargs):
       breakpoints=[0.0, 2., 2.01],
       **kwargs,
     ),
-    Maneuver(
-      "slow to 5m/s with allow_throttle = False and pitch = +0.1",
-      duration=25.,
-      initial_speed=20.,
-      lead_relevancy=False,
-      prob_throttle_values=[1., 0., 0.],
-      cruise_values=[20., 20., 20.],
-      pitch_values=[0., 0.1, 0.1],
-      breakpoints=[0.0, 2., 2.01],
-      ensure_slowdown=True,
-      **kwargs,
-    ),
     Maneuver(
       "approach slower cut-in car at 20m/s",
       duration=20.,
@@ -163,6 +151,20 @@ def create_maneuvers(kwargs):
       **kwargs,
     ),
   ]
+  if not kwargs['e2e']:
+    # allow_throttle won't trigger with e2e
+    maneuvers.append(Maneuver(
+      "slow to 5m/s with allow_throttle = False and pitch = +0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=False,
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., 0.1, 0.1],
+      breakpoints=[0.0, 2., 2.01],
+      ensure_slowdown=True,
+      **kwargs,
+    ))
   if not kwargs['force_decel']:
     # controls relies on planner commanding to move for stock-ACC resume spamming
     maneuvers.append(Maneuver(

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-2f17dc637bb685cadc08521c63f26c2f9af604d0
+992ac80ef848afb85562ca24b1c5a3d410aacd05