f16_dyn_decoupled.c

f16的非线性模型设计

#if defined(MDL_UPDATE)
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif


/*==========================*/
/* Function: mdlDerivatives */
/*==========================*/
#define MDL_DERIVATIVES  
#if defined(MDL_DERIVATIVES)
  static void mdlDerivatives(SimStruct *S)
  {
    real_T *x  = ssGetContStates(S);
    real_T *dx = ssGetdX(S);
    
    InputRealPtrsType u = ssGetInputPortRealSignalPtrs(S, 0);
    InputRealPtrsType dlef_in = ssGetInputPortRealSignalPtrs(S, 1);
    InputRealPtrsType fi_flag_in = ssGetInputPortRealSignalPtrs(S, 2);
    InputRealPtrsType dist = ssGetInputPortRealSignalPtrs(S, 3);
    
    /* Declare a lot of variables */
    double *temp;
    double Gamma;
    double CX_tot, CY_tot, CZ_tot, Cl_tot, Cm_tot, Cn_tot;
	double Cx, Cz, Cm, Cy, Cn, Cl;
    double Cxq, Cyr, Cyp, Czq, Clr, Clp, Cmq, Cnr, Cnp;
    double delta_Cx_lef, delta_Cz_lef,delta_Cm_lef;
    double delta_Cy_lef,delta_Cn_lef,delta_Cl_lef;
    double delta_Cxq_lef, delta_Cyr_lef, delta_Cyp_lef, delta_Czq_lef; 
    double delta_Clr_lef, delta_Clp_lef, delta_Cmq_lef, delta_Cnr_lef;
    double delta_Cnp_lef;
    double delta_Cy_r30, delta_Cn_r30, delta_Cl_r30;
    double delta_Cy_a20, delta_Cy_a20_lef, delta_Cn_a20, delta_Cn_a20_lef;
    double delta_Cl_a20, delta_Cl_a20_lef;
    double delta_Cnbeta, delta_Clbeta, delta_Cm, eta_el, delta_Cm_ds;
	double da_norm, dr_norm, dlef_norm;
	double ALPHA, BETA, DE, DA, DR, DLEF, alpha_lef, DEL, DER, DAL, DAR;
    double Vt, alpha, beta, Vt_dist;
    double cpow, g, dq;
      
    temp = (double *) malloc(9 * sizeof(double)); 
    
    /* Body velocity components */
    u_body = Vt_st * cos(alpha_st) * cos(beta_st);
    v_body = Vt_st * sin(beta_st);
    w_body = Vt_st * sin(alpha_st) * cos(beta_st);

    /* calculate influence of turbulence on aerodynamic angles */
    Vt_dist = sqrt(u_dist * u_dist + v_dist * v_dist + w_dist * w_dist);
    Vt = sqrt(Vt_st * Vt_st + Vt_dist * Vt_dist);
    alpha = atan((w_body + w_dist) / (u_body + u_dist));
    beta = asin((v_body + v_dist) / Vt);
    
    /* Usefull notation of moments of inertia,
     see e.g. Lewis and Stevens "Aircraft control and simulation" */
    Gamma = Ixx * Izz - (Ixz  * Ixz);
    C1 = ((Iyy - Izz)  * Izz  - (Ixz * Ixz))/ Gamma;
    C2 = ((Ixx - Iyy + Izz ) * Ixz ) / Gamma;
    C3 = Izz / Gamma;
    C4 = Ixz / Gamma;
    C5 = (Izz - Ixx) / Iyy;
    C6 = Ixz / Iyy ;
    C7 = 1 / Iyy;
    C8 = (Ixx * (Ixx - Iyy ) + Ixz * Ixz) / Gamma;
    C9 = Ixx / Gamma;  
        
    /* Engine model, based on Ying Huo's m-files */
    cpow = tgear (dth);
    pow_dot = pdot ( pow, cpow );
    Thrust = thrst ( pow, -z_earth, Mach ); 
    
	/* ISA atmosphere */
    atmos(-z_earth, Vt, temp);
        Mach = temp[0];
        qbar = temp[1];  
        g    = temp[2]; 
        
    /* Transformation rad to deg for the lookup tables */
    ALPHA = alpha * rtd;
    BETA = beta * rtd;
	DE = (del + der) / 2 * rtd;
    DEL = del * rtd;
    DER = der * rtd;
	DA = (dal - dar) / 2 * rtd;
    DAL = dal * rtd;
    DAR = dar * rtd;
	DR = dr * rtd;	 
	DLEF = dlef * rtd;	 
    
	/* LEF tables are only valid up until alpha = 45 degrees*/
	if (ALPHA > 45)
	{
	alpha_lef = 45;
	}
	else
	{
	alpha_lef = ALPHA;
	}
    
    /* Limit of elevator deflection is 25 degrees*/
	if (DE > 25)
	{
	DE = 25;
	}
	if (DE < -25)
	{
	DE = -25;
	}	 
    
    /* Normalizing the control deflections */
	da_norm = (DAR - DAL) /21.5;
	dr_norm = DR / 30.0;
      
    if (fi_flag == 1)          /* hifi lookup tables */
    {   
        
        dlef_norm = (1 - DLEF/25.0);
        
        Cx = 0.5 * (_Cx(ALPHA, BETA, DEL) + _Cx(ALPHA, BETA, DER));
        Cy = _Cy(ALPHA, BETA);
        Cz = 0.5 * (_Cz(ALPHA, BETA, DEL) + _Cz(ALPHA, BETA, DER));
        Cl = _Cl(ALPHA, BETA, DE) + (_Cz(ALPHA, BETA, DER) - _Cz(ALPHA, BETA, DEL)) * y_stab_cg;
        Cm = 0.5 * (_Cm(ALPHA, BETA, DEL) + _Cm(ALPHA, BETA, DER));
        Cn = _Cn(ALPHA, BETA, DE);
	 
        Cxq = _CXq(ALPHA);
        Cyp = _CYp(ALPHA);
        Cyr = _CYr(ALPHA);
        Czq = _CZq(ALPHA);
        Clp = _CLp(ALPHA);
        Clr = _CLr(ALPHA);
        Cmq = _CMq(ALPHA);
        Cnp = _CNp(ALPHA);
        Cnr = _CNr(ALPHA);
	 
        delta_Cx_lef = _Cx_lef(alpha_lef, BETA) - _Cx(ALPHA, BETA, 0);
        delta_Cy_lef = _Cy_lef(alpha_lef, BETA) - _Cy(ALPHA, BETA);
        delta_Cz_lef = _Cz_lef(alpha_lef, BETA) - _Cz(ALPHA, BETA, 0);
        delta_Cl_lef = _Cl_lef(alpha_lef, BETA) - _Cl(ALPHA, BETA, 0);
        delta_Cm_lef = _Cm_lef(alpha_lef, BETA) - _Cm(ALPHA, BETA, 0);
        delta_Cn_lef = _Cn_lef(alpha_lef, BETA) - _Cn(ALPHA, BETA, 0);
	 
        delta_Cxq_lef = _delta_CXq_lef(alpha_lef);
        delta_Cyp_lef = _delta_CYp_lef(alpha_lef);
        delta_Cyr_lef = _delta_CYr_lef(alpha_lef);
        delta_Czq_lef = _delta_CZq_lef(alpha_lef);
        delta_Clp_lef = _delta_CLp_lef(alpha_lef);
        delta_Clr_lef = _delta_CLr_lef(alpha_lef);
        delta_Cmq_lef = _delta_CMq_lef(alpha_lef);
        delta_Cnp_lef = _delta_CNp_lef(alpha_lef);
        delta_Cnr_lef = _delta_CNr_lef(alpha_lef);
	 
        delta_Cy_r30 = _Cy_r30(ALPHA, BETA) - _Cy(ALPHA, BETA);
        delta_Cl_r30 = _Cl_r30(ALPHA, BETA) - _Cl(ALPHA, BETA, 0);
        delta_Cn_r30 = _Cn_r30(ALPHA, BETA) - _Cn(ALPHA, BETA, 0);
	 
        delta_Cy_a20     = _Cy_a20(ALPHA, BETA) - _Cy(ALPHA, BETA);
        delta_Cy_a20_lef = _Cy_a20_lef(alpha_lef, BETA) - 
            _Cy_lef(alpha_lef, BETA) - delta_Cy_a20;
        delta_Cn_a20     = _Cn_a20(ALPHA, BETA) - _Cn(ALPHA, BETA, 0);
        delta_Cn_a20_lef = _Cn_a20_lef(alpha_lef, BETA) - 
            _Cn_lef(alpha_lef, BETA) - delta_Cn_a20;
        delta_Cl_a20     = _Cl_a20(ALPHA, BETA) - _Cl(ALPHA, BETA, 0);
        delta_Cl_a20_lef = _Cl_a20_lef(alpha_lef, BETA) - 
            _Cl_lef(alpha_lef, BETA) - delta_Cl_a20;

        delta_Cnbeta = _delta_CNbeta(ALPHA);
        delta_Clbeta = _delta_CLbeta(ALPHA);
        delta_Cm     = _delta_Cm(ALPHA);
        eta_el       = _eta_el(DE);
        delta_Cm_ds  = _delta_Cm_ds(ALPHA,DE);
    }

else if (fi_flag == 0)  /* lofi lookup tables (do not include dlef) */
    {  
        dlef_norm = 0.0;

        damping(ALPHA, temp);
            Cxq = temp[0];
            Cyr = temp[1];
            Cyp = temp[2];
            Czq = temp[3];
            Clr = temp[4];
            Clp = temp[5];
            Cmq = temp[6];
            Cnr = temp[7];
            Cnp = temp[8];
        
        dmomdcon(ALPHA, BETA, temp);
            delta_Cl_a20 = temp[0];    
            delta_Cl_r30 = temp[1];    
            delta_Cn_a20 = temp[2];    
            delta_Cn_r30 = temp[3];    

        clcn(ALPHA, BETA, temp);
            Cl = temp[0];
            Cn = temp[1];

        cxcm(ALPHA, DE, temp);
            Cx = temp[0];
            Cm = temp[1];

            Cy = -.02*BETA + .021*da_norm + .086*dr_norm;

        cz(ALPHA, BETA, DE, temp);
            Cz = temp[0];  
        
        /* All other coeffcients are zero for the lofi model */
        delta_Cx_lef    = 0.0;
        delta_Cz_lef    = 0.0;
        delta_Cm_lef    = 0.0;
        delta_Cy_lef    = 0.0;
        delta_Cn_lef    = 0.0;
        delta_Cl_lef    = 0.0;
        delta_Cxq_lef   = 0.0;
        delta_Cyr_lef   = 0.0;
        delta_Cyp_lef   = 0.0;
        delta_Czq_lef   = 0.0;
        delta_Clr_lef   = 0.0;
        delta_Clp_lef   = 0.0;
        delta_Cmq_lef   = 0.0;
        delta_Cnr_lef   = 0.0;
        delta_Cnp_lef   = 0.0;
        delta_Cy_r30    = 0.0;
        delta_Cy_a20    = 0.0;
        delta_Cy_a20_lef= 0.0;
        delta_Cn_a20_lef= 0.0;
        delta_Cl_a20_lef= 0.0;
        delta_Cnbeta    = 0.0;
        delta_Clbeta    = 0.0;
        delta_Cm        = 0.0;
        eta_el          = 1.0; 
        delta_Cm_ds     = 0.0;
    }

    /* Total force coefficients */
    
    /* Cx_tot */
	CX_tot = Cx + delta_Cx_lef * dlef_norm 
     + (cref/(2*Vt))*(Cxq + delta_Cxq_lef * dlef_norm) * (q_body + q_dist);
    /* Cy_tot */
    CY_tot = Cy + delta_Cy_lef * dlef_norm 
     + (delta_Cy_a20 + delta_Cy_a20_lef * dlef_norm) * da_norm
     + delta_Cy_r30 * dr_norm 
     + (bref / (2*Vt))*(Cyr + delta_Cyr_lef * dlef_norm) * (r_body  + r_dist)
     + (bref/(2*Vt))*(Cyp + delta_Cyp_lef * dlef_norm) * (p_body + p_dist);
    /* Cz_tot */
    CZ_tot = Cz + delta_Cz_lef * dlef_norm 
     + (cref/(2*Vt))*(Czq + delta_Czq_lef * dlef_norm) * (q_body + q_dist);
    
    /* Total moment coefficients */
    
    /* Cl_tot */
    Cl_tot = Cl + delta_Cl_lef * dlef_norm 
     + (delta_Cl_a20 + delta_Cl_a20_lef * dlef_norm) * da_norm
     + delta_Cl_r30 * dr_norm 
     + (bref / ((2*Vt))*(Clr + delta_Clr_lef * dlef_norm)) * (r_body + r_dist)
     + ((bref / (2*Vt)) * (Clp + delta_Clp_lef * dlef_norm)) * (p_body + p_dist)
     + delta_Clbeta * beta;
    /* Cm_tot */
    Cm_tot = Cm * eta_el + CZ_tot * (xcgr - xcg) + delta_Cm_lef * dlef_norm 
     + (cref / (2*Vt))*(Cmq + delta_Cmq_lef * dlef_norm) * (q_body + q_dist)
     + delta_Cm + delta_Cm_ds;
    /* Cn_tot */
    Cn_tot = Cn + delta_Cn_lef * dlef_norm 
     - CY_tot * (xcgr - xcg)*(cref/bref) 
     + (delta_Cn_a20 + delta_Cn_a20_lef * dlef_norm) * da_norm 
     + ((bref / (2*Vt)) * (Cnr + delta_Cnr_lef * dlef_norm))* (r_body + r_dist)
     + ((bref / (2*Vt)) * (Cnp + delta_Cnp_lef * dlef_norm)) * (p_body + p_dist)
     + delta_Cn_r30 * dr_norm + delta_Cnbeta * beta;
	
	/* Total forces */
    Xbar = qbar * Sref * CX_tot;
    Ybar = qbar * Sref * CY_tot;
    Zbar = qbar * Sref * CZ_tot;
    
    /* Total moments */
    Lbar = Cl_tot * qbar * Sref * bref;
    Mbar = Cm_tot * qbar * Sref * cref;
    Nbar = Cn_tot * qbar * Sref * bref;
    
    /* Derivatives */
    u_body_dot = r_body * v_body - q_body * w_body 
     + (Xbar + Thrust) / mass + 2*(q1*q3 - q0*q2)*g;
    v_body_dot = p_body * w_body - r_body * u_body 
     + Ybar / mass + 2*(q2*q3 + q0*q1)*g;
    w_body_dot = q_body * u_body - p_body * v_body 
     + Zbar / mass + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*g;
    
    Vt_dot      = (u_body * u_body_dot + v_body * v_body_dot 
     + w_body * w_body_dot) / Vt;
    beta_dot    = (v_body_dot * Vt - v_body * Vt_dot) 
     / (Vt * Vt * cos(beta));
    alpha_dot   = (u_body * w_body_dot - w_body * u_body_dot) 
     / (u_body * u_body + w_body * w_body);
    
    q0_dot      = 0.5 * (-p_body * q1 - q_body * q2 - r_body * q3);
    q1_dot      = 0.5 * ( p_body * q0 + r_body * q2 - q_body * q3);
    q2_dot      = 0.5 * ( q_body * q0 - r_body * q1 + p_body * q3);
    q3_dot      = 0.5 * ( r_body * q0 + q_body * q1 - p_body * q2);
    
    /* correction term*/
    dq = q0 * q0_dot + q1 * q1_dot + q2 * q2_dot + q3 * q3_dot;
    
    q0_dot -= dq * q0;
    q1_dot -= dq * q1;
    q2_dot -= dq * q2;
    q3_dot -= dq * q3;
      
    p_body_dot  = (C1 * r_body + C2 * p_body) * q_body 
     + C3 * Lbar + C4 * (Nbar + q_body * heng);
    q_body_dot  = C5 * p_body * r_body 
     - C6 * (p_body * p_body - r_body * r_body) 
     + C7 * (Mbar - heng * r_body);
    r_body_dot  = (C8 * p_body - C2 * r_body) * q_body 
     + C4 * Lbar + C9 * (Nbar + q_body * heng);
  
    x_earth_dot = (q0*q0 + q1*q1 - q2*q2 - q3*q3) * u_body 
     + 2*(q1*q2 - q0*q3) * v_body 
     + 2*(q1*q3 + q0*q2) * w_body;
    y_earth_dot = 2*(q1*q2 + q0*q3) * u_body 
     + (q0*q0 - q1*q1 + q2*q2 - q3*q3) * v_body 
     + 2*(q2*q3 - q0*q1) * w_body;
    z_earth_dot = 2*(q1*q3 - q0*q2) * u_body 
     + 2*(q2*q3 + q0*q1) * v_body 
     + (q0*q0 - q1*q1 - q2*q2 + q3*q3) * w_body;
    
    free(temp);
}   /* end mdlDerivatives */

#endif 


/*===================================*/
/* Function: mdlTerminate */
/*===================================*/
static void mdlTerminate(SimStruct *S)
{
}

/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif