class ControllerPD():
parameters = parameters_sys()
# this is a static controller, but I include this here for convenience
# so that all controllers have an estimated disturbance
d_est = numpy.zeros(3)
def __init__(self, parameters=None):
if parameters != None:
# update mass and throttle neutral
self.parameters.kp = parameters[0]
self.parameters.kv = parameters[1]
# self.parameters.kp_z = parameters[2]
# self.parameters.kv_z = parameters[3]
self.parameters.Throttle_neutral = parameters[4]
# def reset_estimate_xy(self):
# return
# def reset_estimate_z(self):
# return
def update_parameters(self, parameters):
self.parameters = parameters
def output(self, t, states, states_d):
# convert euler angles from deg to rotation matrix
ee = states[6:9]
R = GetRotFromEulerAnglesDeg(ee)
R = numpy.reshape(R, 9)
# collecting states
states = numpy.concatenate([states[0:6], R])
return controller(states, states_d, self.parameters)
class ControllerOmega():
# DEFAULT PARAMETERS
parameters = parameters_sys()
# this is a static controller, but I include this here for convenience
# so that all controllers have an estimated disturbance
d_est = numpy.zeros(3)
def __init__(self, parameters=None):
if parameters != None:
# update ...
# self.parameters.m = parameters[0]
pass
# def reset_estimate_xy(self):
# return
# def reset_estimate_z(self):
# return
def update_parameters(self, parameters):
self.parameters = parameters
def output(self, t, states, states_d):
# convert euler angles from deg to rotation matrix
ee = states[6:9]
R = GetRotFromEulerAnglesDeg(ee)
R = numpy.reshape(R, 9)
# collecting states
states = numpy.concatenate([states[0:6], R])
return controller(states, states_d, self.parameters)
class ControllerLoad():
# DEFAULT PARAMETERS
parameters = parameters_sys()
# this is a static controller, but I include this here for convenience
# so that all controllers have an estimated disturbance
d_est = numpy.zeros(3)
def __init__(self, parameters=None):
if parameters != None:
self.parameters.Throttle_neutral = parameters[0]
self._old_input = array([1500., 1500., 0., 1500.])
# def reset_estimate_xy(self):
# return
# def reset_estimate_z(self):
# return
def update_parameters(self, parameters):
self.parameters = parameters
def output(self, t, states, states_d):
# convert euler angles from deg to rotation matrix
ee = states[6:9]
R = GetRotFromEulerAnglesDeg(ee)
R = numpy.reshape(R, 9)
# collecting states
states_body = numpy.concatenate([states[0:6], R])
if states.size == 18:
# load
eel = states[15:18]
Rl = GetRotFromEulerAnglesDeg(eel)
Rl = numpy.reshape(Rl, 9)
states_load = numpy.concatenate([states[9:15], Rl])
self._old_input = controller(states_body, states_load, states_d,
self.parameters)
return self._old_input
return self._old_input
class ControllerPID_bounded():
parameters = parameters_sys()
# estimated disturbance
d_est = numpy.zeros(3)
def __init__(self, parameters=None):
if parameters != None:
# update mass and throttle neutral
self.parameters.kp_C1 = parameters[0]
self.parameters.kv_C1 = parameters[1]
self.parameters.ki_C1 = parameters[2]
self.parameters.kp_z_C1 = parameters[3]
self.parameters.kv_z_C1 = parameters[4]
self.parameters.ki_z_C1 = parameters[5]
self.parameters.Throttle_neutral = parameters[6]
# time is last parameter
self.t_old = parameters[7]
def reset_estimate_xy(self):
self.d_est[0] = 0.0
self.d_est[1] = 0.0
return
def reset_estimate_z(self):
self.d_est[2] = 0.0
return
def update_parameters(self, parameters):
self.parameters = parameters
def output(self, time, states, states_d):
# convert euler angles from deg to rotation matrix
ee = states[6:9]
R = GetRotFromEulerAnglesDeg(ee)
R = numpy.reshape(R, 9)
# collecting states
states = numpy.concatenate([states[0:6], R])
return self.controller(time, states, states_d, self.parameters)
# Controller
def controller(self, t_new, states, states_d, parameters):
# mass of vehicles (kg)
m = parameters.m
# acceleration due to gravity (m/s^2)
g = parameters.g
# Angular velocities multiplication factor
# Dw = parameters.Dw
# third canonical basis vector
e3 = numpy.array([0.0, 0.0, 1.0])
#--------------------------------------#
# transported mass: position and velocity
x = states[0:3]
v = states[3:6]
# thrust unit vector and its angular velocity
R = states[6:15]
R = numpy.reshape(R, (3, 3))
n = R.dot(e3)
# print(GetEulerAngles(R)*180/3.14)
#--------------------------------------#
# desired quad trajectory
xd = states_d[0:3]
vd = states_d[3:6]
ad = states_d[6:9]
#--------------------------------------#
# position error and velocity error
ep = xd - x
ev = vd - v
u = cmd_di_3D(ep, ev, parameters)
# -----------------------------------------------------------------------------#
# vector of commnads to be sent by the controller
U = numpy.zeros(4)
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
Full_actuation = m * (ad + u + g * e3 + self.d_est)
# -----------------------------------------------------------------------------#
# update disturbance estimate
# derivatice of disturbance estimate
d_est_dot = self.disturbance_estimate(ep, ev, parameters)
# new disturbance estimate
self.d_est = self.d_est + d_est_dot * (t_new - self.t_old)
# element-wise division
Max_d = parameters.Max_disturbance_C1
ratio = self.d_est / Max_d
# saturate estimate just for safety (element wise multiplication)
self.d_est = Max_d * bound(ratio, 1, -1)
# update old time
self.t_old = t_new
# -----------------------------------------------------------------------------#
Throttle = numpy.dot(Full_actuation, n)
# this decreases the throtle, which will be increased
Throttle = Throttle * numpy.dot(n, e3)
U[0] = Throttle
#--------------------------------------#
# current euler angles
euler = GetEulerAngles(R)
# current phi and theta
phi = euler[0]
theta = euler[1]
# current psi
psi = euler[2]
#--------------------------------------#
# Rz(psi)*Ry(theta_des)*Rx(phi_des) = n_des
# desired roll and pitch angles
n_des = Full_actuation / numpy.linalg.norm(Full_actuation)
n_des_rot = Rz(-psi).dot(n_des)
sin_phi = -n_des_rot[1]
sin_phi = bound(sin_phi, 1, -1)
phi = numpy.arcsin(sin_phi)
U[1] = phi
sin_theta = n_des_rot[0] / c(phi)
sin_theta = bound(sin_theta, 1, -1)
cos_theta = n_des_rot[2] / c(phi)
cos_theta = bound(cos_theta, 1, -1)
pitch = numpy.arctan2(sin_theta, cos_theta)
U[2] = pitch
#--------------------------------------#
# yaw control: gain
k_yaw = parameters.k_yaw
# desired yaw: psi_star
psi_star = parameters.psi_star
psi_star_dot = 0
psi_dot = psi_star_dot - k_yaw * s(psi - psi_star)
U[3] = 1 / c(phi) * (c(theta) * psi_dot - s(phi) * U[2])
U = self.Cmd_Converter(U, parameters)
return U
#--------------------------------------------------------------------------#
# Comand converter (from 1000 to 2000)
def Cmd_Converter(self, U, parameters):
# mass of vehicles (kg)
m = parameters.m
# acceleration due to gravity (m/s^2)
g = parameters.g
# degrees per second
MAX_PSI_SPEED_Deg = parameters.MAX_PSI_SPEED_Deg
MAX_PSI_SPEED_Rad = MAX_PSI_SPEED_Deg * math.pi / 180.0
MAX_ANGLE_DEG = parameters.MAX_ANGLE_DEG
MAX_ANGLE_RAD = MAX_ANGLE_DEG * math.pi / 180.0
# angles comand between 1000 and 2000 PWM
# desired roll and pitch
# U[1:3] : desired roll and pitch
# Roll
U[1] = 1500.0 + U[1] * 500.0 / MAX_ANGLE_RAD
# Pitch:
# ATTENTTION TO PITCH: WHEN STICK IS FORWARD PITCH,
# pwm GOES TO 1000, AND PITCH IS POSITIVE
U[2] = 1500.0 - U[2] * 500.0 / MAX_ANGLE_RAD
# psi angular speed
U[3] = 1500.0 - U[3] * 500.0 / MAX_PSI_SPEED_Rad
Throttle = U[0]
# REMARK: the throtle comes between 1000 and 2000 PWM
# conversion gain
Throttle_neutral = parameters.Throttle_neutral
K_THROTLE = m * g / Throttle_neutral
U[0] = Throttle / K_THROTLE
# rearrage to proper order
# [roll,pitch,throttle,yaw]
U_new = numpy.zeros(4)
U_new[0] = U[1]
U_new[1] = U[2]
U_new[2] = U[0]
U_new[3] = U[3]
return U_new
def disturbance_estimate(self, ep, ev, parameters):
# derivative = numpy.array([0.0,0.0,0.0])
# kp = parameters.kp_C1
# kv = parameters.kv_C1
# ki = parameters.ki_C1
# derivative[0] = ki*(kp/2*ep[0] + ev[0])
# derivative[1] = ki*(kp/2*ep[1] + ev[1])
# kp = parameters.kp_z_C1
# kv = parameters.kv_z_C1
# ki = parameters.ki_z_C1
# derivative[2] = ki*(kp/2*ep[2] + ev[2])
ki_xy = parameters.ki_C1
ki_z = parameters.ki_z_C1
ki = numpy.array([ki_xy, ki_xy, ki_z])
# element wise multiplication
return ki * Vgrad(ep, ev, parameters)