144 lines
3.9 KiB
Python
144 lines
3.9 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
|
|
from iqpilot.cereal import messaging, custom
|
|
|
|
from iqpilot.common.params import Params
|
|
from iqpilot.common.realtime import DT_MDL
|
|
from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
|
|
|
|
PARAM_PATH = "EndToEndAlert"
|
|
PARAM_LEAD = "EndToEndLeadAlert"
|
|
PARAM_STRIDE_S = 2.0
|
|
|
|
SETTLE_S = 1.0
|
|
CONFIRM_S = 0.4
|
|
ROLL_MPS = 0.3
|
|
|
|
HORIZON_TAIL = 5
|
|
PATH_SPEED_MPS = 3.0
|
|
|
|
LEAD_QUEUE_M = 12.0
|
|
LEAD_SPEED_MPS = 1.0
|
|
LEAD_GAP_M = 0.5
|
|
|
|
|
|
class _Confirm:
|
|
def __init__(self, window_s: float):
|
|
self._window = window_s
|
|
self._held = 0.0
|
|
self._spent = False
|
|
|
|
def clear(self) -> None:
|
|
self._held = 0.0
|
|
self._spent = False
|
|
|
|
def poll(self, holds: bool) -> bool:
|
|
if self._spent:
|
|
return False
|
|
self._held = self._held + DT_MDL if holds else 0.0
|
|
if self._held < self._window:
|
|
return False
|
|
self._spent = True
|
|
return True
|
|
|
|
|
|
class _Dwell:
|
|
def __init__(self):
|
|
self.seconds = 0.0
|
|
self.lead_floor = float('inf')
|
|
|
|
def clear(self) -> None:
|
|
self.seconds = 0.0
|
|
self.lead_floor = float('inf')
|
|
|
|
def tick(self, lead_range: float | None) -> None:
|
|
self.seconds += DT_MDL
|
|
if lead_range is not None:
|
|
self.lead_floor = min(self.lead_floor, lead_range)
|
|
|
|
@property
|
|
def settled(self) -> bool:
|
|
return self.seconds >= SETTLE_S
|
|
|
|
@property
|
|
def queued(self) -> bool:
|
|
return self.lead_floor < LEAD_QUEUE_M
|
|
|
|
|
|
class EndToEndAlertEngine:
|
|
def __init__(self):
|
|
self._params = Params()
|
|
self._on = {"path": False, "lead": False}
|
|
self._elapsed_since_read = PARAM_STRIDE_S
|
|
self._dwell = _Dwell()
|
|
self._confirm = {"path": _Confirm(CONFIRM_S), "lead": _Confirm(CONFIRM_S)}
|
|
self._fired = {"path": False, "lead": False}
|
|
|
|
def _refresh_params(self) -> None:
|
|
self._elapsed_since_read += DT_MDL
|
|
if self._elapsed_since_read < PARAM_STRIDE_S:
|
|
return
|
|
self._elapsed_since_read = 0.0
|
|
self._on["path"] = self._params.get_bool(PARAM_PATH)
|
|
self._on["lead"] = self._params.get_bool(PARAM_LEAD)
|
|
|
|
@staticmethod
|
|
def _car_holds_long(sm: messaging.SubMaster) -> bool:
|
|
# AOL steers without raising selfdriveState.enabled, so the pair reads as long authority
|
|
return bool(sm['selfdriveState'].enabled or sm['carState'].cruiseState.enabled)
|
|
|
|
@staticmethod
|
|
def _halted(cs) -> bool:
|
|
return bool(cs.standstill) or abs(cs.vEgo) < ROLL_MPS
|
|
|
|
@staticmethod
|
|
def _horizon_speed(model) -> float:
|
|
# capnp list readers reject slices
|
|
samples = model.velocity.x
|
|
count = len(samples)
|
|
if count < HORIZON_TAIL:
|
|
return 0.0
|
|
return sum(samples[i] for i in range(count - HORIZON_TAIL, count)) / HORIZON_TAIL
|
|
|
|
def _rearm(self) -> None:
|
|
self._dwell.clear()
|
|
for gate in self._confirm.values():
|
|
gate.clear()
|
|
|
|
def update(self, sm: messaging.SubMaster, iq_events: IQEvents) -> None:
|
|
self._refresh_params()
|
|
self._fired["path"] = self._fired["lead"] = False
|
|
|
|
cs = sm['carState']
|
|
lead = sm['radarState'].leadOne
|
|
lead_range = float(lead.dRel) if lead.status else None
|
|
|
|
if not self._halted(cs) or cs.gasPressed or self._car_holds_long(sm):
|
|
self._rearm()
|
|
return
|
|
|
|
self._dwell.tick(lead_range)
|
|
if not self._dwell.settled:
|
|
return
|
|
|
|
if self._on["path"] and lead_range is None:
|
|
opened = self._horizon_speed(sm['modelV2']) > PATH_SPEED_MPS
|
|
self._fired["path"] = self._confirm["path"].poll(opened)
|
|
|
|
if self._on["lead"] and lead_range is not None and self._dwell.queued:
|
|
pulling = lead.vLead > LEAD_SPEED_MPS and (lead_range - self._dwell.lead_floor) > LEAD_GAP_M
|
|
self._fired["lead"] = self._confirm["lead"].poll(pulling)
|
|
|
|
if self._fired["path"] or self._fired["lead"]:
|
|
iq_events.add(custom.IQOnroadEvent.EventName.e2eChime)
|
|
|
|
@property
|
|
def path_alert(self) -> bool:
|
|
return self._fired["path"]
|
|
|
|
@property
|
|
def lead_alert(self) -> bool:
|
|
return self._fired["lead"]
|