mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-14 00:13:42 +08:00
firestar5683
a1af95a500
Define host-to-Panda AOL intent, policy and wire contracts with neutral defaults before vehicle adapters register capabilities.
222 lines
9.1 KiB
Python
222 lines
9.1 KiB
Python
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
|
|
from opendbc.car.structs import car
|
|
from openpilot.cereal import log
|
|
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS
|
|
|
|
ButtonType = car.CarState.ButtonEvent.Type
|
|
GearShifter = car.CarState.GearShifter
|
|
|
|
AOL_TOGGLE = 9
|
|
PAUSE_LATERAL = 3
|
|
PAUSE_LONGITUDINAL = 4
|
|
|
|
@dataclass(frozen=True)
|
|
class AolSettings:
|
|
enabled: bool
|
|
pause_brake_mps: float
|
|
lkas_action: int
|
|
main_action: int
|
|
distance_actions: tuple[int, int, int]
|
|
cancel_actions: tuple[int, int, int]
|
|
|
|
|
|
def _button_action(params, key: str) -> int:
|
|
try:
|
|
value = int(params.get(key, return_default=True) or '0')
|
|
except (TypeError, ValueError):
|
|
return 0
|
|
return value if value in (0, PAUSE_LATERAL, PAUSE_LONGITUDINAL, AOL_TOGGLE) else 0
|
|
|
|
|
|
def read_settings(params) -> AolSettings:
|
|
# Legacy thresholds are effectively SI despite the mph label; preserve them until replaced.
|
|
# Read raw bytes so malformed overrides cannot silently restore AOL.
|
|
pause_mps = 0.0
|
|
for key in ('AolBrakePauseSpeedMps', 'PauseAOLOnBrake'):
|
|
try:
|
|
pause_mps = float(Path(params.get_param_path(key)).read_bytes())
|
|
except FileNotFoundError:
|
|
continue
|
|
except (OSError, ValueError):
|
|
pause_mps = float('nan')
|
|
break
|
|
valid_pause = 0.0 <= pause_mps <= 100.0
|
|
return AolSettings(
|
|
enabled=params.get_bool('AlwaysOnLateral') and valid_pause, pause_brake_mps=pause_mps if valid_pause else 0.0,
|
|
lkas_action=_button_action(params, 'LKASButtonControl'),
|
|
main_action=_button_action(params, 'MainCruiseButtonControl'),
|
|
distance_actions=(_button_action(params, 'DistanceButtonControl'),
|
|
_button_action(params, 'LongDistanceButtonControl'),
|
|
_button_action(params, 'VeryLongDistanceButtonControl')),
|
|
cancel_actions=(_button_action(params, 'CancelButtonControl'),
|
|
_button_action(params, 'LongCancelButtonControl'),
|
|
_button_action(params, 'VeryLongCancelButtonControl')),
|
|
)
|
|
|
|
|
|
def independent_axis_requested(settings: AolSettings) -> bool:
|
|
pause = (PAUSE_LATERAL, PAUSE_LONGITUDINAL)
|
|
return settings.enabled or any(action in pause for action in
|
|
(settings.lkas_action, settings.main_action, *settings.distance_actions))
|
|
|
|
|
|
def disarming_fault(events, CS) -> bool:
|
|
"""Temporary steering/known non-driving gears pause output, not driver intent."""
|
|
if CS.steerFaultPermanent:
|
|
return True
|
|
for event in events:
|
|
if event.immediateDisable:
|
|
return True
|
|
if event.softDisable:
|
|
if event.name == log.OnroadEvent.EventName.steerTempUnavailable:
|
|
continue
|
|
if event.name == log.OnroadEvent.EventName.wrongGear and CS.gearShifter != GearShifter.unknown:
|
|
continue
|
|
return True
|
|
return False
|
|
|
|
|
|
class AolCardIntent:
|
|
def __init__(self, settings: AolSettings, *, explicit_latch: bool = False):
|
|
self.settings = settings
|
|
self.explicit_latch = explicit_latch
|
|
self.allowed_latch = False
|
|
self.pause_lateral = False
|
|
self.pause_longitudinal = False
|
|
self._gesture_neutral_seen = False
|
|
self._distance_neutral_seen = False
|
|
self._main_held = False
|
|
self._lkas_held = False
|
|
self._fault_inhibit = False
|
|
self._last_latch_edge_ns = 0
|
|
self._native_rejection_ns = 0
|
|
self._timers = {int(ButtonType.gapAdjustCruise): 0}
|
|
self._held = {int(ButtonType.gapAdjustCruise): False}
|
|
|
|
def _perform(self, action: int) -> None:
|
|
if action == AOL_TOGGLE and self.settings.enabled and not self.explicit_latch:
|
|
self.allowed_latch = not self.allowed_latch
|
|
if self.allowed_latch:
|
|
self.pause_lateral = False
|
|
elif action == PAUSE_LATERAL:
|
|
self.pause_lateral = not self.pause_lateral
|
|
elif action == PAUSE_LONGITUDINAL:
|
|
self.pause_longitudinal = not self.pause_longitudinal
|
|
|
|
def update(self, CS, *, consumed_buttons: frozenset[int] = frozenset(), fault_active: bool | None = None,
|
|
now_ns: int = 0, native_rejection_ns: int = 0, standard_enabled: bool = False) -> None:
|
|
if self.explicit_latch and self._native_rejection_ns < native_rejection_ns <= now_ns:
|
|
self._native_rejection_ns = native_rejection_ns
|
|
if self._last_latch_edge_ns < native_rejection_ns <= now_ns:
|
|
# Native authorization was revoked. Keep neutral/held-button history so
|
|
# the next fresh physical press arms once, without replaying a held one.
|
|
self.allowed_latch = False
|
|
if self.explicit_latch and (fault_active is not None or CS.steerFaultPermanent):
|
|
self._fault_inhibit = bool(fault_active or CS.steerFaultPermanent)
|
|
if self._fault_inhibit:
|
|
self.allowed_latch = False
|
|
self._gesture_neutral_seen = False
|
|
if not CS.canValid or CS.canTimeout:
|
|
self.allowed_latch = False
|
|
self._gesture_neutral_seen = False
|
|
self._distance_neutral_seen = False
|
|
self._main_held = False
|
|
self._lkas_held = False
|
|
for button in self._held:
|
|
self._held[button] = False
|
|
self._timers[button] = 0
|
|
return
|
|
|
|
if not self.settings.enabled:
|
|
self.allowed_latch = False
|
|
if self.explicit_latch:
|
|
self._gesture_neutral_seen = False
|
|
if not independent_axis_requested(self.settings):
|
|
self.pause_lateral = False
|
|
self.pause_longitudinal = False
|
|
|
|
lkas_managed = self.settings.lkas_action == AOL_TOGGLE
|
|
main_managed = self.settings.main_action == AOL_TOGGLE
|
|
if self.explicit_latch and CS.accFaulted:
|
|
self.allowed_latch = False
|
|
self._gesture_neutral_seen = False
|
|
elif self.settings.enabled and not self.explicit_latch and not lkas_managed and not main_managed:
|
|
self.allowed_latch = bool(CS.cruiseState.available)
|
|
elif not self.explicit_latch and not CS.cruiseState.available:
|
|
self.allowed_latch = False
|
|
|
|
for button in consumed_buttons:
|
|
if button in self._held:
|
|
self._held[button] = False
|
|
self._timers[button] = 0
|
|
|
|
distance_events = [event for event in CS.buttonEvents if event.type in (ButtonType.gapAdjustCruise, ButtonType.lkas)]
|
|
if not self._distance_neutral_seen:
|
|
self._distance_neutral_seen = not any(event.pressed for event in distance_events)
|
|
|
|
if self.explicit_latch:
|
|
previously_held = self._main_held or self._lkas_held
|
|
cancel_pressed = False
|
|
for event in CS.buttonEvents:
|
|
button = int(event.type.raw)
|
|
if button == int(ButtonType.mainCruise):
|
|
self._main_held = bool(event.pressed)
|
|
elif button == int(ButtonType.lkas):
|
|
self._lkas_held = bool(event.pressed)
|
|
elif button == int(ButtonType.cancel) and event.pressed:
|
|
cancel_pressed = True
|
|
gesture_held = self._main_held or self._lkas_held
|
|
if not previously_held and gesture_held:
|
|
self._last_latch_edge_ns = now_ns
|
|
if cancel_pressed:
|
|
self.allowed_latch = False
|
|
elif not self._gesture_neutral_seen:
|
|
self._gesture_neutral_seen = not gesture_held
|
|
elif not previously_held and gesture_held and self.settings.enabled and not CS.accFaulted and not self._fault_inhibit:
|
|
# The physical gesture owns lateral intent even while ordinary cruise
|
|
# is engaged. Dropping only the AOL latch leaves standard steering on.
|
|
lateral_engaged = not self.pause_lateral and (self.allowed_latch or standard_enabled)
|
|
self.allowed_latch = not lateral_engaged
|
|
self.pause_lateral = lateral_engaged
|
|
|
|
for event in CS.buttonEvents:
|
|
button = int(event.type.raw)
|
|
if button in consumed_buttons:
|
|
continue
|
|
if self.explicit_latch and button in (int(ButtonType.cancel), int(ButtonType.lkas), int(ButtonType.mainCruise)):
|
|
continue
|
|
if button == int(ButtonType.gapAdjustCruise) and not self._distance_neutral_seen:
|
|
continue
|
|
if button == int(ButtonType.lkas) and event.pressed:
|
|
self._perform(self.settings.lkas_action)
|
|
elif button == int(ButtonType.mainCruise) and event.pressed:
|
|
self._perform(self.settings.main_action)
|
|
elif button in self._held:
|
|
if event.pressed:
|
|
self._held[button] = True
|
|
self._timers[button] = 0
|
|
else:
|
|
ticks = self._timers[button]
|
|
actions = self.settings.distance_actions if button == int(ButtonType.gapAdjustCruise) else self.settings.cancel_actions
|
|
if 0 < ticks < CRUISE_LONG_PRESS:
|
|
self._perform(actions[0])
|
|
self._held[button] = False
|
|
self._timers[button] = 0
|
|
for button, held in self._held.items():
|
|
if held:
|
|
self._timers[button] += 1
|
|
actions = self.settings.distance_actions if button == int(ButtonType.gapAdjustCruise) else self.settings.cancel_actions
|
|
if self._timers[button] == CRUISE_LONG_PRESS:
|
|
self._perform(actions[1])
|
|
elif self._timers[button] == CRUISE_LONG_PRESS * 5:
|
|
self._perform(actions[2])
|
|
|
|
def output(self, CS) -> tuple[bool, bool, bool]:
|
|
driving = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
|
|
return (self.allowed_latch and self.settings.enabled and CS.canValid and driving,
|
|
self.pause_lateral, self.pause_longitudinal)
|