mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-06 20:43:46 +08:00
firestar5683
e4f3d6661c
Implement acceleration profiles, FollowJerk, force-stop and lead-departure owners. Keep selected acceleration limits and hazard arbitration explicit at their shared boundaries. Co-authored-by: whoisdomi <dcantu83@gmail.com>
54 lines
2.0 KiB
Python
54 lines
2.0 KiB
Python
"""Source-timed Ioniq longitudinal throttle/coast comfort gate."""
|
|
|
|
import math
|
|
|
|
DISABLE_PROBABILITY = 0.35
|
|
ENABLE_PROBABILITY = 0.45
|
|
CONFIRM_NS = 250_000_000
|
|
MAX_MODEL_AGE_NS = 250_000_000
|
|
MAX_MODEL_GAP_NS = 150_000_000
|
|
|
|
|
|
class ModelThrottleGate:
|
|
def __init__(self):
|
|
self.allowed = True
|
|
self.pending_since_ns: int | None = None
|
|
self.last_model_ns: int | None = None
|
|
|
|
def reset(self) -> None:
|
|
self.allowed = True
|
|
self.pending_since_ns = None
|
|
self.last_model_ns = None
|
|
|
|
def step(self, probability: float, speed_mps: float, *, low_speed_mps: float,
|
|
model_ns: int, now_ns: int, model_valid: bool) -> bool:
|
|
previous_ns = self.last_model_ns
|
|
if (model_valid is True and type(model_ns) is int and model_ns > 0 and
|
|
type(now_ns) is int and model_ns <= now_ns and now_ns - model_ns <= MAX_MODEL_AGE_NS and
|
|
(previous_ns is None or model_ns > previous_ns)):
|
|
self.last_model_ns = model_ns
|
|
if (model_valid is not True or type(model_ns) is not int or model_ns <= 0 or
|
|
type(now_ns) is not int or now_ns <= 0 or model_ns > now_ns or now_ns - model_ns > MAX_MODEL_AGE_NS or
|
|
(previous_ns is not None and not 0 < model_ns - previous_ns <= MAX_MODEL_GAP_NS) or
|
|
type(probability) not in (int, float) or not math.isfinite(probability) or not 0.0 <= probability <= 1.0 or
|
|
type(speed_mps) not in (int, float) or not math.isfinite(speed_mps)):
|
|
self.allowed = False
|
|
self.pending_since_ns = None
|
|
return False
|
|
|
|
if speed_mps <= low_speed_mps:
|
|
self.allowed = True
|
|
self.pending_since_ns = None
|
|
return True
|
|
|
|
crossing = probability <= DISABLE_PROBABILITY if self.allowed else probability > ENABLE_PROBABILITY
|
|
if crossing:
|
|
if self.pending_since_ns is None:
|
|
self.pending_since_ns = model_ns
|
|
elif model_ns - self.pending_since_ns >= CONFIRM_NS:
|
|
self.allowed = not self.allowed
|
|
self.pending_since_ns = None
|
|
else:
|
|
self.pending_since_ns = None
|
|
return self.allowed
|