f16_dynam_complex.c

f16的非线性模型设计

    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    ssSetOptions(S, 0);
}

/*===================================*/
/* Function: mdlInitalizeSampleTimes */
/*===================================*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);

}

/*==================================*/
/* Function: mdlInitalizeConditions */
/*==================================*/

#define MDL_INITIALIZE_CONDITIONS  
#if defined(MDL_INITIALIZE_CONDITIONS)

  static void mdlInitializeConditions(SimStruct *S)
  {
  }
#endif 


/*====================*/
/* Function: mdlStart */
/*====================*/
#define MDL_START 
#if defined(MDL_START) 

  static void mdlStart(SimStruct *S)
  {
        real_T *x = ssGetContStates(S);
        int i;
        
        /* Initialize the state vector */
        for (i = 0; i < ssGetNumContStates(S); i++)
        {
            x[i] = mxGetPr(ssGetSFcnParam(S, 0))[i];    
        }
  }
#endif 



/*======================*/
/* Function: mdlOutputs */
/*======================*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    real_T *x = ssGetContStates(S);
    real_T *y = ssGetOutputPortRealSignal(S, 0);
    
	 int i;
	 
	 for (i = 0; i < ssGetNumContStates(S); i++)
	 {
		y[i] = x[i]; /* outputs are the states */
	 }
}


/*=====================*/
/* Function: mdlUpdate */
/*=====================*/
#undef MDL_UPDATE  
#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 u2 = ssGetInputPortRealSignalPtrs(S, 1);
    InputRealPtrsType u3 = ssGetInputPortRealSignalPtrs(S, 2);
    InputRealPtrsType u4 = ssGetInputPortRealSignalPtrs(S, 3);
    InputRealPtrsType u5 = ssGetInputPortRealSignalPtrs(S, 4);
    
    /* 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;
    double cpow, g;
    double Fv[3][3] = {0};
    double Fw[3][3] = {0};
    double Ff[3][1] = {0};
    double Mv[3][3] = {0};
    double Mw[3][3] = {0};
    double Mf[3][1] = {0};    
    double Fv_inv[3][3] = {0};
    double Mw_inv[3][3] = {0};
    double Omega_dot[3][1] = {0};
    double VV_dot[3][1] = {0};
    double MvFv_inv[3][3] = {0};
    double MvFv_invFw[3][3] = {0};
    double inv_w[3][3] = {0};
    double MvFv_invFf[3][1] = {0};   
    double FwMw_inv[3][3] = {0};
    double FwMw_invMv[3][3] = {0};
    double inv_v[3][3] = {0};
    double FwMw_invMf[3][1] = {0};    
    double lol[3][3] = {0};
    double lol1[3][3] = {0};
    double lol2[3][3] = {0};
    double lol3[3][3] = {0};

    double Vx[3][3] = {0};
    double Dx[3][3] = {0};
    double Ox[3][3] = {0};
    double Ix[3][3] = {0};
    double OxDx[3][3] = {0};
    double OxIx[3][3] = {0};
    double VxDx[3][3] = {0};
    
    double AA[6][6] = {0};
    double BB[6][6] = {0};
    double BB_inv[6][6] = {0};
    double AAx[6][1] = {0};
    double AAt[6][1] = {0};
    double statex[6][1] = {0};
    double statex_dot[6][1] = {0};
    
    temp = (double *)malloc(9*sizeof(double)); 
    
    /* 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;  
    
    /* ISA atmosphere */
    atmos(-z_earth, Vt, temp);
        Mach = temp[0];
        qbar = temp[1];  
        g    = temp[2];
        
    /* Engine model, based on Ying Huo's m-files */
    cpow = tgear (dth);
    power_dot = pdot ( power, cpow );
    Thrust = thrst ( power, -z_earth, Mach ); 
    
    /* Body velocity components */
    u_body = Vt * cos(alpha) * cos(beta);
    v_body = Vt * sin(beta);
    w_body = Vt * sin(alpha) * cos(beta);
	 
    /* Transformation rad to deg for the lookup tables */
    ALPHA = alpha * rtd;
    BETA = beta * rtd;
	DE = de * rtd;
	DA = da * 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 = DA/21.5;
	dr_norm = DR/30.0;
      
    if (fi_flag == 1)          /* hifi lookup tables */
    {   
        
        dlef_norm = (1 - DLEF/25.0);
        
        Cx = _Cx(ALPHA, BETA, DE);
        Cy = _Cy(ALPHA, BETA);
        Cz = _Cz(ALPHA, BETA, DE);
        Cl = _Cl(ALPHA, BETA, DE);
        Cm = _Cm(ALPHA, BETA, DE);
        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);