mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-12 15:33:45 +08:00
firestar5683
1ab5058bc8
Implement conditional driving modes with explicit scene, button and traffic state ownership.
93 lines
3.4 KiB
Python
93 lines
3.4 KiB
Python
"""Frozen CEM curve predicate and filter, without mode or actuator authority."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
from openpilot.common.realtime import DT_MDL
|
|
from openpilot.starpilot.model_geometry import road_curvature as frozen_road_curvature
|
|
|
|
|
|
CRUISING_SPEED_MPS = 5.0
|
|
MIN_LATERAL_ACCEL_MPS2 = 1.3
|
|
FILTER_THRESHOLD = 1.0 - 1.0 / math.e
|
|
INITIAL_FILTER_TIME_S = 0.8
|
|
MAX_FRAME_GAP_NS = 250_000_000
|
|
MPS_TO_MPH = 1.0 / 0.44704
|
|
|
|
|
|
def _finite(value: object, *, low: float, high: float) -> float | None:
|
|
if isinstance(value, bool) or not isinstance(value, (int, float)):
|
|
return None
|
|
try:
|
|
result = float(value)
|
|
except (OverflowError, ValueError):
|
|
return None
|
|
return result if math.isfinite(result) and low <= result <= high else None
|
|
|
|
|
|
def frozen_raw_curve(model: object, speed_mps: float, curvature: float, left_blinker: bool, right_blinker: bool) -> tuple[bool, bool] | None:
|
|
"""Return (predicted road curve, measured curve), before CEM filtering."""
|
|
speed = _finite(speed_mps, low=0.0, high=80.0)
|
|
measured = _finite(curvature, low=-1.0, high=1.0)
|
|
if speed is None or measured is None or type(left_blinker) is not bool or type(right_blinker) is not bool:
|
|
return None
|
|
road = frozen_road_curvature(model, speed)
|
|
if road is None:
|
|
return None
|
|
road_curvature, _ = road
|
|
# Algebraically equivalent to sqrt(1 / abs(curvature)) < vEgo,
|
|
# including the zero-curvature false case without division by zero.
|
|
predicted = speed > CRUISING_SPEED_MPS and abs(road_curvature) * speed * speed > 1.0 and not (left_blinker or right_blinker)
|
|
driving = abs(speed * speed * measured) >= MIN_LATERAL_ACCEL_MPS2
|
|
return predicted, driving
|
|
|
|
|
|
def _filter_time_no_traffic(speed_mps: float) -> float:
|
|
speed_mph = speed_mps * MPS_TO_MPH
|
|
if speed_mph <= 35.0:
|
|
return 0.0
|
|
if speed_mph >= 45.0:
|
|
return INITIAL_FILTER_TIME_S
|
|
return INITIAL_FILTER_TIME_S * (speed_mph - 35.0) / 10.0
|
|
|
|
|
|
class CurveDetector:
|
|
"""One CEM model-tick filter. Reset on missing evidence or frame discontinuity."""
|
|
|
|
def __init__(self):
|
|
self.reset()
|
|
|
|
def reset(self) -> None:
|
|
self.value = 0.0
|
|
self.filter_time_s = INITIAL_FILTER_TIME_S
|
|
self.last_observed_mono_ns: int | None = None
|
|
|
|
def step(self, *, observed_mono_ns: int, speed_mps: float, raw_curve: tuple[bool, bool] | None, traffic_mode: bool | None) -> bool | None:
|
|
observed = observed_mono_ns if type(observed_mono_ns) is int and 0 <= observed_mono_ns <= 10**21 else None
|
|
speed = _finite(speed_mps, low=0.0, high=80.0)
|
|
if (
|
|
observed is None
|
|
or speed is None
|
|
or raw_curve is None
|
|
or len(raw_curve) != 2
|
|
or any(type(item) is not bool for item in raw_curve)
|
|
or type(traffic_mode) is not bool
|
|
):
|
|
self.reset()
|
|
return None
|
|
if self.last_observed_mono_ns is not None:
|
|
interval = observed - self.last_observed_mono_ns
|
|
if interval <= 0:
|
|
return None # Re-reading one model event does not advance the filter.
|
|
if interval > MAX_FRAME_GAP_NS:
|
|
self.reset()
|
|
return None # Expired scene history cannot seed a new observation.
|
|
# Use the previous alpha; stop detection updates it for the next model tick.
|
|
alpha = DT_MDL / (self.filter_time_s + DT_MDL)
|
|
self.value = (1.0 - alpha) * self.value + alpha * float(any(raw_curve))
|
|
self.last_observed_mono_ns = observed
|
|
if not traffic_mode:
|
|
self.filter_time_s = _filter_time_no_traffic(speed)
|
|
return self.value >= FILTER_THRESHOLD and speed > CRUISING_SPEED_MPS
|