class TiciFanController(BaseFanController):
def __init__(self) -> None:
super().__init__()
cloudlog.info("Setting up TICI fan handler")
self.last_ignition = False
self.controller = PIDController(k_p=0,
k_i=4e-3,
k_f=1,
neg_limit=-80,
pos_limit=0,
rate=(1 / DT_TRML))
def update(self, max_cpu_temp: float, ignition: bool) -> int:
self.controller.neg_limit = -(80 if ignition else 30)
self.controller.pos_limit = -(30 if ignition else 0)
if ignition != self.last_ignition:
self.controller.reset()
error = 70 - max_cpu_temp
fan_pwr_out = -int(
self.controller.update(error=error,
feedforward=interp(
max_cpu_temp, [60.0, 100.0], [0, -80])))
self.last_ignition = ignition
return fan_pwr_out
class LatControlPID(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
k_d=(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.errors = []
def reset(self):
super().reset()
self.pid.reset()
self.errors = []
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
error = angle_steers_des - CS.steeringAngleDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = error
if CS.vEgo < MIN_STEER_SPEED or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
error_rate = 0
if len(self.errors) >= ERROR_RATE_FRAME:
error_rate = (error - self.errors[-ERROR_RATE_FRAME]) / ERROR_RATE_FRAME
self.errors.append(float(error))
while len(self.errors) > ERROR_RATE_FRAME:
self.errors.pop(0)
output_steer = self.pid.update(error, error_rate, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
return output_steer, angle_steers_des, pid_log
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 33, 55., 78]
kdBP = [i * CV.MPH_TO_MS for i in kdBP]
kdV = [0.05, 0.4, 0.8, 1.2]
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.lastdecelForTurn = False
#self.had_lead = False
self.last_output_gb = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def dynamic_gas(self, v_ego, gas_interceptor, gas_button_status):
dynamic = False
if gas_interceptor:
if gas_button_status == 0:
dynamic = True
x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2447, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.0581, 32.7101, 35.7633]
y = [0.218, 0.222, 0.233, 0.25, 0.273, 0.294, 0.337, 0.362, 0.38, 0.389, 0.398, 0.41, 0.421, 0.459, 0.512, 0.564, 0.621]
elif gas_button_status == 1:
y = [0.3, 0.9, 0.9]
elif gas_button_status == 2:
y = [0.2, 0.5, 0.7]
else:
if gas_button_status == 0:
dynamic = True
x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]
y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
elif gas_button_status == 1:
y = [0.5, 0.95, 0.99]
elif gas_button_status == 2:
y = [0.25, 0.2, 0.2]
if not dynamic:
x = [0., 9., 55.] # default BP values
accel = interp(v_ego, x, y)
#if dynamic and hasLead: # dynamic gas profile specific operations, and if lead
# if v_ego < 6.7056: # if under 15 mph
# x = [1.61479, 1.99067, 2.28537, 2.49888, 2.6312, 2.68224]
# y = [-accel, -(accel / 1.06), -(accel / 1.2), -(accel / 1.8), -(accel / 4.4), 0] # array that matches current chosen accel value
# accel += interp(vRel, x, y)
# else:
# x = [-0.89408, 0, 0.89408, 4.4704]
# y = [-.15, -.05, .005, .05]
# accel += interp(vRel, x, y)
min_return = 0.0
max_return = 1.0
return round(max(min(accel, max_return), min_return), 5) # ensure we return a value between range
def update(self, active, CS, v_target, v_target_future, a_target, CP, hasLead, radarState, decelForTurn, longitudinalPlanSource, gas_button_status):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
try:
gas_interceptor = CP.enableGasInterceptor
except AttributeError:
gas_interceptor = False
# Actuation limits
#gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
gas_max = self.dynamic_gas(CS.vEgo, gas_interceptor, gas_button_status)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
else:
dRel = radarState.leadOne.dRel
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)
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.brakePressed or CS.gasPressed 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 CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
#if not self.had_lead and has_lead:
# if enableGasInterceptor:
# self.pid._k_p = ([0., 5., 35.], [1.2, 0.8, 0.5])
# self.pid._k_i = ([0., 35.], [0.18, 0.12])
# else:
# self.pid._k_p = ([0., 5., 35.], [3.6, 2.4, 1.5])
# self.pid._k_i = ([0., 35.], [0.54, 0.36])
#elif self.had_lead and not has_lead:
# self.pid._k_p = (CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV)
# self.pid._k_i = (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV)
#self.had_lead = has_lead
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)
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], [3.0,2.1,1.5,1.0,0.6,0.29,0.0])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE * factor
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:
factor = 1
if hasLead:
factor = interp(dRel,[0.0,2.0,4.0,6.0], [0.0,0.5,1.0,2.0])
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE * factor
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 final_gas, final_brake
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.08, 0.215, 0.51]
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kfBP, CP.longitudinalTuning.kfV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
self.stop = False
self.stop_timer = 0
def update(self, active, CS, v_target, v_target_future, a_target, CP, hasLead, radarState):
"""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
vLead = 0
else:
dRel = radarState.leadOne.dRel
vLead = radarState.leadOne.vLead
if hasLead and dRel < 5. and radarState.leadOne.status:
self.stop = True
if self.stop:
self.stop_timer = 100
elif self.stop_timer > 0:
self.stop_timer -= 1
else:
self.stop = False
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo, v_target,
output_gb, CS.standstill, self.stop, vLead)
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, leadvisible=hasLead, leaddistance=dRel, leadvel=vLead)
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., 3., 4., 5., 6., 7., 8.], [5., 2.5, 1., .5, .25, .05, .005])
if output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= (STOPPING_BRAKE_RATE * factor) / RATE
if output_gb < -.5 and CS.standstill:
output_gb += .033
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:
factor = 1.
if hasLead:
factor = interp(dRel, [0., 2., 4., 6.], [2., 2., 2., 3.])
if output_gb < 2.:
output_gb += (STARTING_BRAKE_RATE * factor) / 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 final_gas, final_brake
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.08, 0.215, 0.51]
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kfBP, CP.longitudinalTuning.kfV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.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, v_target, v_target_future, a_target, CP):
"""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
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 final_gas, final_brake
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(CP.lateralTuning.torque.kp,
CP.lateralTuning.torque.ki,
k_f=CP.lateralTuning.torque.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.kf = CP.lateralTuning.torque.kf
def reset(self):
super().reset()
self.pid.reset()
def update(self, active, CS, VM, params, last_actuators, desired_curvature,
desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralTorqueState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
output_torque = 0.0
pid_log.active = False
if not active:
self.pid.reset()
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
else:
actual_curvature = llk.angularVelocityCalibrated.value[
2] / CS.vEgo
desired_lateral_accel = desired_curvature * CS.vEgo**2
desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
actual_lateral_accel = actual_curvature * CS.vEgo**2
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
error = setpoint - measurement
pid_log.error = error
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(desired_lateral_jerk,
[-JERK_THRESHOLD, JERK_THRESHOLD],
[-self.friction, self.friction])
ff += friction_compensation / self.kf
output_torque = self.pid.update(
error,
override=CS.steeringPressed,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=CS.steeringRateLimited)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.d = self.pid.d
pid_log.f = self.pid.f
pid_log.output = -output_torque
pid_log.saturated = self._check_saturation(
self.steer_max - abs(output_torque) < 1e-3, CS)
pid_log.actualLateralAccel = actual_lateral_accel
pid_log.desiredLateralAccel = desired_lateral_accel
# TODO left is positive in this convention
return -output_torque, 0.0, pid_log
class LongControl:
def __init__(self, CP):
self.CP = CP
self.long_control_state = LongCtrlState.off # initialized to 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.v_pid = 0.0
self.last_output_accel = 0.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, long_plan, accel_limits, t_since_plan,
radar_state):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target = interp(t_since_plan, T_IDXS[:CONTROL_N],
long_plan.accels)
v_target_lower = interp(
self.CP.longitudinalActuatorDelayLowerBound + t_since_plan,
T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (
v_target_lower - v_target
) / self.CP.longitudinalActuatorDelayLowerBound - a_target
v_target_upper = interp(
self.CP.longitudinalActuatorDelayUpperBound + t_since_plan,
T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (
v_target_upper - v_target
) / self.CP.longitudinalActuatorDelayUpperBound - a_target
a_target = min(a_target_lower, a_target_upper)
v_target_future = speeds[-1]
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
# TODO: This check is not complete and needs to be enforced by MPC
a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO)
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(
self.CP, active, self.long_control_state, CS.vEgo, v_target,
v_target_future, CS.brakePressed, CS.cruiseState.standstill,
radar_state)
if self.long_control_state == LongCtrlState.off:
self.reset(CS.vEgo)
output_accel = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
# 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 self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP,
self.CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
error = self.v_pid - CS.vEgo
error_deadzone = apply_deadzone(error, deadzone)
output_accel = self.pid.update(error_deadzone,
speed=CS.vEgo,
feedforward=a_target,
freeze_integrator=freeze_integrator)
if prevent_overshoot:
output_accel = min(output_accel, 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_accel > self.CP.stopAccel:
output_accel -= self.CP.stoppingDecelRate * DT_CTRL * \
interp(output_accel, [self.CP.stopAccel, self.CP.stopAccel/2., self.CP.stopAccel/4., 0.], [0.3, 0.65, 1., 2.])
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)
self.last_output_accel = output_accel
final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return final_accel
class LatControlPID(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer,
derivative_period=0.1)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def reset(self):
super().reset()
self.pid.reset()
def update(self, active, CS, CP, VM, params, desired_curvature,
desired_curvature_rate):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(
VM.get_steer_from_curvature(-desired_curvature, CS.vEgo,
params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < MIN_STEER_SPEED or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(
angle_steers_des_no_offset, CS.vEgo)
# torque for steer rate. ~0 angle, steer rate ~= steer command.
steer_rate_actual = CS.steeringRateDeg
steer_rate_desired = math.degrees(
VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo,
0))
speed_mph = CS.vEgo * CV.MS_TO_MPH
steer_rate_max = 0.0389837 * speed_mph**2 - 5.34858 * speed_mph + 223.831
steer_feedforward += ((steer_rate_desired - steer_rate_actual) /
steer_rate_max)
deadzone = 0.0
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(angle_steers_des,
CS.steeringAngleDeg,
check_saturation=check_saturation,
override=CS.steeringPressed,
feedforward=steer_feedforward,
speed=CS.vEgo,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(
steers_max - abs(output_steer) < 1e-3, CS)
return output_steer, angle_steers_des, pid_log
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.CP = CP
self.pid = PIDController(CP.lateralTuning.torque.kp,
CP.lateralTuning.torque.ki,
k_d=CP.lateralTuning.torque.kd,
k_f=CP.lateralTuning.torque.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.deadzone = CP.lateralTuning.torque.deadzone
self.errors = []
self.tune = nTune(CP, self)
def reset(self):
super().reset()
self.pid.reset()
self.errors = []
def update(self, active, CS, VM, params, last_actuators, desired_curvature,
desired_curvature_rate, llk):
self.tune.check()
pid_log = log.ControlsState.LateralTorqueState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
output_torque = 0.0
pid_log.active = False
self.pid.reset()
angle_steers_des = 0.0
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
else:
actual_curvature = llk.angularVelocityCalibrated.value[
2] / CS.vEgo
desired_lateral_accel = desired_curvature * CS.vEgo**2
desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
actual_lateral_accel = actual_curvature * CS.vEgo**2
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
error = setpoint - measurement
error_rate = 0
if len(self.errors) >= ERROR_RATE_FRAME:
error_rate = (
error - self.errors[-ERROR_RATE_FRAME]) / ERROR_RATE_FRAME
self.errors.append(float(error))
while len(self.errors) > ERROR_RATE_FRAME:
self.errors.pop(0)
error_deadzone = apply_deadzone(error, self.deadzone)
pid_log.error = error_deadzone
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(desired_lateral_jerk,
[-JERK_THRESHOLD, JERK_THRESHOLD],
[-self.friction, self.friction])
ff += friction_compensation / self.CP.lateralTuning.torque.kf
output_torque = self.pid.update(
error_deadzone,
error_rate,
override=CS.steeringPressed,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=CS.steeringRateLimited)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.d = self.pid.d
pid_log.f = self.pid.f
pid_log.output = -output_torque
pid_log.saturated = self._check_saturation(
self.steer_max - abs(output_torque) < 1e-3, CS)
angle_steers_des = math.degrees(
VM.get_steer_from_curvature(
-desired_curvature, CS.vEgo,
params.roll)) + params.angleOffsetDeg
# TODO left is positive in this convention
return -output_torque, angle_steers_des, pid_log