From f8cec6f270d4b0437b9f600808f251c5c1d091d8 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 22 Jun 2026 19:35:51 -0500 Subject: [PATCH] Long Blong --- .../controls/lib/longitudinal_planner.py | 61 ++++++++++++++-- .../tests/test_longitudinal_planner.py | 72 +++++++++++++++++++ starpilot/controls/starpilot_planner.py | 4 +- 3 files changed, 131 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 036b5ff25..925046e99 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -48,8 +48,8 @@ STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 0.8 STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08 STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL = 0.18 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.12 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 1.4 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1 STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME = 0.30 LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75 LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 @@ -75,7 +75,7 @@ RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL = 0.9 RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED = 2.0 RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH = 4.0 LEAD_DEPART_ACCEL_HOLD_TIME = 1.2 -LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 1.5 +LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 2.0 LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6 LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_DELTA = 0.5 LEAD_DEPART_ACCEL_HOLD_MIN_GAP = 3.5 @@ -86,6 +86,10 @@ 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_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 +LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_HEADWAY_MARGIN = 0.10 LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED = 4.5 LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE = 4.0 LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE = 18.0 @@ -595,6 +599,7 @@ class LongitudinalPlanner: self.untracked_slow_lead_confirm_t = 0.0 self.manual_stop_resume_override_until = 0.0 self.lead_depart_accel_hold_until = 0.0 + self.lead_depart_accel_hold_floor = None self.spacious_follow_cap_bypass_until = 0.0 self.post_departure_follow_settle_until = 0.0 @@ -1216,6 +1221,37 @@ class LongitudinalPlanner: 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)) + 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: + return None + + lead_radar = bool(getattr(lead, "radar", False)) + lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) + if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: + return None + + if abs(float(getattr(lead, "yRel", 0.0))) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET: + return None + + d_rel = float(getattr(lead, "dRel", 0.0)) + if d_rel < LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_GAP: + return None + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_LEAD_BRAKE: + return None + + closing_speed = max(float(v_ego) - float(getattr(lead, "vLead", 0.0)), 0.0) + if closing_speed > LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_CLOSING_SPEED: + return None + + actual_headway = d_rel / max(float(v_ego), 1e-3) + headway_margin = actual_headway - float(t_follow) + if headway_margin < LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_HEADWAY_MARGIN: + return None + + return float(self.lead_depart_accel_hold_floor) + def get_low_speed_weak_lead_accel_cap(self, lead, v_ego): if lead is None or not lead.status: return None @@ -2751,8 +2787,10 @@ class LongitudinalPlanner: if depart_safety_veto or output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)): self.lead_depart_accel_hold_until = 0.0 + self.lead_depart_accel_hold_floor = None lead_depart_accel_floor = None + lead_depart_accel_floor_reused = False if lead_control_active and not output_should_stop and not depart_safety_veto: lead_depart_accel_floors = [ floor for floor in ( @@ -2762,8 +2800,23 @@ class LongitudinalPlanner: ] if lead_depart_accel_floors: lead_depart_accel_floor = max(lead_depart_accel_floors) + self.lead_depart_accel_hold_floor = lead_depart_accel_floor if sm['carState'].standstill: self.lead_depart_accel_hold_until = now_t + LEAD_DEPART_ACCEL_HOLD_TIME + elif self.lead_depart_accel_hold_floor is not None and now_t < self.lead_depart_accel_hold_until: + reusable_hold_floors = [ + floor for floor in ( + self.get_reusable_lead_depart_accel_floor(self.lead_one, scene_v_ego, effective_t_follow), + self.get_reusable_lead_depart_accel_floor(self.lead_two, scene_v_ego, effective_t_follow), + ) if floor is not None + ] + if reusable_hold_floors: + lead_depart_accel_floor = max(reusable_hold_floors) + lead_depart_accel_floor_reused = True + else: + self.lead_depart_accel_hold_floor = None + elif now_t >= self.lead_depart_accel_hold_until: + self.lead_depart_accel_hold_floor = None lead_depart_accel_hold_active = ( lead_depart_accel_floor is not None and @@ -3029,7 +3082,7 @@ class LongitudinalPlanner: if lead_depart_accel_hold_active: output_a_target = max(output_a_target, lead_depart_accel_floor) - if low_speed_weak_lead_accel_cap is not None: + if low_speed_weak_lead_accel_cap is not None and not (lead_depart_accel_hold_active and lead_depart_accel_floor_reused): self.a_desired = min(self.a_desired, low_speed_weak_lead_accel_cap) output_a_target = min(output_a_target, low_speed_weak_lead_accel_cap) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 66e7bc074..9d25a752e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1248,6 +1248,32 @@ def test_standstill_slow_creep_depart_releases_after_short_confirm(model_version assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_slow_creep_depart_releases_near_stop_gap_with_modest_accel(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.0, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=5.7, v_lead=1.0, a_lead=0.09, radar=True, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) + for _ in range(max(frames, 1)): + planner.update(sm, make_toggles(model_version)) + + assert not planner.output_should_stop + assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_slow_creep_depart_does_not_release_on_gap_without_motion_signal(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -1489,6 +1515,52 @@ def test_standstill_moving_lead_holds_depart_accel_floor_after_stop_release(mode assert outputs[4] >= 0.25 +@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) + planner = LongitudinalPlanner(CP, init_v=0.0) + toggles = make_toggles(model_version) + + sm_release = make_sm( + 0.0, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=4.1, v_lead=1.05, a_lead=1.2, radar=False, model_prob=1.0), + ) + sm_release["carState"].standstill = True + sm_release["controlsState"].longControlState = LongCtrlState.stopping + sm_release["starpilotPlan"].vCruise = 10.0 + sm_release["modelV2"].action.shouldStop = False + + for _ in range(6): + planner.update(sm_release, toggles) + assert planner.output_should_stop + + planner.update(sm_release, toggles) + assert not planner.output_should_stop + assert planner.output_a_target >= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + + sm_hold = make_sm( + 0.5, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=5.8, v_lead=1.25, a_lead=0.08, radar=False, model_prob=1.0), + ) + sm_hold["carState"].standstill = False + sm_hold["controlsState"].longControlState = LongCtrlState.pid + sm_hold["starpilotPlan"].vCruise = 10.0 + sm_hold["modelV2"].action.shouldStop = False + + planner.update(sm_hold, toggles) + + assert not planner.output_should_stop + assert planner.output_a_target >= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_depart_accel_hold_cancels_if_lead_brakes(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 60582dc6b..8d196ca43 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -201,11 +201,11 @@ class StarPilotPlanner: self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles) conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled - if conditional_tracking_active and starpilot_toggles.conditional_experimental_mode: + if conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)): # Keep CEM's filters warm in AOL so engagement can inherit the current scene. self.starpilot_cem.update(v_ego, sm, starpilot_toggles) self.starpilot_ccm.experimental_mode = True - elif conditional_tracking_active and starpilot_toggles.conditional_chill_mode: + elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_chill_mode", False)): self.starpilot_ccm.update(v_ego, v_cruise, sm, starpilot_toggles) self.starpilot_cem.experimental_mode = False else: