mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
152 lines
8.4 KiB
Python
152 lines
8.4 KiB
Python
"""Read-only projection of current planner inputs into one curve runtime frame."""
|
|
|
|
from dataclasses import replace
|
|
import math
|
|
import time
|
|
|
|
from openpilot.cereal.services import SERVICE_LIST
|
|
from openpilot.common.realtime import DT_MDL
|
|
from openpilot.starpilot.curve_speed.runtime import Frame, INPUT_MAX_AGE_NS, MODEL_MAX_AGE_NS, Runtime
|
|
from openpilot.starpilot.curve_speed.target import CurveProfile
|
|
from openpilot.starpilot.lead_tracking import FILTER_THRESHOLD, FILTER_TIME_S, LeadDetector
|
|
|
|
CLOCK_PAIR_MAX_SKEW_NS = 5_000_000
|
|
LEAD_WARMUP_FRAMES = math.ceil(math.log(1 - FILTER_THRESHOLD) / math.log(1 - DT_MDL / (FILTER_TIME_S + DT_MDL)))
|
|
|
|
|
|
def clock_pair_ns() -> tuple[int, int] | None:
|
|
"""Bracket BOOTTIME with Python MONOTONIC; never compare raw epochs."""
|
|
try:
|
|
before = time.monotonic_ns()
|
|
boot_clock = getattr(time, 'CLOCK_BOOTTIME', time.CLOCK_MONOTONIC)
|
|
boot = time.clock_gettime_ns(boot_clock)
|
|
after = time.monotonic_ns()
|
|
except OSError:
|
|
return None
|
|
return ((before + after) // 2, boot) if 0 <= after - before <= CLOCK_PAIR_MAX_SKEW_NS else None
|
|
|
|
|
|
def _fresh(sm, service: str, now_ns: int) -> bool:
|
|
stamp = sm.logMonoTime.get(service, 0)
|
|
frequency = SERVICE_LIST[service].frequency
|
|
age_limit = min(INPUT_MAX_AGE_NS, int(2e9 / frequency)) if frequency > 0 else 0
|
|
return (type(stamp) is int and 0 < stamp <= now_ns and now_ns - stamp <= age_limit and
|
|
bool(sm.valid.get(service, False)) and bool(sm.alive.get(service, False)))
|
|
|
|
|
|
def frame_from_submaster(sm, CP, now_ns: int, *, drive_id: int = 0) -> Frame:
|
|
"""Missing transport clears authority without manufacturing a no-lead claim."""
|
|
services = ('modelV2', 'carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState')
|
|
current = all(_fresh(sm, name, now_ns) for name in services)
|
|
model_ns = sm.logMonoTime.get('modelV2', 0)
|
|
model_ns = model_ns if type(model_ns) is int else 0
|
|
try:
|
|
model = sm['modelV2']
|
|
profile = CurveProfile.from_model(model, model_ns) if _fresh(sm, 'modelV2', now_ns) else None
|
|
car_state, car_control = sm['carState'], sm['carControl']
|
|
controls, selfdrive = sm['controlsState'], sm['selfdriveState']
|
|
cruise_mps = float(car_state.vCruise) / 3.6
|
|
system_longitudinal = bool(CP.openpilotLongitudinalControl) and not any(getattr(CP, flag, False) for flag in ('passive', 'dashcamOnly', 'notCar'))
|
|
return Frame(now_ns=now_ns, model_ns=model_ns, inputs_fresh=current and bool(car_state.canValid) and not bool(car_state.canTimeout),
|
|
profile=profile, system_longitudinal=system_longitudinal, long_active=bool(car_control.longActive),
|
|
enabled=bool(selfdrive.enabled), force_decel=bool(controls.forceDecel), cruise_mps=cruise_mps,
|
|
ego_mps=float(car_state.vEgo), measured_curvature=float(controls.curvature), tracking_lead=None,
|
|
blinker=bool(car_state.leftBlinker or car_state.rightBlinker), gas_pressed=bool(car_state.gasPressed),
|
|
brake_pressed=bool(car_state.brakePressed), drive_id=drive_id)
|
|
except (KeyError, AttributeError, TypeError, ValueError, OverflowError):
|
|
return Frame(now_ns, model_ns, False, None, False, False, False, False, 0.0, 0.0,
|
|
0.0, None, False, False, False, drive_id)
|
|
|
|
|
|
class CurveHost:
|
|
"""Planner-owned candidate/status shell; no Params writes or plan publication."""
|
|
|
|
def __init__(self, document: object = None, *, enabled: bool = False, no_lead: bool = False,
|
|
clock_pair=clock_pair_ns, replay: bool = False):
|
|
self.runtime = Runtime(document, enabled=enabled, no_lead=no_lead)
|
|
self.clock_pair = clock_pair
|
|
self.replay = replay
|
|
self.boot_offset_ns: int | None = None
|
|
self.source_floor_ns = 0
|
|
self.leads = LeadDetector()
|
|
self.lead_samples = 0
|
|
self.lead_drive_id = 0
|
|
self.previous_long_active = False
|
|
self.previous_active_headway: float | None = None
|
|
|
|
def _reset_leads(self) -> None:
|
|
self.leads.reset()
|
|
self.lead_samples = 0
|
|
self.previous_long_active = False
|
|
self.previous_active_headway = None
|
|
|
|
def sample(self, sm, CP, now_ns: int, *, drive_id: int = 0, event=None, follow_time_s: float | None = None):
|
|
frame = frame_from_submaster(sm, CP, now_ns, drive_id=drive_id)
|
|
if drive_id and self.lead_drive_id and drive_id != self.lead_drive_id:
|
|
self._reset_leads()
|
|
if drive_id:
|
|
self.lead_drive_id = drive_id
|
|
if not self.replay:
|
|
pair = self.clock_pair()
|
|
if pair is None or any(type(value) is not int or value <= 0 for value in pair):
|
|
self.boot_offset_ns = None
|
|
self.source_floor_ns = max(self.source_floor_ns, now_ns)
|
|
self.runtime.reset(drive_id=drive_id)
|
|
self._reset_leads()
|
|
return self.runtime.step(replace(frame, inputs_fresh=False), event=None)
|
|
offset = pair[1] - pair[0]
|
|
rearmed = False
|
|
if self.boot_offset_ns is None or abs(offset - self.boot_offset_ns) > CLOCK_PAIR_MAX_SKEW_NS:
|
|
self.boot_offset_ns = offset
|
|
self.source_floor_ns = pair[0]
|
|
self.runtime.reset(drive_id=drive_id)
|
|
self._reset_leads()
|
|
rearmed = True
|
|
required = ('modelV2', 'carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState')
|
|
try:
|
|
camera_eof = sm['modelV2'].timestampEof
|
|
camera_age = pair[1] - camera_eof if type(camera_eof) is int and camera_eof > 0 else -1
|
|
camera_current = (0 <= camera_age <= MODEL_MAX_AGE_NS and camera_eof <= now_ns + offset)
|
|
except (KeyError, AttributeError, TypeError, ValueError, OverflowError):
|
|
camera_current = False
|
|
# A newly published model envelope cannot renew the age of its camera
|
|
# input. EOF uses BOOTTIME; only the bracketed clock pair may compare it.
|
|
# Offline replay intentionally keeps its separate recorded timeline.
|
|
if (not camera_current or not 0 < frame.model_ns <= now_ns or
|
|
pair[0] - frame.model_ns > MODEL_MAX_AGE_NS or not 0 <= pair[0] - now_ns <= INPUT_MAX_AGE_NS):
|
|
frame = replace(frame, inputs_fresh=False)
|
|
if rearmed or any(type(sm.logMonoTime.get(name, 0)) is not int or sm.logMonoTime[name] <= self.source_floor_ns
|
|
for name in required):
|
|
frame = replace(frame, inputs_fresh=False)
|
|
if frame.inputs_fresh and frame.profile is not None:
|
|
try:
|
|
# Tracking precedes headway reset: the first inactive tick retains the prior headway,
|
|
# then uses zero. Active control requires this cycle's MPC headway.
|
|
detector_headway = (follow_time_s if frame.long_active else
|
|
self.previous_active_headway if self.previous_long_active and
|
|
self.previous_active_headway is not None else 0.0)
|
|
headway_fresh = (not frame.long_active or
|
|
(detector_headway is not None and type(detector_headway) in (int, float) and math.isfinite(detector_headway) and
|
|
0.5 <= detector_headway <= 3.0))
|
|
observed = self.leads.step(sm['radarState'].leadOne, sm['modelV2'], speed_mps=frame.ego_mps,
|
|
t_follow_s=detector_headway, standstill=bool(sm['carState'].standstill),
|
|
observed_mono_s=frame.model_ns / 1e9, now_mono_s=now_ns / 1e9,
|
|
radar_fresh=_fresh(sm, 'radarState', now_ns), model_fresh=_fresh(sm, 'modelV2', now_ns),
|
|
car_fresh=_fresh(sm, 'carState', now_ns), headway_fresh=headway_fresh)
|
|
if self.leads.last_observed_mono_s is None:
|
|
self.lead_samples = 0
|
|
elif observed.tracked is not None and not sm['carState'].standstill:
|
|
self.lead_samples += 1
|
|
tracked = observed.tracked
|
|
following = (observed.following if frame.long_active else
|
|
False if observed.tracked is not None else None)
|
|
if self.lead_samples < LEAD_WARMUP_FRAMES and not tracked and observed.raw_present:
|
|
tracked = following = None
|
|
frame = replace(frame, tracking_lead=tracked, following_lead=following)
|
|
self.previous_long_active = frame.long_active if observed.tracked is not None else False
|
|
self.previous_active_headway = (float(detector_headway) if detector_headway is not None and frame.long_active and headway_fresh and
|
|
observed.tracked is not None else None)
|
|
except (KeyError, AttributeError, TypeError, ValueError, OverflowError):
|
|
self._reset_leads()
|
|
return self.runtime.step(frame, event=event)
|