예제 #1
0
파일: aircraft.py 프로젝트: andradr/pycda
    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)
예제 #2
0
    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]);