mirror of
https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
synced 2026-07-25 05:22:11 +08:00
IQ.Lvbs CI [bot]
250 lines
10 KiB
Python
250 lines
10 KiB
Python
"""
|
|
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
|