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

Refine HybridExperimentalMode: enhance vision filtering logic, improve standstill reset behavior, and expand low-speed acceleration lockout conditions

Browse Source
This commit is contained in:
Prabhaav Pillai
2026-08-26 16:11:12 -04:00
parent 513fc586cc
commit 1d02eeac0b
1 changed files with 19 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
starpilot/controls/lib/hybrid_experimental_mode.py
+19 -11
View File
@@ -150,15 +150,20 @@ class HybridExperimentalMode:
raw_vision_metric = max(speed_drop_ratio, stop_target_active, model_decel_strength)
w_vision_raw = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0))
self.w_vision_filtered = max(w_vision_raw, self.w_vision_filtered * 0.95)
# Standstill reset logic to prevent launch lag on green lights
# Hold the latch during the approach, decay only on departure
lead_departing = lead_status and (getattr(lead_one, "vLead", 0.0) > 0.5)
driver_departing = (a_chill > 0.4) and (not lead_status or lead_d_rel > 10.0)
model_stop_predicted = len(traj_v) > 1 and v_horizon < 0.5
departing_from_standstill = (lead_departing or driver_departing) and not model_stop_predicted
if v_ego < 0.15 and departing_from_standstill:
vision_departing = (v_horizon > 0.5) and (a_exp > 0.1)
departing = (lead_departing or vision_departing or driver_departing) and not model_stop_predicted
if departing:
self.w_vision_filtered = 0.0
elif w_vision_raw > 0.15:
self.w_vision_filtered = max(self.w_vision_filtered, w_vision_raw) # hold, no decay
else:
self.w_vision_filtered *= 0.97
w_vision = self.w_vision_filtered
@@ -170,12 +175,12 @@ class HybridExperimentalMode:
slow_horizon = sigmoid(3.0 - v_horizon, k=2.0)
stop_confidence = max(stop_target_active, slow_horizon * speed_drop_ratio)
# Bug A & B Fix: check if model plans a stop anywhere in near-to-mid distance
# Bug A & B Fix: check if model plans a stop/slowdown anywhere in near-to-mid distance
near_stop_planned = False
if len(traj_x) == len(traj_v) and len(traj_v) > 0:
near_stop_planned = np.any((traj_v < 1.0) & (traj_x < 35.0))
near_stop_planned = np.any((traj_v < 4.0) & (traj_x < 35.0))
elif len(traj_v) > 0:
near_stop_planned = np.any(traj_v[:12] < 1.0)
near_stop_planned = np.any(traj_v[:12] < 4.0)
if near_stop_planned:
stop_confidence = max(stop_confidence, 0.8)
@@ -232,13 +237,16 @@ class HybridExperimentalMode:
# 3. STANDSTILL ANCHOR
is_stopped = sigmoid(0.4 - v_ego, k=8.0)
is_staying_stopped = sigmoid(0.5 - v_horizon, k=6.0)
vision_departing = (v_horizon > 0.5) and (a_exp > 0.1)
departing = (lead_departing or vision_departing or driver_departing) and not model_stop_predicted
# Bug C Fix: Low-speed acceleration lockout (no longer bypassed during a rolling glitch)
if v_ego < 3.0 and self.w_vision_filtered > 0.25:
# Bug C Fix: Acceleration lockout expanded to approach band and gated on active latch
if self.w_vision_filtered > 0.25 and v_ego < 5.0 and not departing:
a_fused = min(a_fused, 0.0)
# Smooth soft lockout ramp: scale positive acceleration to zero based on filter intensity
if v_ego < 4.0 and a_fused > 0.0:
scale = max(0.0, 1.0 - (self.w_vision_filtered / 0.30))
a_fused *= scale
standstill_weight = (0.0 if departing else 1.0) * is_stopped * is_staying_stopped
a_anchored = lerp(a_fused, smooth_min(a_fused, -0.5, k=6.0), standstill_weight)