mirror of
https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
synced 2026-07-16 04:32:08 +08:00
IQ.Lvbs CI [bot]
109 lines
4.1 KiB
Python
109 lines
4.1 KiB
Python
import numpy as np
|
|
from cereal import car
|
|
from openpilot.common.realtime import DT_CTRL
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
|
from openpilot.common.pid import PIDController
|
|
from openpilot.selfdrive.modeld.constants import ModelConstants
|
|
from openpilot.iqpilot.selfdrive.controls.lib.smooth_stops import SmoothStopController
|
|
|
|
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
|
|
|
LongCtrlState = car.CarControl.Actuators.LongControlState
|
|
|
|
|
|
def long_control_state_trans(CP, CP_IQ, active, long_control_state, v_ego,
|
|
should_stop, brake_pressed, cruise_standstill):
|
|
# Gas Interceptor
|
|
cruise_standstill = cruise_standstill and not CP_IQ.enableGasInterceptor
|
|
|
|
stopping_condition = should_stop
|
|
starting_condition = (not should_stop and
|
|
not cruise_standstill and
|
|
not brake_pressed)
|
|
started_condition = v_ego > CP.vEgoStarting
|
|
|
|
if not active:
|
|
long_control_state = LongCtrlState.off
|
|
|
|
else:
|
|
if long_control_state == LongCtrlState.off:
|
|
if not starting_condition:
|
|
long_control_state = LongCtrlState.stopping
|
|
else:
|
|
if starting_condition and CP.startingState:
|
|
long_control_state = LongCtrlState.starting
|
|
else:
|
|
long_control_state = LongCtrlState.pid
|
|
|
|
elif long_control_state == LongCtrlState.stopping:
|
|
if starting_condition and CP.startingState:
|
|
long_control_state = LongCtrlState.starting
|
|
elif starting_condition:
|
|
long_control_state = LongCtrlState.pid
|
|
|
|
elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
|
|
if stopping_condition:
|
|
long_control_state = LongCtrlState.stopping
|
|
elif started_condition:
|
|
long_control_state = LongCtrlState.pid
|
|
return long_control_state
|
|
|
|
class LongControl:
|
|
def __init__(self, CP, CP_IQ):
|
|
self.CP = CP
|
|
self.CP_IQ = CP_IQ
|
|
self.long_control_state = LongCtrlState.off
|
|
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
|
|
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
|
|
rate=1 / DT_CTRL)
|
|
self.last_output_accel = 0.0
|
|
self.smooth = SmoothStopController()
|
|
|
|
def reset(self):
|
|
self.pid.reset()
|
|
|
|
def update(self, active, CS, a_target, should_stop, accel_limits, lead_distance=0.0, has_lead=False):
|
|
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
|
|
self.pid.neg_limit = accel_limits[0]
|
|
self.pid.pos_limit = accel_limits[1]
|
|
self.smooth.update()
|
|
|
|
if self.smooth.enabled and active and self.long_control_state != LongCtrlState.stopping:
|
|
stop_now = self.smooth.want_hold(should_stop, CS.vEgo, CS.standstill)
|
|
else:
|
|
stop_now = should_stop
|
|
|
|
self.long_control_state = long_control_state_trans(self.CP, self.CP_IQ, active, self.long_control_state, CS.vEgo,
|
|
stop_now, CS.brakePressed,
|
|
CS.cruiseState.standstill)
|
|
if self.long_control_state == LongCtrlState.off:
|
|
self.reset()
|
|
self.smooth.reset()
|
|
output_accel = 0.
|
|
|
|
elif self.long_control_state == LongCtrlState.stopping:
|
|
output_accel = self.last_output_accel
|
|
if output_accel > self.CP.stopAccel:
|
|
output_accel = min(output_accel, 0.0)
|
|
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
|
|
self.reset()
|
|
self.smooth.reset()
|
|
|
|
elif self.long_control_state == LongCtrlState.starting:
|
|
output_accel = self.CP.startAccel
|
|
self.reset()
|
|
self.smooth.reset()
|
|
|
|
else: # LongCtrlState.pid
|
|
if self.smooth.enabled and active and should_stop:
|
|
output_accel = self.smooth.settle(a_target, CS.vEgo, lead_distance, has_lead, self.last_output_accel)
|
|
self.reset()
|
|
else:
|
|
error = a_target - CS.aEgo
|
|
output_accel = self.pid.update(error, speed=CS.vEgo,
|
|
feedforward=a_target)
|
|
self.smooth.reset()
|
|
|
|
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
|
|
return self.last_output_accel
|