diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4c218d3d7..744c69b2a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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: diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 91486da03..b672b68c8 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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]))