plan stan

selfdrive/controls/lib/longitudinal_planner.py

@@ -29,6 +29,7 @@ ALLOW_THROTTLE_THRESHOLD = 0.4
 ALLOW_THROTTLE_HYSTERESIS = 0.05
 ALLOW_THROTTLE_ENABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD + ALLOW_THROTTLE_HYSTERESIS
 ALLOW_THROTTLE_DISABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD - ALLOW_THROTTLE_HYSTERESIS
+ALLOW_THROTTLE_TRANSITION_CONFIRM_TIME = 0.25
 MIN_ALLOW_THROTTLE_SPEED = 5.0
 MODEL_LAUNCH_DISARM_SPEED = 2.0
 MODEL_LAUNCH_COMMIT_TIME = 3.5
@@ -582,6 +583,7 @@ class LongitudinalPlanner:
     self.fcw = False
     self.dt = dt
     self.model_allow_throttle = True
+    self.model_allow_throttle_transition_t = 0.0
     self.allow_throttle = True
     self.mode = 'acc'
     self.is_preap = (
@@ -2476,6 +2478,7 @@ class LongitudinalPlanner:
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active
       self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
       self.model_allow_throttle = True
+      self.model_allow_throttle_transition_t = 0.0
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -2494,14 +2497,24 @@ class LongitudinalPlanner:
       self.model_launch_stop_seen = False
     model_launch_v = np.array(v, copy=True)
     model_launch_a = np.array(a, copy=True)
-    # Don't clip at low speeds since throttle_prob doesn't account for creep. Use
-    # hysteresis here because raw gasPressProb noise can chatter the throttle gate.
+    # Don't clip at low speeds since throttle_prob doesn't account for creep. Raw
+    # gasPressProb can cross both hysteresis thresholds for only a few model frames,
+    # so confirm transitions before changing the physical coast cap.
     if v_ego <= MIN_ALLOW_THROTTLE_SPEED:
       self.model_allow_throttle = True
-    elif self.model_allow_throttle:
-      self.model_allow_throttle = throttle_prob > ALLOW_THROTTLE_DISABLE_THRESHOLD
+      self.model_allow_throttle_transition_t = 0.0
     else:
-      self.model_allow_throttle = throttle_prob > ALLOW_THROTTLE_ENABLE_THRESHOLD
+      transition_requested = (
+        throttle_prob <= ALLOW_THROTTLE_DISABLE_THRESHOLD if self.model_allow_throttle
+        else throttle_prob > ALLOW_THROTTLE_ENABLE_THRESHOLD
+      )
+      if transition_requested:
+        self.model_allow_throttle_transition_t += self.dt
+        if self.model_allow_throttle_transition_t + 1e-6 >= ALLOW_THROTTLE_TRANSITION_CONFIRM_TIME:
+          self.model_allow_throttle = not self.model_allow_throttle
+          self.model_allow_throttle_transition_t = 0.0
+      else:
+        self.model_allow_throttle_transition_t = 0.0
     self.allow_throttle = self.model_allow_throttle and not sm['starpilotPlan'].disableThrottle
 
     if not self.allow_throttle:

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -2296,6 +2296,10 @@ def test_allow_throttle_hysteresis_filters_gas_prob_chatter():
   assert planner.allow_throttle
 
   sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.34)
+  for _ in range(4):
+    planner.update(sm, toggles)
+    assert planner.model_allow_throttle
+    assert planner.allow_throttle
   planner.update(sm, toggles)
   assert not planner.model_allow_throttle
   assert not planner.allow_throttle
@@ -2306,11 +2310,41 @@ def test_allow_throttle_hysteresis_filters_gas_prob_chatter():
   assert not planner.allow_throttle
 
   sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.46)
+  for _ in range(4):
+    planner.update(sm, toggles)
+    assert not planner.model_allow_throttle
+    assert not planner.allow_throttle
   planner.update(sm, toggles)
   assert planner.model_allow_throttle
   assert planner.allow_throttle
 
 
+def test_allow_throttle_confirmation_filters_route_length_model_pulses():
+  v_ego = 25.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, gas_press_prob=0.6)
+  toggles = make_toggles()
+
+  planner.update(sm, toggles)
+
+  # Representative gasPressProb runs from route b85c25a4c6f99d83/0000000c:
+  # repeated 0.05-0.20 second threshold crossings must not change the coast cap.
+  for probability, frames in ((0.30, 2), (0.50, 1), (0.32, 3), (0.48, 4), (0.29, 4)):
+    sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=probability)
+    for _ in range(frames):
+      planner.update(sm, toggles)
+      assert planner.model_allow_throttle
+      assert planner.allow_throttle
+
+  # A sustained model request still applies the physical coast cap.
+  sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.2)
+  for _ in range(5):
+    planner.update(sm, toggles)
+  assert not planner.model_allow_throttle
+  assert not planner.allow_throttle
+
+
 def test_no_throttle_cap_stays_at_coast_limit_until_throttle_returns():
   v_ego = 8.5
 
@@ -2320,7 +2354,8 @@ def test_no_throttle_cap_stays_at_coast_limit_until_throttle_returns():
   sm["carControl"].orientationNED = [0.0, 0.1, 0.0]
   toggles = make_toggles()
 
-  planner.update(sm, toggles)
+  for _ in range(5):
+    planner.update(sm, toggles)
 
   accel_coast = max(get_vehicle_min_accel(CP, v_ego), get_coast_accel(sm["carControl"].orientationNED[1]))