From 119fcb15a1fb7e7289a719d18d83c4925e98a358 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 3 Jun 2026 13:29:59 -0500 Subject: [PATCH] leady speedy --- .../controls/lib/longitudinal_planner.py | 26 +++++++++- .../tests/test_longitudinal_planner.py | 47 +++++++++++++++++++ 2 files changed, 72 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 71c146775..195a6c560 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -45,6 +45,8 @@ LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 +RADAR_ONLY_DEPART_HOLD_MAX_EGO_SPEED = 1.6 +RADAR_ONLY_DEPART_HOLD_MAX_DISTANCE = 18.0 LEAD_DEPART_ACCEL_HOLD_TIME = 1.2 LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 1.5 LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6 @@ -960,6 +962,8 @@ class LongitudinalPlanner: return False lead_radar = bool(getattr(lead, "radar", False)) + if lead_radar: + return 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 False @@ -980,6 +984,8 @@ class LongitudinalPlanner: return None lead_radar = bool(getattr(lead, "radar", False)) + if lead_radar: + return None 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 @@ -1903,15 +1909,27 @@ class LongitudinalPlanner: standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5 moving_leads = [lead for lead in (self.lead_one, self.lead_two) - if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap] + if lead.status and not bool(getattr(lead, "radar", False)) and + lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap] confident_depart_ready = any(self.is_confident_lead_depart(lead, float(sm['carState'].vEgo)) for lead in (self.lead_one, self.lead_two)) lead_depart_ready = any( lead.status and + not bool(getattr(lead, "radar", False)) and lead.vLead >= STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED and lead.dRel >= standstill_nudge_gap + STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN for lead in (self.lead_one, self.lead_two) ) + radar_depart_hold = bool( + float(sm['carState'].vEgo) <= RADAR_ONLY_DEPART_HOLD_MAX_EGO_SPEED and + any( + lead.status and + bool(getattr(lead, "radar", False)) and + float(getattr(lead, "dRel", 0.0)) > 0.0 and + float(getattr(lead, "dRel", 0.0)) <= RADAR_ONLY_DEPART_HOLD_MAX_DISTANCE + for lead in (self.lead_one, self.lead_two) + ) + ) if lead_control_active and sm['carState'].standstill and moving_leads: output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) @@ -2127,6 +2145,12 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, close_release_hold_cap) output_a_target = min(output_a_target, close_release_hold_cap) + if radar_depart_hold and lead_depart_accel_floor is None and not confident_depart_ready and not lead_depart_ready: + self.a_desired = min(self.a_desired, 0.0) + output_a_target = min(output_a_target, 0.0) + if sm['carState'].standstill: + output_should_stop = True + if lead_depart_accel_hold_active: output_a_target = max(output_a_target, lead_depart_accel_floor) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index c0ecd4438..78519e48b 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1232,6 +1232,53 @@ def test_standstill_moving_lead_depart_accel_hold_cancels_if_lead_brakes(model_v assert planner.output_a_target < 0.1 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_radar_only_lead_does_not_trigger_depart_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.45, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + assert planner.output_a_target <= 0.0 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_low_speed_radar_only_lead_does_not_trigger_depart_accel_hold(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=1.25) + + sm = make_sm( + 1.25, + desired_accel=0.20, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999), + ) + sm["carState"].standstill = False + sm["controlsState"].longControlState = LongCtrlState.pid + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_a_target <= 0.0 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): far_v_ego = 29.26