68 lines
2.8 KiB
Python
68 lines
2.8 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
from iqpilot.common.constants import CV
|
|
from iqpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
|
|
|
from iqdbc.car import structs
|
|
|
|
ENHANCED_STOCK_LONGITUDINAL_CONTROL_SET_SPEED_KPH_KEY = "enhancedStockLongitudinalControl.setSpeedKph"
|
|
|
|
|
|
def _float_param(key: str, value: float) -> dict[str, object]:
|
|
return {"key": key, "type": "float", "value": f"{float(value):.3f}".encode("utf-8")}
|
|
|
|
|
|
def _clamp_set_speed_kph(value: float) -> float:
|
|
return max(0.0, min(V_CRUISE_MAX, float(value)))
|
|
|
|
|
|
def build_iq_control_params_from_plan(CP: structs.CarParams, iq_plan, selfdrive_enabled: bool,
|
|
current_set_speed_kph: float, previous_sync_limit_kph: float | None,
|
|
pending_sync_limit_kph: float | None) -> tuple[list[dict[str, object]], float | None, float | None]:
|
|
if not CP.openpilotLongitudinalControl or not selfdrive_enabled:
|
|
return [], None, None
|
|
|
|
resolver = getattr(getattr(iq_plan, "speedLimit", None), "resolver", None)
|
|
assist = getattr(getattr(iq_plan, "speedLimit", None), "assist", None)
|
|
if resolver is None or assist is None:
|
|
return [], None, None
|
|
|
|
speed_limit_final_last = float(getattr(resolver, "speedLimitFinalLast", 0.0) or 0.0)
|
|
assist_enabled = bool(getattr(assist, "enabled", False))
|
|
if not assist_enabled or speed_limit_final_last <= 0.0:
|
|
return [], None, None
|
|
|
|
resolved_limit_kph = _clamp_set_speed_kph(speed_limit_final_last * CV.MS_TO_KPH)
|
|
limit_changed = previous_sync_limit_kph is None or abs(resolved_limit_kph - previous_sync_limit_kph) > 0.05
|
|
if limit_changed:
|
|
pending_sync_limit_kph = resolved_limit_kph
|
|
|
|
if pending_sync_limit_kph is not None:
|
|
if abs(current_set_speed_kph - pending_sync_limit_kph) <= 0.25:
|
|
pending_sync_limit_kph = None
|
|
set_speed_kph = _clamp_set_speed_kph(current_set_speed_kph or resolved_limit_kph)
|
|
else:
|
|
set_speed_kph = pending_sync_limit_kph
|
|
else:
|
|
set_speed_kph = _clamp_set_speed_kph(current_set_speed_kph or resolved_limit_kph)
|
|
|
|
return [_float_param(ENHANCED_STOCK_LONGITUDINAL_CONTROL_SET_SPEED_KPH_KEY, set_speed_kph)], resolved_limit_kph, pending_sync_limit_kph
|
|
|
|
|
|
def get_set_speed_kph_from_params(params) -> float | None:
|
|
for param in params:
|
|
key = param.key if hasattr(param, "key") else param.get("key")
|
|
if key != ENHANCED_STOCK_LONGITUDINAL_CONTROL_SET_SPEED_KPH_KEY:
|
|
continue
|
|
raw_value = param.value if hasattr(param, "value") else param.get("value")
|
|
try:
|
|
raw = raw_value.decode("utf-8") if isinstance(raw_value, (bytes, bytearray)) else str(raw_value)
|
|
value = float(raw)
|
|
except (AttributeError, TypeError, ValueError):
|
|
return None
|
|
return _clamp_set_speed_kph(value)
|
|
return None
|