mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-01 05:33:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

hem rolled through sign

Browse Source
This commit is contained in:
Prabhaav Pillai
2026-08-27 16:39:15 -04:00
parent d3c8050a36
commit 1489b38dac
1 changed files with 78 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
starpilot/controls/lib/hybrid_experimental_mode.py
+78 -42
View File
@@ -1,10 +1,11 @@
#!/usr/bin/env python3
import time
import math
from openpilot.common.realtime import DT_MDL
def lerp(a: float, b: float, t: float) -> float:
"""Linear interpolation / blend between a and b by weight t (0.0 to 1.0)."""
"""Linear interpolation between a and b by weight t (0.0 to 1.0)."""
return (1.0 - t) * a + t * b
@@ -14,18 +15,26 @@ def clamp(val: float, low: float, high: float) -> float:
class HybridExperimentalMode:
"""
1. Open Road: Follows Chill MPC cruise & radar headway.
2. Slower Lead Approach: Blends smooth vision decel with MPC follow distance.
3. Stop Signs / Red Lights: Pure vision stopping authority (locks out positive cruise throttle).
4. Standstill / Stop Completion: Latches should_stop for LongControl mechanical brake hold.
5. Green Light / Lead Depart: Instant release back to Chill cruise acceleration.
Hybrid Experimental Mode (HEM)
1. Open Road: Pure Chill MPC cruise & radar follow distance.
2. Pure Braking Authority: Experimental decel dominates 100% with time-based latching.
3. Rolling-Dip Immunity: Cruise throttle locked out during stops; low-speed dips cannot surge.
4. Subtle Blending: Smooth transitions only when throttle commands are nearly identical.
5. Standstill Lock: Hardware brake hold latched until verified green/lead departure.
"""
# Time in seconds to hold braking dominance once triggered (immune to rolling dips)
BRAKE_HOLD_TIME = 3.0
STANDSTILL_SPEED = 0.8 # m/s (~1.8 mph)
MINOR_DIFF_THRESHOLD = 0.25 # m/s^2
def __init__(self):
self.DT = DT_MDL
self.w_vision = 0.0
self.prev_a_target = 0.0
self.last_exp_dominant = False
self.stopping_latched = False
self.brake_hold_until = 0.0
self.diag = {}
self.record_diag = False
@@ -38,6 +47,8 @@ class HybridExperimentalMode:
self.prev_a_target = float(a_ego) if math.isfinite(a_ego) else 0.0
self.w_vision = 0.0
self.last_exp_dominant = False
self.stopping_latched = False
self.brake_hold_until = 0.0
self.diag = {}
def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float):
@@ -46,6 +57,7 @@ class HybridExperimentalMode:
def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp,
should_stop_exp=False, should_stop_chill=False, gas_pressed=False):
now = time.monotonic()
# 0. Sanitize inputs
if not math.isfinite(a_chill):
@@ -68,48 +80,73 @@ class HybridExperimentalMode:
lead_v = float(getattr(lead_one, "vLead", 0.0))
lead_d = float(getattr(lead_one, "dRel", 150.0))
# 2. Vision Departure / Driver Override Detection (Priority Check)
at_standstill = v_ego < 0.8
lead_departing = at_standstill and lead_status and (lead_v > 0.6) and ((lead_v - v_ego) > 0.4)
vision_departing = at_standstill and (v_horizon > 1.5) and (a_exp > 0.15)
# 2. Standstill & Departure Detection
at_standstill = v_ego < self.STANDSTILL_SPEED
driver_override = bool(gas_pressed)
lead_departing = at_standstill and lead_status and (lead_v > 0.6) and ((lead_v - v_ego) > 0.4)
# Require sustained vision acceleration & open lookahead to verify departure
vision_departing = at_standstill and (v_horizon > 2.5) and (v_short > 1.2) and (a_exp > 0.20)
is_departing = lead_departing or vision_departing or driver_override
# 3. Dual-Regime Fusion
# Braking: Experimental mode gets 100% authority whenever it wants to decelerate
# more than Chill MPC (a_exp < a_chill). Openpilot vision models rarely plan a
# hard v=0 in the trajectory until actual standstill (they show a rolling dip to
# ~2 m/s then resume), so gating braking behind trajectory thresholds released the
# brakes at ~5 mph mid-stop. No trajectory gating, no filtering, no dilution here.
a_brake_fused = min(a_chill, a_exp)
# Throttle: Chill MPC controls cruise acceleration, with optional exp bias.
a_throttle_fused = a_chill + max(0.0, a_exp - a_chill) * self.HYBRID_EXP_BIAS
# 3. Pure Braking & Stopping Trigger
# Trigger on any real vision decel, stop request, or low-speed stop-line trajectory
wants_pure_braking = (
(a_exp < (a_chill - 0.05) and a_exp < -0.10) or
should_stop_exp or
(v_min < 0.5 and v_ego < 4.0 and a_exp < 0.1)
)
# 4. Output Arbitration
# If the model is commanding real deceleration (a_exp < 0, stronger than Chill),
# grant it full braking immediately and hold it all the way to standstill.
is_braking = not is_departing and (a_exp < a_chill and a_exp < 0.0)
self.last_exp_dominant = bool(is_braking or should_stop_exp)
# 4. Stopping State & Time-Based Latch Management
if driver_override or is_departing:
self.stopping_latched = False
self.brake_hold_until = 0.0
elif wants_pure_braking:
self.stopping_latched = True
self.brake_hold_until = now + self.BRAKE_HOLD_TIME
elif self.stopping_latched and not at_standstill:
# Road opened up mid-slowdown (e.g. light turned green before complete stop)
if a_exp > 0.35 and v_min > (v_ego * 0.9) and not should_stop_exp and now >= self.brake_hold_until:
self.stopping_latched = False
# w_vision is a diagnostic mirror of braking dominance (used by replay tooling).
if is_departing:
self.w_vision = 0.0
elif self.last_exp_dominant:
latch_active = (now < self.brake_hold_until) or self.stopping_latched
# 5. Output Arbitration & Regime Fusion
if latch_active:
# PURE BRAKING DOMINANCE: Lock out positive cruise throttle completely
self.last_exp_dominant = True
self.w_vision = 1.0
else:
self.w_vision = max(0.0, self.w_vision - 0.05)
if self.last_exp_dominant:
a_out = a_brake_fused
else:
a_out = a_throttle_fused
if a_exp < 0.0:
a_vision_brake = a_exp * self.VISION_BRAKE_SENSITIVITY
a_out = min(a_chill, a_vision_brake)
else:
# Near bottom of rolling dip: prevent positive acceleration surges
a_out = min(a_chill, 0.0)
# 5. Authoritative Standstill Handshake
# Bring to full mechanical stop hold when stopping is requested, or when ego is
# near zero speed while still intending to brake.
else:
# CRUISE / THROTTLE REGIME:
self.last_exp_dominant = False
a_diff = a_exp - a_chill
if a_diff < 0.0 and a_exp < 0.0:
# Transient mild deceleration
a_out = min(a_chill, a_exp)
self.w_vision = max(0.0, self.w_vision - 0.05)
elif abs(a_diff) <= self.MINOR_DIFF_THRESHOLD:
# Minor difference: smooth blend between Chill and Exp
blend_weight = abs(a_diff) / self.MINOR_DIFF_THRESHOLD
a_out = lerp(a_chill, a_exp, blend_weight * 0.5)
self.w_vision = max(0.0, self.w_vision - 0.05)
else:
# Open road acceleration governed by Chill MPC + optional exp bias
a_out = a_chill + max(0.0, a_diff) * self.HYBRID_EXP_BIAS
self.w_vision = max(0.0, self.w_vision - 0.05)
# 6. Authoritative Standstill Handshake
standstill_intent = not is_departing and (
should_stop_exp or
(v_ego < 0.6 and (v_horizon < 0.8 or a_exp < -0.1))
(self.stopping_latched and v_ego < 0.6) or
(v_ego < 0.5 and (v_horizon < 1.0 or a_exp < -0.05))
)
should_stop_fused = bool(should_stop_chill or standstill_intent)
@@ -127,14 +164,13 @@ class HybridExperimentalMode:
"vision_departing": vision_departing,
"driver_override": driver_override,
"is_departing": is_departing,
"is_braking": is_braking,
"a_brake_fused": a_brake_fused,
"a_throttle_fused": a_throttle_fused,
"stopping_latched": self.stopping_latched,
"latch_active": latch_active,
"exp_dominant": self.last_exp_dominant,
"standstill_intent": standstill_intent,
"should_stop_fused": should_stop_fused,
"a_out": a_out,
"regime": "brake" if self.last_exp_dominant else "throttle",
"regime": "pure_brake" if latch_active else "cruise",
"standstill": standstill_intent,
}