J3ff/IQ.Pilot
1
0
Fork 0
forked from IQ.Lvbs/IQ.Pilot
Code Pull Requests Activity

IQ.Pilot Release Commit @ 0798119

Browse Source
This commit is contained in:
IQ.Lvbs history cleanup
2026-08-22 23:42:42 -05:00
commit b42569dbca
4529 changed files with 1132125 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

249
iqpilot/selfdrive/controls/lib/iq_dynamic/engine.py Normal file
View File

@@ -0,0 +1,249 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from cereal import messaging
from numpy import interp
from iqdbc.car import structs
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import (
IQConstants,
IQFilterEngine,
IQModeEngine,
IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM,
IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM,
IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM,
IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM,
IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM,
IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM,
IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM,
IQ_DYNAMIC_MODE_PARAM,
IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM,
IQ_DYNAMIC_MODEL_STOP_TIME_PARAM,
IQ_FORCE_STOPS_PARAM,
compute_slowdown_need,
)
S_Y = 33
class IQDynamicController:
def __init__(self, CP: structs.CarParams, mpc, params=None):
self.IQS = CP
self._mpc = mpc
self.IQParams = params or Params()
self.IQDynamicStatus = False
self.IQDynamicA = False
self.IQDynamicF = 0
self.IQDynamicU = 0.0
self.IQEngineManager = IQModeEngine()
self.IQFilterL = IQFilterEngine(measurement_noise=0.17, process_noise=0.04, process_decay=1.03, smoothing_floor=0.9)
self.IQFilterSDL = IQFilterEngine(measurement_noise=0.12, process_noise=0.098, process_decay=1.01, smoothing_floor=0.8)
self.IQFilterSFL = IQFilterEngine(measurement_noise=0.11, process_noise=0.06, process_decay=1.000, smoothing_floor=0.90)
self.IQFilterFCW = IQFilterEngine(measurement_noise=0.19, process_noise=0.11, process_decay=1.11, smoothing_floor=0.4)
self.IQFilterSlowLead = IQFilterEngine(measurement_noise=0.15, process_noise=0.08, process_decay=1.02, smoothing_floor=0.75)
self.IQFilterModelStop = IQFilterEngine(measurement_noise=0.15, process_noise=0.06, process_decay=1.01, smoothing_floor=0.7)
self.hasIQFilterLED = False
self.hasIQSDL = False
self.hasIQSFL = False
self.hasIQL = False
self.curve_detected = False
self.slow_lead_detected = False
self.stop_light_detected = False
self.low_speed_detected = False
self.low_speed_lead_detected = False
self.model_stopped = False
self.tracking_lead = False
self.force_stops_enabled = True
self.slc_experimental_mode = False
self.kph = 0.0
self.cruise_kph = 0.0
self.aeb = 0
self.aeb_c = 0
self.ss_c = 0
self.e_x = float('inf')
self.e_d = 0.0
self.model_length = 0.0
self.lead_speed = 0.0
self.conditional_curves = True
self.conditional_slower_lead = True
self.conditional_stopped_lead = True
self.conditional_model_stops = True
self.conditional_slc_fallback = True
self.conditional_speed = IQConstants.CONDITIONAL_SPEED_DEFAULT
self.conditional_lead_speed = IQConstants.CONDITIONAL_LEAD_SPEED_DEFAULT
self.model_stop_time = IQConstants.MODEL_STOP_TIME_DEFAULT
self.minimum_force_stop_length = IQConstants.MINIMUM_FORCE_STOP_LENGTH_DEFAULT
def _read_bool(self, key: str, default: bool) -> bool:
value = self.IQParams.get_bool(key)
return default if value is None else bool(value)
def _read_float(self, key: str, default: float) -> float:
value = self.IQParams.get(key)
if value is None:
return default
if isinstance(value, bytes):
value = value.decode('utf-8')
try:
return float(value)
except (TypeError, ValueError):
return default
def _readIQParams(self) -> None:
if self.IQDynamicF % int(1. / DT_MDL) != 0:
return
self.IQDynamicStatus = self._read_bool(IQ_DYNAMIC_MODE_PARAM, False)
self.conditional_curves = self._read_bool(IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM, True)
self.conditional_slower_lead = self._read_bool(IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM, True)
self.conditional_stopped_lead = self._read_bool(IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM, True)
self.conditional_model_stops = self._read_bool(IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM, True)
self.conditional_slc_fallback = self._read_bool(IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM, True)
self.conditional_speed = self._read_float(IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM, IQConstants.CONDITIONAL_SPEED_DEFAULT)
self.conditional_lead_speed = self._read_float(IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM, IQConstants.CONDITIONAL_LEAD_SPEED_DEFAULT)
self.model_stop_time = self._read_float(IQ_DYNAMIC_MODEL_STOP_TIME_PARAM, IQConstants.MODEL_STOP_TIME_DEFAULT)
self.minimum_force_stop_length = self._read_float(IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM, IQConstants.MINIMUM_FORCE_STOP_LENGTH_DEFAULT)
self.force_stops_enabled = self._read_bool(IQ_FORCE_STOPS_PARAM, True)
def set_slc_experimental_mode(self, active: bool) -> None:
self.slc_experimental_mode = bool(active)
def mode(self) -> str:
return self.IQEngineManager.get_mode()
def enabled(self) -> bool:
return self.IQDynamicStatus
def active(self) -> bool:
return self.IQDynamicA
def force_stop_requested(self) -> bool:
return bool(self.force_stops_enabled and self.stop_light_detected and self.model_stopped and not self.tracking_lead)
def setaeb(self) -> None:
self.aeb = self.aeb_c
def IQDynamicEngine(self, sm: messaging.SubMaster) -> None:
car_state = sm['carState']
radar_state = sm['radarState']
model = sm['modelV2']
self.kph = car_state.vEgo * 3.6
self.cruise_kph = car_state.vCruise
self.ss_c = min(20, self.ss_c + 1) if car_state.standstill else max(0, self.ss_c - 1)
lead_status = float(getattr(radar_state.leadOne, "status", False))
self.IQFilterL.push(lead_status)
self.hasIQFilterLED = (self.IQFilterL.value() or 0.0) > IQConstants.LEAD_LOCK_GATE
self.tracking_lead = self.hasIQFilterLED
self.lead_speed = float(getattr(radar_state.leadOne, "vLead", 0.0))
prev_fcw = self.IQFilterFCW.value() or 0.0
self.IQFilterFCW.push(float(self.aeb > 0))
self.hasIQL = prev_fcw > 0.5
valid_model = len(model.position.x) == S_Y and len(model.orientation.x) == S_Y
if valid_model:
self.model_length = float(model.position.x[S_Y - 1])
self.e_x = self.model_length
self.e_d = interp(self.kph, IQConstants.BRAKE_CURVE_SPEED_AXIS, IQConstants.BRAKE_CURVE_DISTANCE_AXIS)
need = compute_slowdown_need(self.kph, self.model_length, self.e_d)
else:
self.model_length = 0.0
self.e_x = float('inf')
self.e_d = 0.0
need = 0.3 if self.kph > 20.0 else 0.0
self.IQFilterSDL.push(need)
self.IQDynamicU = self.IQFilterSDL.value() or 0.0
self.hasIQSDL = self.IQDynamicU > (IQConstants.BRAKE_CURVE_GATE * 0.8)
self.curve_detected = self.hasIQSDL
if self.ss_c <= 5 and not self.hasIQSDL:
slowness_observed = float(self.kph <= (self.cruise_kph * IQConstants.CRUISE_LAG_RATIO_GATE))
self.IQFilterSFL.push(slowness_observed)
threshold = IQConstants.CRUISE_LAG_GATE * (0.8 if self.hasIQSFL else 1.1)
self.hasIQSFL = (self.IQFilterSFL.value() or 0.0) > threshold
v_ego = float(car_state.vEgo)
self.low_speed_detected = not self.tracking_lead and IQConstants.CRUISING_SPEED <= v_ego < self.conditional_speed
self.low_speed_lead_detected = self.tracking_lead and IQConstants.CRUISING_SPEED <= v_ego < self.conditional_lead_speed
if self.tracking_lead:
slower_lead = (v_ego - self.lead_speed) > IQConstants.CRUISING_SPEED and self.conditional_slower_lead
stopped_lead = self.lead_speed < 1.0 and self.conditional_stopped_lead
self.IQFilterSlowLead.push(float(slower_lead or stopped_lead))
self.slow_lead_detected = (self.IQFilterSlowLead.value() or 0.0) >= IQConstants.SLOW_LEAD_THRESHOLD
else:
self.IQFilterSlowLead.reset()
self.slow_lead_detected = False
should_stop = bool(getattr(getattr(model, "action", None), "shouldStop", False))
model_stopping = self.model_length > 0.0 and self.model_length < max(v_ego * self.model_stop_time, IQConstants.CRUISING_SPEED)
self.model_stopped = bool(should_stop or model_stopping)
self.IQFilterModelStop.push(float(self.model_stopped and not self.tracking_lead))
self.stop_light_detected = (self.IQFilterModelStop.value() or 0.0) >= IQConstants.MODEL_STOP_THRESHOLD
def _request_blended(self, urgency: float = 1.0, emergency: bool = False) -> None:
self.IQEngineManager.request('blended', urgency=urgency, emergency=emergency)
def _request_acc(self, urgency: float = 0.8) -> None:
self.IQEngineManager.request('acc', urgency=urgency)
def IQStateEngine(self) -> None:
if self.hasIQL:
self._request_blended(1.0, True)
elif self.stop_light_detected and self.conditional_model_stops:
self._request_blended(1.0, self.model_stopped)
elif self.low_speed_detected or self.low_speed_lead_detected:
self._request_blended(0.95)
elif self.slow_lead_detected:
self._request_blended(0.9)
elif self.conditional_curves and self.hasIQSDL:
self._request_blended(max(0.8, min(1.0, self.IQDynamicU * 1.5)))
elif self.conditional_slc_fallback and self.slc_experimental_mode:
self._request_blended(0.8)
elif self.ss_c > 3:
self._request_blended(0.9)
elif self.hasIQSFL and not self.hasIQSDL:
self._request_acc(0.8)
else:
self._request_acc(0.7)
def IQStateEngine_R(self) -> None:
if self.hasIQL:
self._request_blended(1.0, True)
elif self.stop_light_detected and self.conditional_model_stops:
self._request_blended(1.0, self.model_stopped)
elif self.low_speed_detected or self.low_speed_lead_detected:
self._request_blended(0.95)
elif self.slow_lead_detected:
self._request_blended(0.9)
elif self.conditional_curves and self.hasIQSDL:
self._request_blended(max(0.8, min(1.0, self.IQDynamicU * 1.3)))
elif self.conditional_slc_fallback and self.slc_experimental_mode:
self._request_blended(0.8)
elif self.hasIQFilterLED and not (self.ss_c > 3):
self._request_acc(1.0)
elif self.ss_c > 3:
self._request_blended(0.9)
elif self.hasIQSFL and not self.hasIQSDL:
self._request_acc(0.8)
else:
self._request_acc(0.7)
def update(self, sm: messaging.SubMaster) -> None:
self._readIQParams()
self.setaeb()
self.IQDynamicEngine(sm)
if self.IQS.radarUnavailable:
self.IQStateEngine()
else:
self.IQStateEngine_R()
self.IQEngineManager.update()
self.IQDynamicA = sm['selfdriveState'].experimentalMode and self.IQDynamicStatus
self.IQDynamicF += 1