Radar for Leads Button

common/params_keys.h

@@ -447,6 +447,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},

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)

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

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"),

starpilot/common/safe_mode.py

@@ -79,6 +79,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "QOLLongitudinal",
   "ForceStops",
   "ForceStandstill",
+  "RadarTakeoffs",
   "IncreasedStoppedDistance",
   "MapGears",
   "MapAcceleration",

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)

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:",

starpilot/ui/qt/offroad/longitudinal_settings.cc

@@ -159,6 +159,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
     {"ForceStops", tr("Force Stop at \"Detected\" Stop Lights/Signs"), tr("<b>Force openpilot to stop whenever the driving model \"detects\" a red light or stop sign.</b><br><br><i><b>Disclaimer</b>: 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!</i>"), ""},
     {"ForceStopDistanceOffset", tr("Force Stop Distance Offset"), tr("<b>Tune where Force Stops bring the car to rest.</b> 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("<b>Keep openpilot in the standstill state until you press the gas pedal or the Resume/+ cruise button.</b><br><br>This applies to any engaged stop, not just red lights or stop signs."), ""},
+    {"RadarTakeoffs", tr("Radar for Takeoffs"), tr("<b>Turns on/off using radar data to track leads at standstill</b>, making following/takeoffs more responsive once leads move."), ""},
     {"IncreasedStoppedDistance", tr("Increase Stopped Distance by:"), tr("<b>Add extra space when stopped behind vehicles.</b> Increase for more room; decrease for shorter gaps."), ""},
     {"MapGears", tr("Map Accel/Decel to Gears"), tr("<b>Map the Acceleration or Deceleration profiles to the vehicle's \"Eco\" and \"Sport\" gear modes.</b>"), ""},
     {"SetSpeedOffset", tr("Offset Set Speed by:"), tr("<b>Increase the set speed by the chosen offset.</b> 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;

starpilot/ui/qt/offroad/longitudinal_settings.h

@@ -34,7 +34,7 @@ private:
   QSet<QString> curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"};
   QSet<QString> customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"};
   QSet<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune", "NavLongitudinalAllowed"};
-  QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"};
+  QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "RadarTakeoffs", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"};
   QSet<QString> relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"};
   QSet<QString> speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"};
   QSet<QString> speedLimitControllerOffsetsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7"};