From 85e3e11c10e4877ff8301738116e21fa63132489 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Thu, 4 Jun 2026 05:29:28 -0500 Subject: [PATCH] Radar for Leads Button --- common/params_keys.h | 1 + .../controls/lib/longitudinal_planner.py | 3 +- .../tests/test_longitudinal_planner.py | 51 ++++++++++++++++++- .../settings/starpilot/longitudinal.py | 5 ++ starpilot/common/safe_mode.py | 1 + starpilot/common/starpilot_variables.py | 1 + .../tools/device_settings_layout.json | 8 +++ .../ui/qt/offroad/longitudinal_settings.cc | 5 ++ .../ui/qt/offroad/longitudinal_settings.h | 2 +- 9 files changed, 74 insertions(+), 3 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 8456146aa..343b6a762 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -447,6 +447,7 @@ inline static std::unordered_map keys = { {"QOLLateral", {PERSISTENT, BOOL, "1", "0", 1}}, {"QOLLongitudinal", {PERSISTENT, BOOL, "1", "0", 1}}, {"QOLVisuals", {PERSISTENT, BOOL, "1", "0", 0}}, + {"RadarTakeoffs", {PERSISTENT, BOOL, "0", "0", 2}}, {"RadarTracksUI", {PERSISTENT, BOOL, "0", "0", 3}}, {"RainbowPath", {PERSISTENT, BOOL, "0", "0", 1}}, {"RandomEvents", {PERSISTENT, BOOL, "0", "0", 1}}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 654375acb..889f2d322 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1978,7 +1978,8 @@ class LongitudinalPlanner: lead.dRel >= standstill_nudge_gap + STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN for lead in (self.lead_one, self.lead_two) ) - depart_safety_veto = self.has_offcenter_radar_depart_conflict(sm) + depart_safety_veto = (not bool(getattr(starpilot_toggles, "radar_takeoffs", False)) + and self.has_offcenter_radar_depart_conflict(sm)) if lead_control_active and sm['carState'].standstill and moving_leads and not depart_safety_veto: output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 416fc0f42..8c833f799 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -111,7 +111,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta } -def make_toggles(model_version: str = "v11"): +def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False): return SimpleNamespace( taco_tune=False, classic_model=False, @@ -119,6 +119,7 @@ def make_toggles(model_version: str = "v11"): model_version=model_version, stop_distance=6.0, vEgoStopping=0.5, + radar_takeoffs=radar_takeoffs, ) @@ -1315,6 +1316,54 @@ def test_low_speed_radar_depart_hold_blocks_offcenter_radar_conflict(model_versi assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_radar_takeoffs_toggle_bypasses_offcenter_veto(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, y_rel=2.3), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) + + planner.update(sm, make_toggles(model_version, radar_takeoffs=True)) + + assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_low_speed_radar_takeoffs_toggle_bypasses_offcenter_veto(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, y_rel=2.2), + ) + sm["carState"].standstill = False + sm["controlsState"].longControlState = LongCtrlState.pid + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2) + + planner.update(sm, make_toggles(model_version, radar_takeoffs=True)) + + 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 diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 4596b9292..cbacffd50 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -240,6 +240,11 @@ class StarPilotLongitudinalLayout(_SettingsPage): get_value=lambda: f"{self._params.get_int('ForceStopDistanceOffset'):+d} ft", on_click=lambda: self._show_slider("ForceStopDistanceOffset", -20, 20, unit=" ft"), visible=lambda: self._params.get_bool("QOLLongitudinal") and self._params.get_bool("ForceStops")), + SettingRow("RadarTakeoffs", "toggle", tr_noop("Radar for Takeoffs"), + subtitle=tr_noop("Turns on/off using radar data to track leads at standstill, making following/takeoffs more responsive once leads move."), + get_state=lambda: self._params.get_bool("RadarTakeoffs"), + set_state=lambda s: self._params.put_bool("RadarTakeoffs", s), + visible=lambda: self._params.get_bool("QOLLongitudinal") and starpilot_state.car_state.hasRadar), ], tab_key="daily", column_pair="daily"), SettingSection(tr_noop("Standstill & Gears"), [ SettingRow("ForceStandstill", "toggle", tr_noop("Force Standstill"), diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 6a88761eb..63f02dd4c 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -79,6 +79,7 @@ SAFE_MODE_MANAGED_KEYS = ( "QOLLongitudinal", "ForceStops", "ForceStandstill", + "RadarTakeoffs", "IncreasedStoppedDistance", "MapGears", "MapAcceleration", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index c205ff244..7788708bf 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1128,6 +1128,7 @@ class StarPilotVariables: toggle.force_stops = self.get_value("ForceStops", condition=quality_of_life_longitudinal) toggle.force_stop_distance_offset = self.get_value("ForceStopDistanceOffset", cast=int, condition=(quality_of_life_longitudinal and toggle.force_stops)) toggle.force_standstill = self.get_value("ForceStandstill", condition=quality_of_life_longitudinal) + toggle.radar_takeoffs = self.get_value("RadarTakeoffs", condition=quality_of_life_longitudinal) toggle.increase_stopped_distance = self.get_value("IncreasedStoppedDistance", cast=float, condition=quality_of_life_longitudinal, conversion=distance_conversion) map_gears = self.get_value("MapGears", condition=quality_of_life_longitudinal) toggle.map_acceleration = self.get_value("MapAcceleration", condition=map_gears) diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index 0a407a960..eb329f0bb 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -1085,6 +1085,14 @@ "ui_type": "toggle", "parent_key": "QOLLongitudinal" }, + { + "key": "RadarTakeoffs", + "label": "Radar for Takeoffs", + "description": "Turns on/off using radar data to track leads at standstill, making following/takeoffs more responsive once leads move.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "QOLLongitudinal" + }, { "key": "IncreasedStoppedDistance", "label": "Increase Stopped Distance by:", diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.cc b/starpilot/ui/qt/offroad/longitudinal_settings.cc index bd5415a18..555acef29 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/starpilot/ui/qt/offroad/longitudinal_settings.cc @@ -159,6 +159,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * {"ForceStops", tr("Force Stop at \"Detected\" Stop Lights/Signs"), tr("Force openpilot to stop whenever the driving model \"detects\" a red light or stop sign.

Disclaimer: openpilot does not explicitly detect traffic lights or stop signs. In \"Experimental Mode\", openpilot makes end-to-end driving decisions from camera input, which means it may stop even when there's no clear reason!"), ""}, {"ForceStopDistanceOffset", tr("Force Stop Distance Offset"), tr("Tune where Force Stops bring the car to rest. Positive values let the car roll further before stopping (longer stop, closer to the line). Negative values stop the car sooner (more buffer before the line)."), ""}, {"ForceStandstill", tr("Force Standstill State"), tr("Keep openpilot in the standstill state until you press the gas pedal or the Resume/+ cruise button.

This applies to any engaged stop, not just red lights or stop signs."), ""}, + {"RadarTakeoffs", tr("Radar for Takeoffs"), tr("Turns on/off using radar data to track leads at standstill, making following/takeoffs more responsive once leads move."), ""}, {"IncreasedStoppedDistance", tr("Increase Stopped Distance by:"), tr("Add extra space when stopped behind vehicles. Increase for more room; decrease for shorter gaps."), ""}, {"MapGears", tr("Map Accel/Decel to Gears"), tr("Map the Acceleration or Deceleration profiles to the vehicle's \"Eco\" and \"Sport\" gear modes."), ""}, {"SetSpeedOffset", tr("Offset Set Speed by:"), tr("Increase the set speed by the chosen offset. For example, set +5 if you usually drive 5 over the limit."), ""}, @@ -1000,6 +1001,10 @@ void StarPilotLongitudinalPanel::updateToggles() { setVisible &= parent->hasRadar; } + else if (key == "RadarTakeoffs") { + setVisible &= parent->hasRadar; + } + else if (key == "MapGears") { setVisible &= parent->isToyota || parent->isHKG; setVisible &= !parent->isTSK; diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.h b/starpilot/ui/qt/offroad/longitudinal_settings.h index e98de7380..7a12c5c5a 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.h +++ b/starpilot/ui/qt/offroad/longitudinal_settings.h @@ -34,7 +34,7 @@ private: QSet curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"}; QSet customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"}; QSet longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune", "NavLongitudinalAllowed"}; - QSet qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"}; + QSet qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "RadarTakeoffs", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"}; QSet relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"}; QSet speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"}; QSet speedLimitControllerOffsetsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7"};