def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.dynamic_gas = DynamicGas(CP)
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.05, 0.935, 1.65]
self.pid = PIDController(
(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', True)
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 = PIController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.dynamic_gas = DynamicGas(CP, candidate)
self.gas_pressed = False
self.lead_data = {
'v_rel': None,
'a_lead': None,
'x_lead': None,
'status': False
}
self.track_data = []
self.mpc_TR = 1.8
self.blinker_status = False
self.dynamic_lane_speed = DynamicLaneSpeed()
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
CP.longitudinalTuning.kf,
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.dynamic_gas = DynamicGas(CP)
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, sm):
"""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)
gas_max = self.dynamic_gas.update(CS, sm)
# 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)
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.
# 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 -= 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:
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / 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 final_gas, final_brake
class LongControl():
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.dynamic_gas = DynamicGas(CP, candidate)
self.gas_pressed = False
self.lead_data = {
'v_rel': None,
'a_lead': None,
'x_lead': None,
'status': False
}
self.track_data = []
self.mpc_TR = 1.8
self.blinker_status = False
self.dynamic_lane_speed = DynamicLaneSpeed()
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def handle_passable(self, passable, v_ego):
CS = passable['car_state']
self.blinker_status = CS.leftBlinker or CS.rightBlinker
self.gas_pressed = CS.gasPressed
self.lead_data['v_rel'] = passable['lead_one'].vRel
self.lead_data['a_lead'] = passable['lead_one'].aLeadK
self.lead_data['x_lead'] = passable['lead_one'].dRel
self.lead_data['status'] = passable[
'has_lead'] # this fixes radarstate always reporting a lead, thanks to arne
self.mpc_TR = passable['mpc_TR']
self.track_data = []
for track in passable['live_tracks']:
self.track_data.append({
'v_lead': v_ego + track.vRel,
'y_rel': track.yRel,
'x_lead': track.dRel
})
def update(self, active, v_ego, brake_pressed, standstill,
cruise_standstill, v_cruise, v_target, v_target_future,
a_target, CP, passable):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
if not travis:
self.handle_passable(passable, v_ego)
gas_max = self.dynamic_gas.update(v_ego, self.lead_data,
self.mpc_TR, self.blinker_status)
# v_target, v_target_future, a_target = self.dynamic_lane_speed.update(v_target, v_target_future, v_cruise, a_target, v_ego, self.track_data, self.lead_data)
else:
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(
active, self.long_control_state, v_ego, v_target_future,
self.v_pid, output_gb, brake_pressed, cruise_standstill)
v_ego_pid = max(
v_ego, MIN_CAN_SPEED
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or (self.gas_pressed
and not travis):
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 v_ego < 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 standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.pid.reset()
# 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.v_pid = v_ego
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
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.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
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.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)
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.v_pid = CS.vEgo
self.pid.reset()
# 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.v_pid = CS.vEgo
self.pid.reset()
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, 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)