diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a7b563f6f..bb5a45bfc 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -90,7 +90,8 @@ LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB = 0.85 LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL = 0.12 LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE = 0.2 LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL = 0.25 -LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.45 +LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.55 +LEAD_DEPART_ACCEL_ASSIST = 0.10 LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_GAP = 3.75 LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_CLOSING_SPEED = 0.45 LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_LEAD_BRAKE = 0.2 @@ -1418,7 +1419,8 @@ class LongitudinalPlanner: max(LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED, 0.1), 0.0, 1.0)) accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip( 0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0) - return min(accel_cap, max(float(model_desired_accel), LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL)) + assisted_model_accel = float(model_desired_accel) + LEAD_DEPART_ACCEL_ASSIST + return min(accel_cap, max(assisted_model_accel, LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL)) def get_reusable_lead_depart_accel_floor(self, lead, v_ego, t_follow): if self.lead_depart_accel_hold_floor is None or lead is None or not lead.status: diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index ab6c54e56..8a60e9e4e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1896,6 +1896,24 @@ def test_standstill_moving_lead_holds_depart_accel_floor_after_stop_release(mode assert outputs[4] >= 0.25 +def test_route_251682_rav4_confirmed_depart_adds_bounded_accel_assist(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + lead = make_lead( + status=True, + d_rel=7.7, + v_lead=2.0, + a_lead=1.79, + radar=False, + model_prob=1.0, + ) + + floor = planner.get_lead_depart_accel_floor(lead, v_ego=0.0, model_desired_accel=0.44) + + assert 0.52 <= floor <= 0.54 + assert floor <= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_depart_accel_hold_reuses_floor_through_softening_lead_delta(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)