def Forces(self):
Fx = (self.delta_p*self.MaxSpeed)**2*self.ct
F_prop = array([Fx, 0.0, 0.0])
psiprop = radians(self.attitude[0])
thetaprop = radians(self.attitude[1])
phiprop = radians(self.attitude[2])
return body2earth(F_prop, psiprop, thetaprop, phiprop)
def Forces(self):
Fx = self.delta_p*self.Fmaxi*(self._rho/self.rhoi)**(self.nrho)
F_prop = array([Fx, 0.0, 0.0])
psiprop = radians(self.attitude[0])
thetaprop = radians(self.attitude[1])
phiprop = radians(self.attitude[2])
return body2earth(F_prop, psiprop, thetaprop, phiprop)
def Moments(self):
MD = (self.delta_p*self.MaxSpeed)**2*self.cd
if self.sense == 'counter-clockwise':
MD = -MD
elif self.sense == 'clockwise':
MD = MD
else:
MD = 0.0
print('Incorrect sense')
MD_vec = array([MD, 0.0, 0.0])
psiprop = radians(self.attitude[0])
thetaprop = radians(self.attitude[1])
phiprop = radians(self.attitude[2])
return cross(self.position, self.Forces) + body2earth(MD_vec, psiprop, thetaprop, phiprop)
def dynamics(self, t, X, U):
# State space
z = X[2]
u = X[3]
v = X[4]
w = X[5]
uvw = array([u, v, w])
phi = X[6]
theta = X[7]
psi = X[8]
p = X[9]
q = X[10]
r = X[11]
pqr = array([p, q, r])
#Controls
delta_p = U[0]
delta_e = U[1]
delta_a = U[2]
delta_r = U[3]
# Aircraft
m = self.aircraft._mass
Inertia = self.aircraft.inertia
InvInertia = self.aircraft.invInertia
#gravity model
self.gravity._altitude = -z
g = self.gravity._gravity
F_gravity_earth = m * g
F_gravity_body = earth2body(F_gravity_earth, psi, theta, phi)
#Atmospheric model
self.atmosphere._altitude = -z
rho = self.atmosphere._rho
#Aerodynamic model
self.aircraft._rho = rho
alpha, beta, TAS = computeTAS(uvw)
self.aircraft.delta_r = delta_r
self.aircraft.delta_e = delta_e
self.aircraft.delta_a = delta_a
self.aircraft.alpha = alpha
self.aircraft.beta = beta
self.aircraft.TAS = TAS
self.aircraft.p = p
self.aircraft.q = q
self.aircraft.r = r
F_aero_aero = self.aircraft.Forces
M_aero_aero = self.aircraft.Moments
F_aero_body = aero2body(F_aero_aero, alpha, beta)
M_aero_body = aero2body(M_aero_aero, alpha, beta)
#Propulsion model
self.propulsion.delta_p = delta_p
self.propulsion._rho = rho
F_prop_body = self.propulsion.Forces
M_prop_body = self.propulsion.Moments
"""
Kinematics
"""
# Linear Kinematics
xyz_d = body2earth(uvw, psi, theta, phi)
x_d = xyz_d[0]
y_d = xyz_d[1]
z_d = xyz_d[2]
# Angular Kinematics
phithetapsi_d = body2euler(pqr, theta, phi)
phi_d = phithetapsi_d[0]
theta_d = phithetapsi_d[1]
psi_d = phithetapsi_d[2]
"""
Dynamics
"""
# Linear Dynamics
uvw_d = 1 / m * (F_prop_body + F_aero_body + F_gravity_body) - cross(
pqr, uvw)
u_d = uvw_d[0]
v_d = uvw_d[1]
w_d = uvw_d[2]
# Angular Dynamics
Moments = M_aero_body + M_prop_body
pqr_d = InvInertia.dot(Moments + cross(pqr, Inertia.dot(pqr)))
p_d = pqr_d[0]
q_d = pqr_d[1]
r_d = pqr_d[2]
# Obtain alpha_d and beta_d to use on next iteration of the aeodynamic model
# TODO: It slows down hugly the simulation. Solve it.
"""
if t!=0:
Valphabeta_d = array([1., 1./(TAS),1./(TAS*cos(beta))])*body2aero(uvw_d, alpha, beta)
beta_d = Valphabeta_d[1]
alpha_d = Valphabeta_d[2]
# Send information to aerodynamic model
self.aircraft.alpha_d = alpha_d
self.aircraft.beta_d = beta_d
"""
return array([
x_d, y_d, z_d, u_d, v_d, w_d, phi_d, theta_d, psi_d, p_d, q_d, r_d
])
def Dronedynamics(self, t, X, U):
# State space
z = X[2]
u = X[3]
v = X[4]
w = X[5]
uvw = array([u, v, w])
phi = X[6]
theta = X[7]
psi = X[8]
p = X[9]
q = X[10]
r = X[11]
pqr = array([p, q, r])
# Aircraft
m = self.aircraft._mass
Inertia = self.aircraft.inertia
InvInertia = self.aircraft.invInertia
#gravity model
self.gravity._altitude = -z
g = self.gravity._gravity
F_gravity_earth = m * g
F_gravity_body = earth2body(F_gravity_earth, psi, theta, phi)
#Atmospheric model
self.atmosphere._altitude = -z
rho = self.atmosphere._rho
#Aerodynamic model
self.aircraft._rho = rho
alpha, beta, TAS = computeTAS(uvw)
self.aircraft.alpha = alpha
self.aircraft.beta = beta
self.aircraft.TAS = TAS
self.aircraft.p = p
self.aircraft.q = q
self.aircraft.r = r
F_aero_aero = self.aircraft.Forces
M_aero_aero = self.aircraft.Moments
F_aero_body = aero2body(F_aero_aero, alpha, beta)
M_aero_body = aero2body(M_aero_aero, alpha, beta)
#Propulsion model
self.propulsion.delta_p = U
self.propulsion._rho = rho
F_prop_body = self.propulsion.Forces
M_prop_body = self.propulsion.Moments
"""
Kinematics
"""
# Linear Kinematics
xyz_d = body2earth(uvw, psi, theta, phi)
x_d = xyz_d[0]
y_d = xyz_d[1]
z_d = xyz_d[2]
# Angular Kinematics
phithetapsi_d = body2euler(pqr, theta, phi)
phi_d = phithetapsi_d[0]
theta_d = phithetapsi_d[1]
psi_d = phithetapsi_d[2]
"""
Dynamics
"""
# Linear Dynamics
uvw_d = 1 / m * (F_prop_body + F_aero_body + F_gravity_body) - cross(
pqr, uvw)
u_d = uvw_d[0]
v_d = uvw_d[1]
w_d = uvw_d[2]
# Angular Dynamics
Moments = M_aero_body + M_prop_body
pqr_d = InvInertia.dot(Moments + cross(pqr, Inertia.dot(pqr)))
p_d = pqr_d[0]
q_d = pqr_d[1]
r_d = pqr_d[2]
return array([
x_d, y_d, z_d, u_d, v_d, w_d, phi_d, theta_d, psi_d, p_d, q_d, r_d
])