mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-06 20:43:46 +08:00
firestar5683
79c8945a41
Integrate torque feedback, lane-change smoothing and low-speed Turn Assist through dedicated lateral owners. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: pharmacomaniac <blittle65@gmail.com>
91 lines
3.1 KiB
Python
91 lines
3.1 KiB
Python
"""Temporary steering output pauses, independent of driver authorization."""
|
|
|
|
from dataclasses import dataclass
|
|
import math
|
|
|
|
from openpilot.common.constants import CV
|
|
from openpilot.starpilot.saved_source import read_saved
|
|
|
|
KEYS = ('PauseLateralSpeed', 'PauseLateralOnSignal', 'LateralResumeDelay')
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class PauseSettings:
|
|
speed_mps: float = 0.0
|
|
signal_only: bool = False
|
|
delay_s: float = 0.0
|
|
|
|
|
|
def read_settings(params) -> PauseSettings | None:
|
|
values = {key: read_saved(params, key, 128) for key in (*KEYS, 'IsMetric')}
|
|
if not all(readable for _, readable in values.values()):
|
|
return None
|
|
try:
|
|
speed_raw = values[KEYS[0]][0]
|
|
speed = float(b'0' if speed_raw is None else speed_raw)
|
|
signal = values[KEYS[1]][0]
|
|
delay_raw = values[KEYS[2]][0]
|
|
delay = float(b'0' if delay_raw is None else delay_raw)
|
|
metric = values['IsMetric'][0]
|
|
if (signal not in (None, b'0', b'1') or metric not in (None, b'0', b'1') or
|
|
not math.isfinite(speed) or not 0 <= speed <= 100 or not math.isfinite(delay) or not 0 <= delay <= 5):
|
|
return None
|
|
return PauseSettings(speed * (CV.KPH_TO_MS if metric == b'1' else CV.MPH_TO_MS), signal == b'1', delay)
|
|
except (ValueError, OverflowError):
|
|
return None
|
|
|
|
|
|
class LateralPause:
|
|
def __init__(self, settings: PauseSettings | None, params=None):
|
|
self.settings = settings or PauseSettings()
|
|
self.params = params
|
|
self._refresh_ns = 0
|
|
self._drive_id = 0
|
|
self.reset()
|
|
|
|
def reset(self) -> None:
|
|
self._blinker = False
|
|
self._minimum = math.inf
|
|
self._release_ns = 0
|
|
self._last_ns = 0
|
|
|
|
def refresh(self, now_ns: int) -> None:
|
|
if self.params is not None and (now_ns < self._refresh_ns or now_ns - self._refresh_ns >= 1_000_000_000):
|
|
settings = read_settings(self.params)
|
|
if settings is not None:
|
|
if settings != self.settings:
|
|
self.reset()
|
|
self.settings = settings
|
|
self._refresh_ns = now_ns
|
|
|
|
def allowed(self, cs, *, now_ns: int, source_ns: int, drive_id: int | None = None) -> bool:
|
|
self.refresh(now_ns)
|
|
if self.settings.speed_mps == 0:
|
|
self.reset()
|
|
return True
|
|
if now_ns < self._last_ns:
|
|
self.reset()
|
|
return False
|
|
self._last_ns = now_ns
|
|
if drive_id is not None:
|
|
if drive_id <= 0:
|
|
return False
|
|
if drive_id != self._drive_id:
|
|
self.reset()
|
|
self._drive_id = drive_id
|
|
self._last_ns = now_ns
|
|
if (not cs.canValid or cs.canTimeout or not math.isfinite(cs.vEgo) or
|
|
not 0 < source_ns <= now_ns or now_ns - source_ns > 250_000_000):
|
|
return False
|
|
signal = bool(cs.leftBlinker or cs.rightBlinker)
|
|
if signal:
|
|
self._minimum = min(self._minimum, cs.vEgo) if self._blinker else cs.vEgo
|
|
self._release_ns = 0
|
|
elif self._blinker:
|
|
self._release_ns = now_ns
|
|
self._blinker = signal
|
|
delayed = (self.settings.signal_only and self._release_ns > 0 and
|
|
self._minimum < self.settings.speed_mps / 2 and
|
|
now_ns - self._release_ns < self.settings.delay_s * 1e9)
|
|
return bool((cs.vEgo >= self.settings.speed_mps or self.settings.signal_only and not signal) and not delayed)
|