From 1489b38dac53748d58a0b7e382cd5ff8c8cc4cec Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Thu, 27 Aug 2026 16:39:15 -0400 Subject: [PATCH] hem rolled through sign --- .../controls/lib/hybrid_experimental_mode.py | 120 ++++++++++++------ 1 file changed, 78 insertions(+), 42 deletions(-) diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index ec206f4c3..eb6457022 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -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, }