📄 f16_dynam_complex.c
字号:
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;
/* 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
+ (bref/(2*Vt))*(Cyp + delta_Cyp_lef * dlef_norm) * p_body;
/* Cz_tot */
CZ_tot = Cz + delta_Cz_lef * dlef_norm
+ (cref/(2*Vt))*(Czq + delta_Czq_lef * dlef_norm) * q_body;
/* 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
+ ((bref / (2*Vt)) * (Clp + delta_Clp_lef * dlef_norm)) * p_body
+ 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
+ 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
+ ((bref / (2*Vt)) * (Cnp + delta_Cnp_lef * dlef_norm)) * p_body
+ 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;
/*-------------------------------------------------------------------*/
/*General Equations of Motion based on AIAA paper 2007-6306 */
/*by B.J. Bacon and I.M. Gregory of NASA Langley Research Center */
/*-------------------------------------------------------------------*/
Vx[0][0] = 0; Vx[0][1] = -w_body; Vx[0][2] = v_body;
Vx[1][0] = w_body; Vx[1][1] = 0; Vx[1][2] = -u_body;
Vx[2][0] = -v_body; Vx[2][1] = u_body; Vx[2][2] = 0;
Dx[0][0] = 0; Dx[0][1] = -mass*delta_z; Dx[0][2] = mass*delta_y;
Dx[1][0] = mass*delta_z; Dx[1][1] = 0; Dx[1][2] = -mass*delta_x;
Dx[2][0] = -mass*delta_y; Dx[2][1] = mass*delta_z; Dx[2][2] = 0;
Ox[0][0] = 0;Ox[0][1] = -r_body;Ox[0][2] = q_body;
Ox[1][0] = r_body;Ox[1][1] = 0;Ox[1][2] = -p_body;
Ox[2][0] = -q_body;Ox[2][1] = p_body;Ox[2][2] = 0;
Ix[0][0] = Ixx;Ix[0][1] = -Ixy;Ix[0][2] = -Ixz;
Ix[1][0] = -Ixy;Ix[1][1] = Iyy;Ix[1][2] = -Iyz;
Ix[2][0] = -Ixz;Ix[2][1] = -Iyz;Ix[2][2] = Izz;
mult_matrixmatrix(Ox, Dx, OxDx);
mult_matrixmatrix(Ox, Ix, OxIx);
mult_matrixmatrix(Vx, Dx, VxDx);
matrix_sub(OxIx,VxDx,lol);
AA[0][0] = mass*Ox[0][0]; AA[0][1] = mass*Ox[0][1];
AA[0][2] = mass*Ox[0][2];
AA[1][0] = mass*Ox[1][0]; AA[1][1] = mass*Ox[1][1];
AA[1][2] = mass*Ox[1][2];
AA[2][0] = mass*Ox[2][0]; AA[2][1] = mass*Ox[2][1];
AA[2][2] = mass*Ox[2][2];
AA[3][0] = OxDx[0][0];AA[3][1] = OxDx[0][1];AA[3][2] = OxDx[0][2];
AA[4][0] = OxDx[1][0];AA[4][1] = OxDx[1][1];AA[4][2] = OxDx[1][2];
AA[5][0] = OxDx[2][0];AA[5][1] = OxDx[2][1];AA[5][2] = OxDx[2][2];
AA[0][3] = -OxDx[0][0];AA[0][4] = -OxDx[0][1];AA[0][5] = -OxDx[0][2];
AA[1][3] = -OxDx[1][0];AA[1][4] = -OxDx[1][1];AA[1][5] = -OxDx[1][2];
AA[2][3] = -OxDx[2][0];AA[2][4] = -OxDx[2][1];AA[2][5] = -OxDx[2][2];
AA[3][3] = lol[0][0];AA[3][4] = lol[0][1];AA[3][5] = lol[0][2];
AA[4][3] = lol[1][0];AA[4][4] = lol[1][1];AA[4][5] = lol[1][2];
AA[5][3] = lol[2][0];AA[5][4] = lol[2][1];AA[5][5] = lol[2][2];
statex[0][0] = u_body;
statex[1][0] = v_body;
statex[2][0] = w_body;
statex[3][0] = p_body;
statex[4][0] = q_body;
statex[5][0] = r_body;
mult_matrixvector6(AA,statex,AAx);
AAt[0][0] = Xbar-AAx[0][0]-mass*g*sin(theta);
AAt[1][0] = Ybar-AAx[1][0]+mass*g*cos(theta)*sin(phi);
AAt[2][0] = Zbar-AAx[2][0]+mass*g*cos(theta)*cos(phi);
AAt[3][0] = Lbar-AAx[3][0]-mass*g*(-cos(theta)*cos(phi)*delta_y
+cos(theta)*sin(phi)*delta_z);
AAt[4][0] = Mbar-AAx[4][0]-mass*g*(cos(theta)*cos(phi)*delta_x
+sin(theta)*delta_z);
AAt[5][0] = Nbar-AAx[5][0]-mass*g*(-cos(theta)*sin(phi)*delta_x
-sin(theta)*delta_y);
BB[0][0] = mass; BB[0][1] = 0; BB[0][2] = 0;
BB[1][0] = 0; BB[1][1] = mass; BB[1][2] = 0;
BB[2][0] = 0; BB[2][1] = 0; BB[2][2] = mass;
BB[3][0] = Dx[0][0];BB[3][1] = Dx[0][1];BB[3][2] = Dx[0][2];
BB[4][0] = Dx[1][0];BB[4][1] = Dx[1][1];BB[4][2] = Dx[1][2];
BB[5][0] = Dx[2][0];BB[5][1] = Dx[2][1];BB[5][2] = Dx[2][2];
BB[0][3] = -Dx[0][0];BB[0][4] = -Dx[0][1];BB[0][5] = -Dx[0][2];
BB[1][3] = -Dx[1][0];BB[1][4] = -Dx[1][1];BB[1][5] = -Dx[1][2];
BB[2][3] = -Dx[2][0];BB[2][4] = -Dx[2][1];BB[2][5] = -Dx[2][2];
BB[3][3] = Ix[0][0];BB[3][4] = Ix[0][1];BB[3][5] = Ix[0][2];
BB[4][3] = Ix[1][0];BB[4][4] = Ix[1][1];BB[4][5] = Ix[1][2];
BB[5][3] = Ix[2][0];BB[5][4] = Ix[2][1];BB[5][5] = Ix[2][2];
matrix_inverse6(BB, BB_inv);
mult_matrixvector6(BB_inv,AAt,statex_dot);
u_body_dot = statex_dot[0][0];
v_body_dot = statex_dot[1][0];
w_body_dot = statex_dot[2][0];
p_body_dot = statex_dot[3][0];
q_body_dot = statex_dot[4][0];
r_body_dot = statex_dot[5][0];
/*-------------------------------------------------------------------*/
/* Derivatives */
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*sqrt(u_body * u_body + w_body * w_body));
alpha_dot = (u_body * w_body_dot - w_body * u_body_dot)
/ (u_body * u_body + w_body * w_body);
phi_dot = p_body+tan(theta)*(q_body*sin(phi)+r_body*cos(phi));
theta_dot = q_body*cos(phi)-r_body*sin(phi);
psi_dot = (q_body*sin(phi)+r_body*cos(phi))/cos(theta);
x_earth_dot = cos(psi)*cos(theta) * u_body
+ (cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi)) * v_body
+ (cos(psi)*sin(theta)*cos(phi)+sin(phi)*sin(psi)) * w_body;
y_earth_dot = sin(psi)*cos(theta) * u_body
+ (sin(psi)*sin(theta)*sin(phi)+cos(phi)*cos(psi)) * v_body
+ (sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi)) * w_body;
z_earth_dot = -sin(theta) * u_body
+ cos(theta)*sin(phi) * v_body
+ cos(theta)*cos(phi) * 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
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -