📄 f16_dyn_decoupled.c
字号:
/*-----------------------------------------------------------------------*/
/*-----------------------------------------------------------------------*/
/* 6 DOF F-16 FIGHTER AIRCRAFT DYNAMICS */
/*-----------------------------------------------------------------------*/
/*-----------------------------------------------------------------------*/
/* */
/* Based on the F-16 model created by R. S. Russel in */
/* "Nonlinear F-16 simulation using Simulink and Matlab" */
/* 2003 University of Minnesota and on the F-16 model */
/* created by Ying Huo in "Model of F-16 Fighter Aircraft" */
/* */
/* Aerodynamic data and the engine model have been obtained from */
/* "NASA Technical Paper 1538" by Nguyen et al. 1979 */
/* */
/* File "F16_dyn_decoupled.c" */
/* Version 1.0 by E.R. van Oort & L. Sonneveldt */
/* Created with MATLAB 7.6 R2008 */
/* May, 2008 */
/* */
/* Changes wrt to F16_dyn.c: */
/* - decoupled ailerons */
/* - decoupled horizontal stabilizers */
/* - turbulence inputs */
/* - modified quaternion to be more stable */
/* */
/* Notes: */
/* -All units are SI. */
/* -Quaternion transformations are used. */
/* -Flag is used to select between hifi and lofi aerodynamic model. */
/* -Hifi aerodata is obtained from "aerodata/hifi_f16_aerodata.c" */
/* -Lofi aerodata is obtained from "aerodata/lofi_f16_aerodata.c" */
/* -"aerodata/mexndinterp.c" is used for interpolation of the data. */
/* -"aerodata/engine_model.c" contains the engine model. */
/* -"aerodata/ISA_atmos.c" contains the ISA atmosphere model. */
/* */
/*-----------------------------------------------------------------------*/
/*-----------------------------------------------------------------------*/
/* Used variables: */
/* */
/* Input variables: (port 1) */
/* dth throttle setting (between 0 and 1) [-] */
/* del left elevator deflection [rad] */
/* der right elevator deflection [rad] */
/* dal left aileron deflection [rad] */
/* dar right aileron deflection [rad] */
/* dr rudder deflection [rad] */
/* */
/* Additonal input variables: (port 2) */
/* dlef leading edge flap deflection [rad] */
/* */
/* Fidelity flag: (port 3) */
/* fi_flag hifi/lofi aerodynamic model selection flag [-] */
/* */
/* Wind disturbance input: (port 4) */
/* u_dist turbulence in x_body [m/s] */
/* v_dist y_body [m/s] */
/* w_dist z_body [m/s] */
/* p_dist turbulence around roll axis [rad/s] */
/* q_dist pitch axis [rad/s] */
/* r_dist yaw axis [rad/s] */
/* */
/* State variables: */
/* Vt total airspeed [m/s] */
/* beta angle of sideslip [rad] */
/* alpha angle of attack [rad] */
/* q0 quaternion component [-] */
/* q1 quaternion component [-] */
/* q2 quaternion component [-] */
/* q3 quaternion component [-] */
/* p_body roll angular rate [rad/s] */
/* q_body pitch angular rate [rad/s] */
/* r_body yaw angular rate [rad/s] */
/* x_earth x position [m] */
/* y_earth y position [m] */
/* z_earth z position [m] */
/* pow power level (0-100%) [-] [-] */
/* */
/* State derivatives: */
/* Vt_dot rate of change in total airspeed [m/s^2] */
/* beta_dot rate of change in angle of sideslip [rad/s] */
/* alpha_dot rate of change in angle of attack [rad/s] */
/* q0_dot rate of change in quaternion component [1/s] */
/* q1_dot rate of change in quaternion component [1/s] */
/* q2_dot rate of change in quaternion component [1/s] */
/* q3_dot rate of change in quaternion component [1/s] */
/* p_body_dot rate of change in roll angular rate [rad/s^2]*/
/* q_body_dot rate of change in pitch angular rate [rad/s^2]*/
/* r_body_dot rate of change in yaw angular rate [rad/s^2]*/
/* x_earth_dot rate of change in x position [m/s] */
/* y_earth_dot rate of change in y position [m/s] */
/* z_earth_dot rate of change in z position [m/s] */
/* pow_dot rate of change in power level [1/s] */
/* */
/* Output variables: */
/* Vt total airspeed [m/s] */
/* beta angle of sideslip [rad] */
/* alpha angle of attack [rad] */
/* q0 quaternion component [-] */
/* q1 quaternion component [-] */
/* q2 quaternion component [-] */
/* q3 quaternion component [-] */
/* p_body roll angular rate [rad/s] */
/* q_body pitch angular rate [rad/s] */
/* r_body yaw angular rate [rad/s] */
/* x_earth x position [m] */
/* y_earth y position [m] */
/* z_earth z position [m] */
/* pow power level (0-100%) [-] [-] */
/* */
/* Real work vector: (Persistant memory) */
/* C1 coefficients used in moment equations [-] */
/* C2 */
/* C3 */
/* C4 */
/* C5 */
/* C6 */
/* C7 */
/* C8 */
/* C9 */
/* Xbar total force in body fixed x-axis [N] */
/* Ybar total force in body fixed y-axis [N] */
/* Zbar total force in body fixed z-axis [N] */
/* Lbar total moment in body fixed x-axis [Nm] */
/* Mbar total moment in body fixed y-axis [Nm] */
/* Nbar total moment in body fixed z-axis [Nm] */
/* u_body velocity in body fixed x-axis [m/s] */
/* v_body velocity in body fixed y-axis [m/s] */
/* w_body velocity in body fixed z-axis [m/s] */
/* u_body_dot rate of change in velocity x-axis [m/s^2] */
/* v_body_dot rate of change in velocity y-axis [m/s^2] */
/* w_body_dot rate of change in velocity z-axis [m/s^2] */
/* qbar dynamic pressure [N/m] */
/* Mach Mach number [-] */
/* Thrust Total engine thrust [-] */
/* */
/*-----------------------------------------------------------------------*/
#define S_FUNCTION_NAME F16_dyn_decoupled
#define S_FUNCTION_LEVEL 2
/* include files */
#include <math.h>
#include "simstruc.h"
#include "aerodata/mexndinterp.c"
#include "aerodata/hifi_f16_aerodata.c" /* hifi lookup tables */
#include "aerodata/lofi_f16_aerodata.c" /* lofi lookup tables */
#include "aerodata/ISA_atmos.c" /* ISA atmosphere model */
#include "aerodata/engine_model.c" /*engine model */
/* input port 1: control inputs */
#define dth (*u[0])
#define del (*u[1])
#define der (*u[2])
#define dal (*u[3])
#define dar (*u[4])
#define dr (*u[5])
/* input port 2: leading edge flap deflection */
#define dlef (*dlef_in[0])
/* input port 3: fidelity flag, 0 = lofi model, 1 = hifi model */
#define fi_flag (*fi_flag_in[0])
/* input port 4: turbulence inputs */
#define u_dist (*dist[0])
#define v_dist (*dist[1])
#define w_dist (*dist[2])
#define p_dist (*dist[3])
#define q_dist (*dist[4])
#define r_dist (*dist[5])
/* 14 States */
#define Vt_st x[0]
#define beta_st x[1]
#define alpha_st x[2]
#define q0 x[3]
#define q1 x[4]
#define q2 x[5]
#define q3 x[6]
#define p_body x[7]
#define q_body x[8]
#define r_body x[9]
#define x_earth x[10]
#define y_earth x[11]
#define z_earth x[12]
#define pow x[13]
/* State derivatives */
#define Vt_dot dx[0]
#define beta_dot dx[1]
#define alpha_dot dx[2]
#define q0_dot dx[3]
#define q1_dot dx[4]
#define q2_dot dx[5]
#define q3_dot dx[6]
#define p_body_dot dx[7]
#define q_body_dot dx[8]
#define r_body_dot dx[9]
#define x_earth_dot dx[10]
#define y_earth_dot dx[11]
#define z_earth_dot dx[12]
#define pow_dot dx[13]
/* Work Variables */
#define C1 ssGetRWork(S)[0]
#define C2 ssGetRWork(S)[1]
#define C3 ssGetRWork(S)[2]
#define C4 ssGetRWork(S)[3]
#define C5 ssGetRWork(S)[4]
#define C6 ssGetRWork(S)[5]
#define C7 ssGetRWork(S)[6]
#define C8 ssGetRWork(S)[7]
#define C9 ssGetRWork(S)[8]
#define Xbar ssGetRWork(S)[9]
#define Ybar ssGetRWork(S)[10]
#define Zbar ssGetRWork(S)[11]
#define Lbar ssGetRWork(S)[12]
#define Mbar ssGetRWork(S)[13]
#define Nbar ssGetRWork(S)[14]
#define u_body ssGetRWork(S)[15]
#define v_body ssGetRWork(S)[16]
#define w_body ssGetRWork(S)[17]
#define u_body_dot ssGetRWork(S)[18]
#define v_body_dot ssGetRWork(S)[19]
#define w_body_dot ssGetRWork(S)[20]
#define qbar ssGetRWork(S)[21]
#define Mach ssGetRWork(S)[22]
#define Thrust ssGetRWork(S)[23]
/* Aircraft Parameters */
#define mass 9295.44 /*assumed fixed*/
#define Ixx 12874.8
#define Iyy 75673.6
#define Izz 85552.1
#define Ixz 1331.4
#define Sref 27.87
#define bref 9.144
#define cref 3.45
#define xcg 0.3
#define xcgr 0.35
#define heng 216.9 /* engine angular momentum, assumed fixed */
#define y_stab_cg 0.2 /* educated guess of distance of center of pres.
/* Additional parameters */
#define rtd 57.29577951
#define dtr 0.017453293
#define Pi 3.141592654
/*=============================*/
/* Function: mdlInitalizeSizes */
/*=============================*/
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumSFcnParams(S, 1); /* Number of expected parameters */
ssSetNumContStates(S, 14);
ssSetNumDiscStates(S, 0);
ssSetNumInputPorts(S, 4);
ssSetInputPortWidth(S, 0, 6);
ssSetInputPortWidth(S, 1, 1);
ssSetInputPortWidth(S, 2, 1);
ssSetInputPortWidth(S, 3, 6);
/* ssSetInputPortDirectFeedThrough(S, 1, 1); */
ssSetNumOutputPorts(S, 1);
ssSetOutputPortWidth(S, 0, 14);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 23);
ssSetNumIWork(S, 0);
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
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -