def _calc(self, Va, x, u):
"""
V: velocity
x: state vector
u: input vector
"""
#Calc Atmospheric Parameters
R = 287.0
gamma = 1.4
#Va = [50.0,0.0,0.0]
V = sqrt(Va[0]**2+Va[1]**2+Va[2]**2)
alpha = arctan(Va[2]/Va[0])
beta = arcsin(Va[1]/V)
#u = array([0.0,0.0,0.0,0.0,0.0,0.0,0.0])
#x = array([V*cos(alpha)*cos(beta),V*sin(beta),V*sin(alpha)*cos(beta),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
elevator = u[0]
aileron = u[1]
rudder = u[2]
throttle = u[3]
spoiler = u[4]
flap = u[5]
stab = u[6]
if flap >= 0.65:
self.flaps = True
else:
self.flaps = False
self.gear = False
self.spoilers = False
height = -x[5]
T,P = atmosphere(height)
rho = P/(R*T)
a = sqrt(gamma*R*T)
p = x[6]
q = x[7]
r = x[8]
#Calc Mach
Mach = V/a
self._calcmach(Mach)
self._thrust(throttle, rho, height)
self._CL(V, alpha, q, stab, elevator)
self._CD()
self._Cm(V, alpha, p, q, elevator)
self._CY(beta, rudder, aileron, spoiler)
self._Cn(beta, V, r, p, aileron, rudder, spoiler)
self._Cl(beta, p, r, spoiler, rudder, aileron, V, Mach)
def _derivative(self):
a = self.a
x = self.x
u = self.u
HEB = transformation(x[9],x[10],x[11])
gb = HEB*matrix([[0.0],[0.0],[9.80665]])
Va = [x[0],x[1],x[2]]
V = sqrt(Va[0]**2+Va[1]**2+Va[2]**2)
alpha = arctan(Va[2] / Va[0])
beta = arcsin(Va[1] / V)
T,P = atmosphere(-x[5])
R = 287.0
rho = P/R/T
qbar = .5*rho*V*V
qbarS = qbar*a.S
a._calc(Va, x, u)
CL = a.CL
CD = a.CD
CY = a.CY
Cl = a.Cl
Cm = a.Cm
Cn = a.Cn
thrust = a.thrust
Ixx = a.Ixx
Iyy = a.Iyy
Izz = a.Izz
Ixz = a.Ixz
CX = -CD*cos(alpha) + CL*sin(alpha)
CZ = -CD*sin(alpha) - CL*cos(alpha)
Xb = (CX*qbarS + thrust)/a.m
Yb = CY*qbarS/a.m
Zb = CZ*qbarS/a.m
Lb = Cl*qbarS*a.b
Mb = Cm*qbarS*a.c
Nb = Cn*qbarS*a.b
nz = -Zb/9.80665
xd0 = Xb + float(gb[0]) + x[8]*x[1] - x[7]*x[2];
xd1 = Yb + float(gb[1]) - x[8]*x[0] + x[6]*x[2];
xd2 = Zb + float(gb[2]) + x[7]*x[0] - x[6]*x[1];
y = HEB.transpose()*matrix([[x[0]],[x[1]],[x[2]]])
xd3 = float(y[0]);
xd4 = float(y[1]);
xd5 = float(y[2]);
xd6 = (Izz*Lb + Ixz*Nb - (Ixz*(Iyy - Ixx - Izz)*x[6]+\
(Ixz**2 + Izz*(Izz - Iyy))*x[8]) * x[7]) / (Ixx*Izz - Ixz**2);
xd7 = (Mb - (Ixx - Izz) * x[6] * x[8] - Ixz * (x[6]**2 - x[8]**2)) / Iyy;
xd8 = (Ixz*Lb + Ixx*Nb + (Ixz*(Iyy - Ixx - Izz)*x[8] + \
(Ixz**2 + Ixx*(Ixx - Iyy))*x[6])*x[7])/(Ixx*Izz - Ixz**2);
cosPitch = cos(x[10]);
if abs(cosPitch) <= 0.00001:
cosPitch = 0.00001 * sign(cosPitch)
tanPitch = sin(x[10]) / cosPitch;
xd9 = x[6] + (sin(x[9]) * x[7] + cos(x[9]) * x[8]) * tanPitch;
xd10 = cos(x[9]) * x[7] - sin(x[9]) * x[8];
xd11 = (sin(x[9]) * x[7] + cos(x[9]) * x[8]) / cosPitch;
return array([xd0,xd1,xd2,xd3,xd4,xd5,xd6,xd7,xd8,xd9,xd10,xd11]);
