carrot/selfdrive/controls/lib/longcontrol.py

152 lines
6.2 KiB
Python
Raw Normal View History

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
2024-09-03 16:09:00 +09:00
from openpilot.common.params import Params
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
LongCtrlState = car.CarControl.Actuators.LongControlState
def long_control_state_trans(CP, active, long_control_state, v_ego,
should_stop, brake_pressed, cruise_standstill, a_ego, stopping_accel, radarState):
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:
2024-09-03 16:09:00 +09:00
stopping_accel = stopping_accel if stopping_accel < 0.0 else -0.5
leadOne = radarState.leadOne
fcw_stop = leadOne.status and leadOne.dRel < 4.0
if a_ego > stopping_accel or fcw_stop: # and v_ego < 1.0:
2024-09-03 16:09:00 +09:00
long_control_state = LongCtrlState.stopping
if long_control_state == LongCtrlState.starting:
long_control_state = LongCtrlState.stopping
elif started_condition:
long_control_state = LongCtrlState.pid
return long_control_state
class LongControl:
def __init__(self, CP):
self.CP = CP
self.long_control_state = LongCtrlState.off
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
self.last_output_accel = 0.0
2024-09-03 16:09:00 +09:00
self.params = Params()
self.readParamCount = 0
self.stopping_accel = 0
self.j_lead = 0.0
2024-09-03 16:09:00 +09:00
def reset(self):
self.pid.reset()
def update(self, active, CS, long_plan, accel_limits, t_since_plan, radarState):
2024-09-03 16:09:00 +09:00
soft_hold_active = CS.softHoldActive > 0
a_target = long_plan.aTarget
should_stop = long_plan.shouldStop
velocity_pid = self.params.get_float("LongVelocityControl")
long_delay = self.params.get_float("LongActuatorDelay")*0.01 + t_since_plan
j_lead_factor = (1 - self.params.get_float("JLeadFactor2") * 0.01) * 2.0 # 최대 2 sec 미래를 봄.. # JLeadFactor2가 0에 가까우면 더 민감함. 100이면 안함.
if j_lead_factor > 0.0 and radarState.leadOne.status and velocity_pid == 0:
j_lead = np.clip(radarState.leadOne.jLead, -2.0, 2.0)
self.j_lead = j_lead * 0.1 + self.j_lead * 0.9
plan_alpha = np.interp(abs(self.j_lead), [0.0, 2.0], [0.0, j_lead_factor])
else:
self.j_lead = 0.0
plan_alpha = 0.0
2024-09-03 16:09:00 +09:00
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
j_target_now = long_plan.jerks[0] #np.interp(long_delay, ModelConstants.T_IDXS[:CONTROL_N], long_plan.jerks)
if j_target_now < 0.2 and self.j_lead > 0.0:
plan_alpha = 0.0
v_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.speeds)
a_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
2024-09-03 16:09:00 +09:00
else:
v_target_now = a_target_now = j_target_now = 0.0
self.readParamCount += 1
if self.readParamCount >= 100:
self.readParamCount = 0
self.stopping_accel = self.params.get_float("StoppingAccel") * 0.01
elif self.readParamCount == 10:
if len(self.CP.longitudinalTuning.kpBP) == 1 and len(self.CP.longitudinalTuning.kiBP)==1:
longitudinalTuningKpV = self.params.get_float("LongTuningKpV") * 0.01
longitudinalTuningKiV = self.params.get_float("LongTuningKiV") * 0.001
self.pid._k_p = (self.CP.longitudinalTuning.kpBP, [longitudinalTuningKpV])
self.pid._k_i = (self.CP.longitudinalTuning.kiBP, [longitudinalTuningKiV])
self.pid._k_f = self.params.get_float("LongTuningKf") * 0.01
"""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.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
should_stop, CS.brakePressed,
CS.cruiseState.standstill, CS.aEgo, self.stopping_accel, radarState)
2024-09-03 16:09:00 +09:00
if active and soft_hold_active:
self.long_control_state = LongCtrlState.stopping
if self.long_control_state == LongCtrlState.off:
self.reset()
output_accel = 0.
elif self.long_control_state == LongCtrlState.stopping:
output_accel = self.last_output_accel
2024-09-03 16:09:00 +09:00
if soft_hold_active:
output_accel = self.CP.stopAccel
stopAccel = self.stopping_accel if self.stopping_accel < 0.0 else self.CP.stopAccel
if output_accel > stopAccel:
output_accel = min(output_accel, 0.0)
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
self.reset()
elif self.long_control_state == LongCtrlState.starting:
output_accel = self.CP.startAccel
self.reset()
else: # LongCtrlState.pid
if velocity_pid == 0:
error = a_target_now - CS.aEgo
else:
error = v_target_now - CS.vEgo
output_accel = self.pid.update(error, speed=CS.vEgo,
feedforward=a_target_now)
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
2024-09-03 16:09:00 +09:00
return self.last_output_accel, a_target_now, j_target_now