mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-06 20:43:46 +08:00
firestar5683
51 lines
2.2 KiB
Python
51 lines
2.2 KiB
Python
"""Display the lane contribution that survives the host curvature limiter."""
|
|
|
|
import math
|
|
|
|
from openpilot.cereal import messaging
|
|
from openpilot.starpilot.lateral.lane_centering import MAX_CORRECTION
|
|
|
|
SERVICE = 'starpilotLateralState'
|
|
MAX_AGE_NS = 250_000_000
|
|
MAX_PAIR_SKEW_NS = 100_000_000
|
|
DIRECTION_EPSILON = 1e-6
|
|
BLUE = (0, 176, 220)
|
|
|
|
|
|
def feedback_message(result, applied, lateral_active, *, model_mono_time, car_control_mono_time, valid, inactive_reason='inactive'):
|
|
event = messaging.new_message(SERVICE, valid=valid)
|
|
state = event.starpilotLateralState.laneCentering
|
|
state.version = 1
|
|
state.modelMonoTime = model_mono_time
|
|
state.carControlMonoTime = car_control_mono_time
|
|
state.lateralActive = lateral_active
|
|
state.requestedCorrection = result.correction if result is not None else 0.0
|
|
state.appliedCorrection = applied if lateral_active else 0.0
|
|
state.reason = result.reason if result is not None else inactive_reason
|
|
return event
|
|
|
|
|
|
def direction(sm, now_ns: int, after_frame: int) -> int:
|
|
try:
|
|
clocks = {}
|
|
for service in (SERVICE, 'carControl', 'modelV2'):
|
|
stamp = int(sm.logMonoTime[service])
|
|
if (not sm.alive[service] or not sm.valid[service] or sm.recv_frame[service] <= after_frame or
|
|
not 0 < stamp <= now_ns or now_ns - stamp > MAX_AGE_NS):
|
|
return 0
|
|
clocks[service] = stamp
|
|
state = sm[SERVICE].laneCentering
|
|
requested, applied = float(state.requestedCorrection), float(state.appliedCorrection)
|
|
if (state.version != 1 or not state.lateralActive or not sm['carControl'].latActive or
|
|
not 0 < state.modelMonoTime <= now_ns or now_ns - state.modelMonoTime > MAX_AGE_NS or
|
|
not 0 < state.carControlMonoTime <= clocks[SERVICE] or
|
|
abs(state.carControlMonoTime - clocks['carControl']) > MAX_PAIR_SKEW_NS or
|
|
abs(state.modelMonoTime - clocks['modelV2']) > MAX_PAIR_SKEW_NS or
|
|
not math.isfinite(requested) or not math.isfinite(applied) or
|
|
abs(requested) > MAX_CORRECTION + 1e-8 or abs(applied) > abs(requested) + 1e-8 or
|
|
requested * applied < 0):
|
|
return 0
|
|
return (applied > DIRECTION_EPSILON) - (applied < -DIRECTION_EPSILON)
|
|
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
|
|
return 0
|