mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 03:13:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

Faster Leads for $300 Trebek, and say hi to your motha

Browse Source 
Faster takeoffs after leads and green lights.
This commit is contained in:
whoisdomi
2026-06-01 16:43:58 -05:00
parent d2a8163f32
commit 78e9b5139c
2 changed files with 36 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+7 -10
View File
@@ -35,16 +35,16 @@ RAW_LEAD_SAFETY_TTC = 7.0
RAW_LEAD_SAFETY_DISTANCE = 40.0
STANDSTILL_LEAD_NUDGE_ACCEL = 0.05
STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0
STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.20
STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.35
STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5
STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6
STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 1.5
STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 0.8
STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08
LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75
LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25
LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.5
LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.45
LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.35
LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3
LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25
LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2
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
@@ -945,11 +945,8 @@ class LongitudinalPlanner:
return False
lead_radar = bool(getattr(lead, "radar", False))
if lead_radar:
return False
lead_prob = float(getattr(lead, "modelProb", 0.0))
if lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB:
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
lead_speed = max(float(lead.vLead), 0.0)
starpilot/controls/lib/conditional_experimental_mode.py
+29
View File
@@ -102,6 +102,7 @@ class ConditionalExperimentalMode:
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self._prev_ce_status = None
self.close_stopped_lead_since = 0.0
def update(self, v_ego, sm, starpilot_toggles):
now = time.monotonic()
@@ -157,7 +158,18 @@ class ConditionalExperimentalMode:
self.stop_light_detected &= not is_manual_ce_status(self.status_value)
self.stop_light_filter.x = 0
# At standstill behind a close, stopped lead, prefer Chill over CEM.
# Why: CEM is slow to release when the lead pulls away (waits on stop-light
# filter + STOP_LIGHT_DETECTED_HOLD_TIME). Chill reacts to lead departure faster.
STANDSTILL_LEAD_OVERRIDE_MAX_DISTANCE = 15.0 # meters
STANDSTILL_LEAD_OVERRIDE_MAX_SPEED = 1.0 # m/s
# Persistence required before handing off CEM->Chill. Cross-traffic (cars passing
# perpendicular in front) briefly registers as a close, stopped lead and would
# otherwise flap CEM out of EXP. Real queue-mates persist much longer than this.
STANDSTILL_LEAD_OVERRIDE_PERSIST_TIME = 0.5 # seconds
def get_standstill_stop_hold(self, sm):
now = time.monotonic()
dash_stop_sign = (
bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or
bool(getattr(sm["starpilotCarState"], "dashboardStopSign", 0) > 0)
@@ -168,6 +180,7 @@ class ConditionalExperimentalMode:
if pedal_override or not bool(sm["carState"].standstill):
self.standstill_stop_reason = None
self.close_stopped_lead_since = 0.0
return False
if dash_stop_sign:
@@ -179,8 +192,24 @@ class ConditionalExperimentalMode:
self.standstill_stop_reason = None
if self.standstill_stop_reason == "sign":
self.close_stopped_lead_since = 0.0
return True
lead = getattr(self.starpilot_planner, "lead_one", None)
close_stopped_lead = bool(
lead is not None and
getattr(lead, "status", False) and
float(getattr(lead, "dRel", float("inf"))) < self.STANDSTILL_LEAD_OVERRIDE_MAX_DISTANCE and
float(getattr(lead, "vLead", float("inf"))) < self.STANDSTILL_LEAD_OVERRIDE_MAX_SPEED
)
if close_stopped_lead:
if self.close_stopped_lead_since == 0.0:
self.close_stopped_lead_since = now
if (now - self.close_stopped_lead_since) >= self.STANDSTILL_LEAD_OVERRIDE_PERSIST_TIME:
return False
else:
self.close_stopped_lead_since = 0.0
return bool(self.stop_light_detected or force_stop_active or model_stopped)
def check_conditions(self, v_ego, sm, starpilot_toggles):