mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-12 07:23:45 +08:00
firestar5683
14cb60b8e5
Implement acceleration profiles, FollowJerk, force-stop and lead-departure owners. Keep selected acceleration limits and hazard arbitration explicit at their shared boundaries.
216 lines
8.8 KiB
Python
216 lines
8.8 KiB
Python
"""Force Stop's model approach, commitment, distance and driver-release policy."""
|
|
|
|
from dataclasses import dataclass
|
|
import math
|
|
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class StopTune:
|
|
handoff_m: float = 6.0
|
|
distance_bias_m: float = 0.0
|
|
lead_veto_m: float = 75.0
|
|
jerk_scale: float = 0.32
|
|
reanchor_speed_tolerance: float | None = None
|
|
low_speed_hold_mps: float | None = None
|
|
|
|
|
|
def vehicle_tune(fingerprint: str) -> StopTune:
|
|
if fingerprint == 'TOYOTA_CAMRY_TSS2':
|
|
return StopTune(handoff_m=4.5, distance_bias_m=6.0)
|
|
if fingerprint == 'HYUNDAI_ELANTRA_2021':
|
|
return StopTune(lead_veto_m=90.0, jerk_scale=0.8)
|
|
if fingerprint == 'HYUNDAI_SANTA_FE_2022':
|
|
return StopTune(reanchor_speed_tolerance=0.25, low_speed_hold_mps=2.5)
|
|
return StopTune()
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class StopFrame:
|
|
model_ns: int
|
|
speed_mps: float
|
|
horizon_m: float | None
|
|
light: bool
|
|
raw_model_stopped: bool
|
|
model_should_stop: bool
|
|
standstill: bool
|
|
enabled: bool
|
|
pedal_override: bool
|
|
tracking_lead: bool
|
|
lead_present: bool
|
|
lead_distance_m: float
|
|
lead_speed_mps: float
|
|
driving_in_curve: bool
|
|
road_curvature: float | None
|
|
blinker: bool
|
|
steering_angle_deg: float
|
|
resume_requested: bool = False
|
|
perception_available: bool = True
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class StopPlan:
|
|
model_ns: int = 0
|
|
speed_ceiling_mps: float | None = None
|
|
obstacle_m: float | None = None
|
|
forcing: bool = False
|
|
should_stop: bool = False
|
|
tracked_distance_m: float = 0.0
|
|
approach_distance_m: float = 0.0
|
|
jerk_scale: float = 1.0
|
|
manual_hold: bool = False
|
|
takeoff_light_observed: bool = False
|
|
takeoff_light: bool = False
|
|
|
|
|
|
class ForceStop:
|
|
def __init__(self):
|
|
self.reset()
|
|
|
|
def reset(self):
|
|
self.last_model_ns = 0
|
|
self.timer = 0.0
|
|
self.forcing = False
|
|
self.from_light = False
|
|
self.light_clear_ns = 0
|
|
self.override_until_ns = 0
|
|
self.entry_speed = None
|
|
self.activation_gate = False
|
|
self.stop_seen_ns = 0
|
|
self.standstill_since_ns = 0
|
|
self.standstill_reason = None
|
|
self.previously_enabled = False
|
|
self.distance = 0.0
|
|
self.distance_cap = 0.0
|
|
|
|
def _clear_standstill(self):
|
|
self.standstill_since_ns = 0
|
|
self.standstill_reason = None
|
|
|
|
def step(self, frame: StopFrame, tune: StopTune, offset_ft: int = 0) -> StopPlan:
|
|
now = frame.model_ns
|
|
if (type(now) is not int or now <= 0 or now <= self.last_model_ns or
|
|
self.last_model_ns and now - self.last_model_ns > 250_000_000):
|
|
self.reset()
|
|
return StopPlan()
|
|
dt = (now - self.last_model_ns) / 1e9 if self.last_model_ns else DT_MDL
|
|
self.last_model_ns = now
|
|
if not frame.enabled:
|
|
self.reset()
|
|
return StopPlan(model_ns=now)
|
|
speed = frame.speed_mps
|
|
horizon = frame.horizon_m if frame.horizon_m is not None else math.nan
|
|
curvature = frame.road_curvature if frame.road_curvature is not None else math.nan
|
|
if (not all(math.isfinite(value) for value in (speed, frame.steering_angle_deg)) or
|
|
frame.perception_available and not all(math.isfinite(value) for value in (horizon, curvature))):
|
|
self.reset()
|
|
return StopPlan()
|
|
available = frame.perception_available
|
|
if available and (frame.light or frame.raw_model_stopped):
|
|
if not frame.standstill:
|
|
self.stop_seen_ns = now
|
|
if frame.standstill:
|
|
self.stop_seen_ns = 0
|
|
stop_then_turn = bool(self.stop_seen_ns and now - self.stop_seen_ns < 4_000_000_000)
|
|
turn = speed <= 18 * 0.44704 and frame.blinker and abs(frame.steering_angle_deg) >= 25 and not stop_then_turn
|
|
curved = available and abs(curvature) >= .003 and not stop_then_turn
|
|
lead = (frame.lead_present and frame.lead_distance_m < tune.lead_veto_m and frame.lead_speed_mps < speed + 2)
|
|
model_active = available and horizon < (108.0 if self.activation_gate and frame.light else 100.0)
|
|
self.activation_gate = model_active and frame.light
|
|
permitted = (available and now >= self.override_until_ns and not frame.driving_in_curve and not turn and
|
|
not frame.tracking_lead and not lead)
|
|
model_path = frame.light and model_active and permitted and not curved
|
|
active = model_path
|
|
if model_path:
|
|
self.from_light = True
|
|
self.light_clear_ns = 0
|
|
elif not self.forcing:
|
|
self.from_light = False
|
|
self.light_clear_ns = 0
|
|
stop_scene = available and (active or frame.raw_model_stopped)
|
|
# Reopened geometry may withdraw a moving approach; it cannot classify
|
|
# a stationary hold as a green light.
|
|
go_scene = available and horizon >= 50.0 and not frame.raw_model_stopped and not frame.model_should_stop
|
|
engaged_stopped = not self.previously_enabled and frame.standstill
|
|
if frame.standstill and (self.forcing or engaged_stopped and stop_scene) and self.standstill_reason is None:
|
|
self.standstill_since_ns = now
|
|
self.standstill_reason = 'unclassified'
|
|
self.distance = 0
|
|
driver_release = frame.pedal_override or frame.resume_requested
|
|
if self.standstill_reason is not None and driver_release:
|
|
self.override_until_ns = now + 10_000_000_000
|
|
self._clear_standstill()
|
|
held = self.standstill_reason is not None
|
|
if not available and not self.forcing:
|
|
self.timer = 0
|
|
elif active and not frame.standstill:
|
|
self.timer = min(2.0, self.timer + DT_MDL)
|
|
elif turn and not self.forcing and not frame.standstill:
|
|
self.timer = 0
|
|
elif held:
|
|
self.timer = max(self.timer, .5)
|
|
elif self.forcing and frame.standstill and not frame.light and not frame.raw_model_stopped:
|
|
self.timer = 0
|
|
else:
|
|
self.timer = max(0, self.timer - DT_MDL * .25)
|
|
force = self.timer >= .5 or self.forcing and not frame.standstill or held
|
|
light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and
|
|
available and not frame.light and go_scene)
|
|
low_speed_hold = (light_cleared and tune.low_speed_hold_mps is not None and self.entry_speed is not None and
|
|
speed <= tune.low_speed_hold_mps and speed < self.entry_speed - .25)
|
|
if light_cleared and not low_speed_hold:
|
|
if not self.light_clear_ns:
|
|
self.light_clear_ns = now
|
|
elif now - self.light_clear_ns >= 500_000_000:
|
|
self.forcing = self.from_light = force = False
|
|
self.timer = 0
|
|
self.light_clear_ns = 0
|
|
else:
|
|
self.light_clear_ns = 0
|
|
if self.forcing and frame.standstill and not force:
|
|
self.override_until_ns = now + 10_000_000_000
|
|
if driver_release and force:
|
|
self.override_until_ns = now + 10_000_000_000
|
|
overridden = now < self.override_until_ns
|
|
offset = max(-20, min(20, offset_ft)) * .3048
|
|
approach = 0.0
|
|
ceiling = None
|
|
if force and not overridden:
|
|
if self.entry_speed is None and not frame.standstill:
|
|
self.entry_speed = speed
|
|
self.forcing |= not frame.standstill or held
|
|
if held:
|
|
self.distance = 0
|
|
ceiling = 0.0
|
|
else:
|
|
self.distance = max(0.0, self.distance - speed * dt)
|
|
if (available and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and
|
|
horizon > self.distance + 3 and (tune.reanchor_speed_tolerance is None or self.entry_speed is None or
|
|
speed >= self.entry_speed - tune.reanchor_speed_tolerance)):
|
|
self.distance = horizon
|
|
elif available:
|
|
self.distance = min(self.distance, horizon)
|
|
self.distance_cap = max(0.0, self.distance_cap - speed * dt)
|
|
self.distance = min(self.distance, self.distance_cap + 15 * min(self.distance_cap / 60, 1))
|
|
effective = self.distance + offset + tune.distance_bias_m
|
|
ceiling = math.sqrt(2 * .65 * max(0, effective - tune.handoff_m))
|
|
else:
|
|
self.forcing = False
|
|
self.entry_speed = None
|
|
self._clear_standstill()
|
|
self.distance = self.distance_cap = horizon if available else 0.0
|
|
if frame.light and permitted and not curved and not overridden:
|
|
approach = horizon
|
|
effective = approach + offset + tune.distance_bias_m
|
|
if effective > tune.handoff_m:
|
|
ceiling = math.sqrt(2 * .65 * (effective - tune.handoff_m))
|
|
self.previously_enabled = frame.enabled
|
|
stop_length = self.distance if self.forcing and self.distance > tune.handoff_m else approach
|
|
# The MPC obstacle includes its standing-distance term; adding it at the
|
|
# native boundary avoids moving the actual stop line by that buffer.
|
|
obstacle = stop_length * .93 + offset + tune.distance_bias_m if stop_length > tune.handoff_m else None
|
|
return StopPlan(now, ceiling, obstacle, self.forcing, bool(self.forcing and ceiling is not None and ceiling <= .5),
|
|
self.distance if self.forcing else 0.0, approach,
|
|
tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden))
|