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 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
self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg
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
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
curvature_deadzone = abs(
VM.calc_curvature(
math.radians(self.steering_angle_deadzone_deg),
CS.vEgo, 0.0))
else:
actual_curvature_vm = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
actual_curvature_llk = llk.angularVelocityCalibrated.value[
2] / CS.vEgo
actual_curvature = interp(
CS.vEgo, [2.0, 5.0],
[actual_curvature_vm, actual_curvature_llk])
curvature_deadzone = 0.0
desired_lateral_accel = desired_curvature * CS.vEgo**2
# desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
#desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo**2
lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2
low_speed_factor = interp(CS.vEgo, [0, 15], [500, 0])
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
error = apply_deadzone(setpoint - measurement,
lateral_accel_deadzone)
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(
error, [-FRICTION_THRESHOLD, FRICTION_THRESHOLD],
[-self.friction, self.friction])
ff += friction_compensation / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(
error,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
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, 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 CarController():
def __init__(self, dbc_name, CP, VM):
self.frame = 0
self.packer = CANPacker(dbc_name)
# Speed, balance and turn PIDs
self.speed_pid = PIDController(0.115, k_i=0.23, rate=1 / DT_CTRL)
self.balance_pid = PIDController(1300,
k_i=0,
k_d=280,
rate=1 / DT_CTRL)
self.turn_pid = PIDController(110, k_i=11.5, rate=1 / DT_CTRL)
self.torque_r_filtered = 0.
self.torque_l_filtered = 0.
@staticmethod
def deadband_filter(torque, deadband):
if torque > 0:
torque += deadband
else:
torque -= deadband
return torque
def update(self, CC, CS):
torque_l = 0
torque_r = 0
llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0
if CC.enabled and llk_valid:
# Read these from the joystick
# TODO: this isn't acceleration, okay?
speed_desired = CC.actuators.accel / 5.
speed_diff_desired = -CC.actuators.steer
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl +
CS.out.wheelSpeeds.fr) / 2.
speed_error = speed_desired - speed_measured
freeze_integrator = (
(speed_error < 0
and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR)
or (speed_error > 0
and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
angle_setpoint = self.speed_pid.update(
speed_error, freeze_integrator=freeze_integrator)
# Clip angle error, this is enough to get up from stands
angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint,
-MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
torque = self.balance_pid.update(angle_error,
error_rate=angle_error_rate)
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl -
CS.out.wheelSpeeds.fr)
turn_error = speed_diff_measured - speed_diff_desired
freeze_integrator = (
(turn_error < 0
and self.turn_pid.error_integral <= -MAX_POS_INTEGRATOR)
or (turn_error > 0
and self.turn_pid.error_integral >= MAX_POS_INTEGRATOR))
torque_diff = self.turn_pid.update(
turn_error, freeze_integrator=freeze_integrator)
# Combine 2 PIDs outputs
torque_r = torque + torque_diff
torque_l = torque - torque_diff
# Torque rate limits
self.torque_r_filtered = np.clip(
self.deadband_filter(torque_r, 10),
self.torque_r_filtered - MAX_TORQUE_RATE,
self.torque_r_filtered + MAX_TORQUE_RATE)
self.torque_l_filtered = np.clip(
self.deadband_filter(torque_l, 10),
self.torque_l_filtered - MAX_TORQUE_RATE,
self.torque_l_filtered + MAX_TORQUE_RATE)
torque_r = int(
np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE))
torque_l = int(
np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
can_sends = []
can_sends.append(
bodycan.create_control(self.packer, torque_l, torque_r,
self.frame // 2))
new_actuators = CC.actuators.copy()
new_actuators.accel = torque_l
new_actuators.steer = torque_r
self.frame += 1
return new_actuators, can_sends
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 LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
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.kf = CP.lateralTuning.torque.kf
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
angle_steers_des = 0.0
self.errors = []
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
else:
actual_curvature_vm = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
actual_curvature_llk = llk.angularVelocityCalibrated.value[
2] / CS.vEgo
actual_curvature = interp(
CS.vEgo, [2.0, 5.0],
[actual_curvature_vm, actual_curvature_llk])
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 = apply_deadzone(setpoint - measurement, self.deadzone)
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)
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(error,
[-JERK_THRESHOLD, JERK_THRESHOLD],
[-self.friction, self.friction])
ff += friction_compensation / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(
error,
error_rate,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
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
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
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
