def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = LongPIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.long_stat = ""
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.05, 1.0285 * 1.45, 1.8975 * 0.8]
self.pid = LongPIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.3, 1.7, 1.5]
self.pid = LongPIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.lastdecelForTurn = False
self.last_output_gb = 0.0
self.long_stat = ""
self.op_params = opParams()
self.enable_dg = self.op_params.get('dynamic_gas')
self.dynamic_gas = DynamicGas(CP, candidate)
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.05, 0.2, 0.5]
self.pid = LongPIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.rst = 0
self.count = 0
self.accel_limiter = 0
self.op_params = opParams()
self.dynamic_gas = DynamicGas(CP, candidate)
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
if CP.enableGasInterceptor:
kdV = [0.05, 1.0285 * 1.45, 1.8975 * 0.8]
else:
kdV = [0.08, 1.215, 2.51]
self.pid = LongPIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.op_params = opParams()
self.enable_dg = self.op_params.get('dynamic_gas')
self.dynamic_gas = DynamicGas(CP, candidate)
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = LongPIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV),
(CP.longitudinalTuning.kfBP, CP.longitudinalTuning.kfV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.long_stat = ""
self.long_plan_source = ""
self.candidate = candidate
self.long_log = Params().get_bool("LongLogDisplay")
self.vRel_prev = 0
self.decel_damping = 1.0
self.decel_damping2 = 1.0
self.damping_timer = 0
class LongControl():
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.3, 1.7, 1.5]
self.pid = LongPIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.lastdecelForTurn = False
self.last_output_gb = 0.0
self.long_stat = ""
self.op_params = opParams()
self.enable_dg = self.op_params.get('dynamic_gas')
self.dynamic_gas = DynamicGas(CP, candidate)
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, v_target, v_target_future, a_target, CP,
hasLead, radarState, decelForTurn, longitudinalPlanSource,
extras):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
multiplier = 0
multiplier2 = 0
multiplier3 = 0
vRel = 0
if self.enable_dg:
gas_max = self.dynamic_gas.update(CS, extras)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
vRel = 0
else:
dRel = radarState.leadOne.dRel
vRel = radarState.leadOne.vRel
# 앞차와 거리가 3.5m이하일때 상태를 강제로 STOP으로 만듬
if hasLead:
stop = True if (dRel < 3.5
and radarState.leadOne.status) else False
else:
stop = False
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill,
stop, CS.gasPressed, CP.minSpeedCan)
v_ego_pid = max(
CS.vEgo, CP.minSpeedCan
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or (CS.brakePressed
or CS.gasPressed):
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
if longitudinalPlanSource == 'cruise':
if decelForTurn and not self.lastdecelForTurn:
self.lastdecelForTurn = True
self.pid._k_p = (CP.longitudinalTuning.kpBP, [
x * 0 for x in CP.longitudinalTuning.kpV
])
self.pid._k_i = (CP.longitudinalTuning.kiBP, [
x * 0 for x in CP.longitudinalTuning.kiV
])
self.pid.i = 0.0
self.pid.k_f = 1.0
self.v_pid = CS.vEgo
self.pid.reset()
if self.lastdecelForTurn and not decelForTurn:
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
CP.longitudinalTuning.kpV)
self.pid._k_i = (CP.longitudinalTuning.kiBP,
CP.longitudinalTuning.kiV)
self.pid.k_f = 1.0
self.v_pid = CS.vEgo
self.pid.reset()
else:
if self.lastdecelForTurn:
self.v_pid = CS.vEgo
self.pid.reset()
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
[x * 1 for x in CP.longitudinalTuning.kpV])
self.pid._k_i = (CP.longitudinalTuning.kiBP,
[x * 1 for x in CP.longitudinalTuning.kiV])
self.pid.k_f = 1.0
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
## 감속 보충을 위해 out_gb -값일 때 임의의 값을 더 곱해줌
#if hasLead and radarState.leadOne.status and 4 < dRel <= 55 and output_gb < 0 and vRel < 0:
# multiplier = max((self.v_pid/(max(v_target_future, 1))), 1)
# multiplier = clip(multiplier, 1.2, 3.5)
# output_gb *= multiplier
# #20m 간격 이하에서 거리보다 속도가 2배 이상인경우 조금더 감속 보충
# if dRel*2 < CS.vEgo*3.6 and dRel <= 20:
# multiplier2 = interp(dRel, [4, 20], [2, 1])
# elif dRel*1.5 < CS.vEgo*3.6 and dRel <= 20:
# multiplier2 = interp(dRel, [4, 20], [1.5, 1])
# output_gb *= multiplier3
# output_gb = clip(output_gb, -brake_max, gas_max)
## 앞차 감속시 가속하는것을 완화해줌
#elif hasLead and radarState.leadOne.status and 4 < dRel <= 55 and output_gb > 0 and vRel < 0:
# multiplier3 = interp(abs(vRel*3.6), [0, 1, 2], [1.0, 0.5, 0.0])
# output_gb *= multiplier3
# output_gb = clip(output_gb, -brake_max, gas_max)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1
if hasLead:
factor = interp(dRel, [2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0],
[6, 3.5, 1.5, 0.8, 0.5, 0.3, 0.2])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE * factor
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1
if hasLead:
factor = interp(dRel, [0.0, 2.0, 3.0, 4.0, 5.0],
[0.0, 0.5, 0.75, 1.0, 1000.0])
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / RATE * factor
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
if self.long_control_state == LongCtrlState.stopping:
self.long_stat = "STP"
elif self.long_control_state == LongCtrlState.starting:
self.long_stat = "STR"
elif self.long_control_state == LongCtrlState.pid:
self.long_stat = "PID"
elif self.long_control_state == LongCtrlState.off:
self.long_stat = "OFF"
else:
self.long_stat = "---"
str_log3 = 'LS={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} TG=P:{:05.2f}/F:{:05.2f}/m:{:.1f}/m2:{:.1f}/m3:{:.1f}/vr:{:+04.1f}'.format(
self.long_stat, final_gas, gas_max,
abs(final_brake), abs(brake_max), output_gb, abs(self.v_pid),
abs(v_target_future), multiplier, multiplier2, multiplier3, vRel)
trace1.printf2('{}'.format(str_log3))
return final_gas, final_brake
class LongControl():
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = LongPIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV),
(CP.longitudinalTuning.kfBP, CP.longitudinalTuning.kfV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.long_stat = ""
self.long_plan_source = ""
self.candidate = candidate
self.long_log = Params().get_bool("LongLogDisplay")
self.vRel_prev = 0
self.decel_damping = 1.0
self.decel_damping2 = 1.0
self.damping_timer = 0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, CP, long_plan, radarState):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .5s for now
if len(long_plan.speeds) == CONTROL_N:
v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.speeds)
v_target_future = long_plan.speeds[-1]
a_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.accels)
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
vRel = 0
else:
dRel = radarState.leadOne.dRel
vRel = radarState.leadOne.vRel
if long_plan.hasLead:
stop = True if (dRel < 4.0 and radarState.leadOne.status) else False
else:
stop = False
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_gb,
CS.brakePressed, CS.cruiseState.standstill, stop, CS.gasPressed, CP.minSpeedCan)
v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if (self.long_control_state == LongCtrlState.off or (CS.brakePressed or CS.gasPressed)) and self.candidate not in [CAR.NIRO_EV]:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
elif self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
# opkr
if self.vRel_prev != vRel and vRel <= 0 and CS.vEgo > 13. and self.damping_timer <= 0: # decel mitigation for a while
if (vRel - self.vRel_prev)*3.6 < -4:
self.damping_timer = 45
self.decel_damping2 = interp(abs((vRel - self.vRel_prev)*3.6), [0, 10], [1, 0.1])
self.vRel_prev = vRel
elif self.damping_timer > 0:
self.damping_timer -= 1
self.decel_damping = interp(self.damping_timer, [0, 45], [1, self.decel_damping2])
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
output_gb *= self.decel_damping
if prevent_overshoot or CS.brakeHold:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1
if long_plan.hasLead:
factor = interp(dRel,[2.0,4.0,5.0,6.0,7.0,8.0], [2.0,1.0,0.7,0.5,0.3,0.0])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE * factor
elif CS.cruiseState.standstill and output_gb < -BRAKE_STOPPING_TARGET:
output_gb += CP.stoppingBrakeRate / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1
if long_plan.hasLead:
factor = interp(dRel,[0.0,2.0,3.0,4.0,5.0], [0.0,0.5,1,250.0,500.0])
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / RATE * factor
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
if self.long_control_state == LongCtrlState.stopping:
self.long_stat = "STP"
elif self.long_control_state == LongCtrlState.starting:
self.long_stat = "STR"
elif self.long_control_state == LongCtrlState.pid:
self.long_stat = "PID"
elif self.long_control_state == LongCtrlState.off:
self.long_stat = "OFF"
else:
self.long_stat = "---"
if long_plan.longitudinalPlanSource == LongitudinalPlanSource.cruise:
self.long_plan_source = "cruise"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead0:
self.long_plan_source = "lead0"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead1:
self.long_plan_source = "lead1"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead2:
self.long_plan_source = "lead2"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.e2e:
self.long_plan_source = "e2e"
else:
self.long_plan_source = "---"
if CP.sccBus != 0 and self.long_log:
str_log3 = 'BUS={:1.0f}/{:1.0f} LS={:s} LP={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} G={:1.0f} GS={} TG={:04.2f}/{:+04.2f}'.format(CP.mdpsBus, CP.sccBus, self.long_stat, self.long_plan_source, final_gas, gas_max, abs(final_brake), abs(brake_max), output_gb, CS.cruiseGapSet, int(CS.gasPressed), v_target, a_target)
trace1.printf2('{}'.format(str_log3))
return final_gas, final_brake, v_target, a_target
class LongControl():
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
if CP.enableGasInterceptor:
kdV = [0.05, 1.0285 * 1.45, 1.8975 * 0.8]
else:
kdV = [0.08, 1.215, 2.51]
self.pid = LongPIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.op_params = opParams()
self.enable_dg = self.op_params.get('dynamic_gas')
self.dynamic_gas = DynamicGas(CP, candidate)
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, v_target, v_target_future, a_target, CP,
extras):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
if self.enable_dg:
gas_max = self.dynamic_gas.update(CS, extras)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill)
v_ego_pid = max(
CS.vEgo, MIN_CAN_SPEED
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return float(final_gas), float(final_brake)
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = LongPIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.long_stat = ""
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, v_target, v_target_future, a_target_raw,
a_target, CP, hasLead, radarState, longitudinalPlanSource,
extras):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
vRel = 0
else:
dRel = radarState.leadOne.dRel
vRel = radarState.leadOne.vRel
if hasLead:
stop = True if (dRel < 4.0
and radarState.leadOne.status) else False
else:
stop = False
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill,
stop, CS.gasPressed, CP.minSpeedCan)
v_ego_pid = max(
CS.vEgo, CP.minSpeedCan
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or (CS.brakePressed
or CS.gasPressed):
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
afactor = 1
vfactor = 1
dfactor = 1
dvfactor = 1
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
# added by opkr
afactor = interp(CS.vEgo, [0, 1, 2, 3, 4, 8, 12, 16, 20],
[4.5, 4.2, 3.65, 3.375, 3.1, 2.3, 2.1, 2, 2])
vfactor = interp(dRel, [1, 30, 50], [15, 7, 4])
dfactor = interp(dRel, [4, 10], [1.6, 1])
dvfactor = interp(((CS.vEgo * 3.6) / (max(3, dRel))), [1, 2, 3],
[1, 3, 5])
# if abs(output_gb) < abs(a_target_raw)/afactor and a_target_raw < 0 and dRel >= 4.2:
# output_gb = (-abs(a_target_raw)/afactor)*dfactor
if output_gb > 0 and a_target_raw < 0 and dRel >= 4.2:
output_gb = output_gb / vfactor
elif output_gb > 0 and a_target_raw > 0 and dRel >= 4.2 and (
CS.vEgo * 3.6) < 65:
output_gb = output_gb / dvfactor
if prevent_overshoot or CS.brakeHold:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1
if hasLead:
factor = interp(dRel, [2.0, 4.2, 5.0, 6.0, 7.0, 8.0],
[2.5, 1, 0.7, 0.5, 0.3, 0.0])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE * factor
elif CS.cruiseState.standstill and output_gb < -BRAKE_STOPPING_TARGET:
output_gb += CP.stoppingBrakeRate / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1
if hasLead:
factor = interp(dRel, [0.0, 2.0, 3.0, 4.2, 5.0],
[0.0, 0.5, 1, 500.0, 1500.0])
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / RATE * factor
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
if self.long_control_state == LongCtrlState.stopping:
self.long_stat = "STP"
elif self.long_control_state == LongCtrlState.starting:
self.long_stat = "STR"
elif self.long_control_state == LongCtrlState.pid:
self.long_stat = "PID"
elif self.long_control_state == LongCtrlState.off:
self.long_stat = "OFF"
else:
self.long_stat = "---"
if Params().get_bool("OpenpilotLongitudinalControl") and Params(
).get_bool("LongLogDisplay"):
str_log3 = 'MDPS={:1.0f} SCC={:1.0f} LS={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} TG={:+04.2f} G={:1.0f} GS={}'.format(
CP.mdpsBus, CP.sccBus, self.long_stat, final_gas, gas_max,
abs(final_brake), abs(brake_max), output_gb, a_target_raw,
CS.cruiseGapSet, CS.gasPressed)
trace1.printf2('{}'.format(str_log3))
return final_gas, final_brake
class LongControl():
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.05, 0.2, 0.5]
self.pid = LongPIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.rst = 0
self.count = 0
self.accel_limiter = 0
self.op_params = opParams()
self.dynamic_gas = DynamicGas(CP, candidate)
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, v_target, v_target_future, a_target, CP,
extras):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
if self.op_params.get('dynamic_gas'):
gas_max, brake_max, lead_car, dRel = self.dynamic_gas.update(
CS, extras)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill,
CP.minSpeedCan, dRel, lead_car)
v_ego_pid = max(
CS.vEgo, CP.minSpeedCan
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_gb = 0.
self.rst = 0
self.count = 0
self.accel_limiter = 0
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
gb_limit = interp(CS.vEgo, GdMAX_V, GdMAX_OUT)
#if self.accel_limiter and not lead_car:
if self.accel_limiter and output_gb > 0: # Test if this is good for lead car also
if (output_gb - self.last_output_gb) > gb_limit:
output_gb = self.last_output_gb + gb_limit
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
self.count = self.count + 1
if self.count > 200:
self.accel_limiter = 1
self.count = 0
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
#output_gb -= CP.stoppingBrakeRate / RATE
output_gb -= CP.stoppingBrakeRate / 10
#output_gb = clip(output_gb, -brake_max, gas_max) #Orig
output_gb = clip(output_gb, -.45, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
#if output_gb < -0.2: # Original
# output_gb += CP.startingBrakeRate / RATE # Original value
if output_gb < -0.05:
output_gb += CP.startingBrakeRate / 2
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return float(final_gas), float(final_brake)
