|
|
- #include <math.h>
-
- #include "assemblage_includes.h"
- #ifdef USE_FLOAT
- #define MATH_COS cosf
- #define MATH_SIN sinf
- #else
- #define MATH_COS cos
- #define MATH_SIN sin
- #endif
- /*
- * The following include file is generated by the Prelude compiler
- * Theoretically we should include main node specific include
- * i.e. assemblage_vX.h but we know that every main node
- * assemblage, assemblage_v2, assemblage_v3, etc... share the
- * very same data type.
- */
- //#include "assemblage.h"
-
- /* Period/Frequency of the nodes */
- // const REAL_TYPE Ts_h = 1.0/50.0;
- // const REAL_TYPE Ts_K1 = 1.0/50.0;
- // const REAL_TYPE Ts_K2 = 1.0/50.0;
- // const REAL_TYPE Ts_f_Va = 1.0/100.0;
- // const REAL_TYPE Ts_f_Vz = 1.0/100.0;
- // const REAL_TYPE Ts_f_q = 1.0/100.0;
- // const REAL_TYPE Ts_f_az = 1.0/100.0;
- // const REAL_TYPE Ts_f_h = 1.0/100.0;
- const REAL_TYPE dt = 1.0f/200.0;
- const REAL_TYPE dt_de = 1.0/200.0;
- const REAL_TYPE dt_dx = 1.0/200.0;
-
- /* Controller parameters */
- /* Altitude hold */
- const REAL_TYPE Kp_h = 0.1014048;
- const REAL_TYPE Ki_h = 0.0048288;
- const REAL_TYPE h_switch = 50.0;
-
-
- // Setpoint commands
- REAL_TYPE Vz_c = -2.5;
- REAL_TYPE Va_c = 0.0;
- REAL_TYPE h_c = 10000;
-
- /* Va Speed controller */
-
- // const REAL_TYPE K1_intVa = 0.06018;
- // const REAL_TYPE K1_Va = -0.53115;
- // const REAL_TYPE K1_Vz = -0.08956;
- // const REAL_TYPE K1_q = 24.44890;
-
- const REAL_TYPE K1_intVa = 0.049802610664357;
- const REAL_TYPE K1_Va = -0.486813084356079;
- const REAL_TYPE K1_Vz = -0.077603095495388;
- const REAL_TYPE K1_q = 21.692383376322041;
-
- /* Vz Speed controller */
-
- // const REAL_TYPE K2_intVz = 0.0006545;
- // const REAL_TYPE K2_Vz = -0.0031107;
- // const REAL_TYPE K2_q = 0.4490749;
- // const REAL_TYPE K2_az = -0.0002038;
-
- const REAL_TYPE K2_intVz = 0.000627342822264;
- const REAL_TYPE K2_Vz = -0.003252836726554;
- const REAL_TYPE K2_q = 0.376071446897134;
- const REAL_TYPE K2_az = -0.001566907423747;
-
-
- /* Trimming parameters */
- const REAL_TYPE h_eq = 10000.0;
- const REAL_TYPE Va_eq = 230.0;
- const REAL_TYPE Vz_eq = 0.0;
- const REAL_TYPE alpha_eq = 0.026485847681737;
- const REAL_TYPE theta_eq = 0.026485847681737;
-
- /* Atmosphere parameters */
- const REAL_TYPE rho0 = 1.225;
- const REAL_TYPE g0 = 9.80665;
- const REAL_TYPE T0_0 = 288.15;
- const REAL_TYPE T0_h = -0.0065;
- const REAL_TYPE Rs = 287.05;
-
- /* Aircraft parameters */
- const REAL_TYPE masse = 57837.5;
- const REAL_TYPE I_y = 3781272.0;
- const REAL_TYPE S = 122.6;
- const REAL_TYPE cbar = 4.29;
- const REAL_TYPE CD_0 = 0.016;
- const REAL_TYPE CD_alpha = 2.5;
- const REAL_TYPE CD_deltae = 0.05;
- const REAL_TYPE CL_alpha = 5.5;
- const REAL_TYPE CL_deltae = 0.193;
- const REAL_TYPE alpha_0 = -0.05;
- const REAL_TYPE Cm_0 = 0.04;
- const REAL_TYPE Cm_alpha = -0.83;
- const REAL_TYPE Cm_deltae = -1.5;
- const REAL_TYPE Cm_q = -30;
-
- /* in-memory buffer for traces */
- //REAL_TYPE sample[SPL_SIZE][NBMAX_SAMPLE];
- //static unsigned long instant = 0;
- //static unsigned long sample_instant=0;
-
- #define FMTFLOAT "%5.15f"
-
- /* Va filter 100 Hz */
- REAL_TYPE
- Va_filter_100(REAL_TYPE Va) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 100 Hz coefficients */
- static REAL_TYPE a0 = 0.956543675476034;
- static REAL_TYPE a1 = -1.955578398054313;
- static REAL_TYPE b0 = 0.000479064865372430;
- static REAL_TYPE b1 = 0.000486212556348925;
-
- if (debut) {
- debut = 0;
- x1 = Va_eq * (1.0 + a1 - b1);
- x2 = Va_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Va;
- x2_tmp = x1 - a1 * x2 + b1 * Va;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Va filter 100 Hz */
-
- /* Va filter 50 Hz */
- REAL_TYPE
- Va_filter_50(REAL_TYPE Va) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 50 Hz coefficients */
- static REAL_TYPE a0 = 0.914975803093201;
- static REAL_TYPE a1 = -1.911199519984605;
- static REAL_TYPE b0 = 0.001860178914816;
- static REAL_TYPE b1 = 0.001916104193780;
-
- if (debut) {
- debut = 0;
- x1 = Va_eq * (1.0 + a1 - b1);
- x2 = Va_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Va;
- x2_tmp = x1 - a1 * x2 + b1 * Va;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Va filter 50 Hz */
-
- /* Va filter 33 Hz */
- REAL_TYPE
- Va_filter_33(REAL_TYPE Va) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 33 Hz coefficients */
- static REAL_TYPE a0 = 0.874036784828483;
- static REAL_TYPE a1 = -1.865563793814790;
- static REAL_TYPE b0 = 0.004141433623051;
- static REAL_TYPE b1 = 0.004331557390642;
-
- if (debut) {
- debut = 0;
- x1 = Va_eq * (1.0 + a1 - b1);
- x2 = Va_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Va;
- x2_tmp = x1 - a1 * x2 + b1 * Va;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Va filter 33 Hz */
-
- /* Va filter 25 Hz */
- REAL_TYPE
- Va_filter_25(REAL_TYPE Va) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 25 Hz coefficients */
- static REAL_TYPE a0 = 0.837180720246048;
- static REAL_TYPE a1 = -1.822731999002980;
- static REAL_TYPE b0 = 0.007010380719078;
- static REAL_TYPE b1 = 0.007438340523990;
-
- if (debut) {
- debut = 0;
- x1 = Va_eq * (1.0 + a1 - b1);
- x2 = Va_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Va;
- x2_tmp = x1 - a1 * x2 + b1 * Va;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Va filter 25 Hz */
-
-
- /* Vz filter 100 Hz */
- REAL_TYPE
- Vz_filter_100(REAL_TYPE Vz) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 100 Hz coefficients */
- static REAL_TYPE a0 = 0.956543675476034;
- static REAL_TYPE a1 = -1.955578398054313;
- static REAL_TYPE b0 = 0.000479064865372430;
- static REAL_TYPE b1 = 0.000486212556348925;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Vz;
- x2_tmp = x1 - a1 * x2 + b1 * Vz;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Vz filter 100 Hz */
-
- /* Vz filter 50 Hz */
- REAL_TYPE
- Vz_filter_50(REAL_TYPE Vz) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0, x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 50 Hz coefficients */
- static REAL_TYPE a0 = 0.914975803093201;
- static REAL_TYPE a1 = -1.911199519984605;
- static REAL_TYPE b0 = 0.001860178914816;
- static REAL_TYPE b1 = 0.001916104193780;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Vz;
- x2_tmp = x1 - a1 * x2 + b1 * Vz;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Vz filter 50 Hz */
-
- /* Vz filter 33 Hz */
- REAL_TYPE
- Vz_filter_33(REAL_TYPE Vz) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0, x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 33 Hz coefficients */
- static REAL_TYPE a0 = 0.874036784828483;
- static REAL_TYPE a1 = -1.865563793814790;
- static REAL_TYPE b0 = 0.004141433623051;
- static REAL_TYPE b1 = 0.004331557390642;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Vz;
- x2_tmp = x1 - a1 * x2 + b1 * Vz;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Vz filter 33 Hz */
-
- /* Vz filter 25 Hz */
- REAL_TYPE
- Vz_filter_25(REAL_TYPE Vz) {
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 25 Hz coefficients */
- static REAL_TYPE a0 = 0.837180720246048;
- static REAL_TYPE a1 = -1.822731999002980;
- static REAL_TYPE b0 = 0.007010380719078;
- static REAL_TYPE b1 = 0.007438340523990;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * Vz;
- x2_tmp = x1 - a1 * x2 + b1 * Vz;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of Vz filter 25 Hz */
-
- /* q filter 100 Hz */
- REAL_TYPE
- q_filter_100(REAL_TYPE q){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 100 Hz coefficients */
- static REAL_TYPE a0 = 0.766000101841272;
- static REAL_TYPE a1 = -1.734903205885821;
- static REAL_TYPE b0 = 0.014857648981438;
- static REAL_TYPE b1 = 0.016239246974013;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * q;
- x2_tmp = x1 - a1 * x2 + b1 * q;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of q filter 100 Hz */
-
- /* q filter 50 Hz */
- REAL_TYPE
- q_filter_50(REAL_TYPE q){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 50 Hz coefficients */
- static REAL_TYPE a0 = 0.586756156020839;
- static REAL_TYPE a1 = -1.477888930110354;
- static REAL_TYPE b0 = 0.049596808318647;
- static REAL_TYPE b1 = 0.059270417591839;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * q;
- x2_tmp = x1 - a1 * x2 + b1 * q;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of q filter 50 Hz */
-
- /* q filter 33 Hz */
- REAL_TYPE
- q_filter_33(REAL_TYPE q){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 33 Hz coefficients */
- static REAL_TYPE a0 = 0.445839214374383;
- static REAL_TYPE a1 = -1.227970132817902;
- static REAL_TYPE b0 = 0.094268996251840;
- static REAL_TYPE b1 = 0.123600085304640;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * q;
- x2_tmp = x1 - a1 * x2 + b1 * q;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of q filter 33 Hz */
-
- /* q filter 25 Hz */
- REAL_TYPE
- q_filter_25(REAL_TYPE q){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 25 Hz coefficients */
- static REAL_TYPE a0 = 0.344282786628352;
- static REAL_TYPE a1 = -1.010643377701049;
- static REAL_TYPE b0 = 0.137177088974822;
- static REAL_TYPE b1 = 0.196462319952482;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * q;
- x2_tmp = x1 - a1 * x2 + b1 * q;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of q filter 25 Hz */
-
- /* az filter 100 Hz */
- REAL_TYPE
- az_filter_100(REAL_TYPE az){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 100 Hz coefficient */
- static REAL_TYPE a0 = 0.411240701442774;
- static REAL_TYPE a1 = -1.158045899830964;
- static REAL_TYPE b0 = 0.107849979167580;
- static REAL_TYPE b1 = 0.145344822444230;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * az;
- x2_tmp = x1 - a1 * x2 + b1 * az;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of az filter 100 Hz */
-
- /* az filter 50 Hz */
- REAL_TYPE
- az_filter_50(REAL_TYPE az){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 50 Hz coefficients */
- static REAL_TYPE a0 = 0.169118914523145;
- static REAL_TYPE a1 = -0.518588903229759;
- static REAL_TYPE b0 = 0.229019233988375;
- static REAL_TYPE b1 = 0.421510777305010;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * az;
- x2_tmp = x1 - a1 * x2 + b1 * az;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of az filter 50 Hz */
-
- /* az filter 33 Hz */
- REAL_TYPE
- az_filter_33(REAL_TYPE az){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 33 Hz coefficients */
- static REAL_TYPE a0 = 0.067700864731348;
- static REAL_TYPE a1 = -0.115832026705568;
- static REAL_TYPE b0 = 0.263451167882487;
- static REAL_TYPE b1 = 0.688417670143293;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * az;
- x2_tmp = x1 - a1 * x2 + b1 * az;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of az filter 33 Hz */
-
- /* az filter 25 Hz */
- REAL_TYPE
- az_filter_25(REAL_TYPE az){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 25 Hz coefficients */
- static REAL_TYPE a0 = 0.028601207249487;
- static REAL_TYPE a1 = 0.069303378493245;
- static REAL_TYPE b0 = 0.228783762747218;
- static REAL_TYPE b1 = 0.869120822995514;
-
- if (debut) {
- debut = 0;
- x1 = 0.0;
- x2 = 0.0;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * az;
- x2_tmp = x1 - a1 * x2 + b1 * az;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of az filter 25 Hz */
-
- /* h filter 100 Hz*/
- REAL_TYPE
- h_filter_100(REAL_TYPE h){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 100 Hz coefficients */
- static REAL_TYPE a0 = 0.766000101841272;
- static REAL_TYPE a1 = -1.734903205885821;
- static REAL_TYPE b0 = 0.014857648981438;
- static REAL_TYPE b1 = 0.016239246974013;
-
- if (debut) {
- debut = 0;
- x1 = h_eq * (1.0 + a1 - b1);
- x2 = h_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * h;
- x2_tmp = x1 - a1 * x2 + b1 * h;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of h filter 100 Hz */
-
- /* h filter 50 Hz*/
- REAL_TYPE
- h_filter_50(REAL_TYPE h){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 50 Hz coefficients */
- static REAL_TYPE a0 = 0.586756156020839;
- static REAL_TYPE a1 = -1.477888930110354;
- static REAL_TYPE b0 = 0.049596808318647;
- static REAL_TYPE b1 = 0.059270417591839;
-
- if (debut) {
- debut = 0;
- x1 = h_eq * (1.0 + a1 - b1);
- x2 = h_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * h;
- x2_tmp = x1 - a1 * x2 + b1 * h;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of h filter 50 Hz */
-
- /* h filter 33 Hz*/
- REAL_TYPE
- h_filter_33(REAL_TYPE h){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 33 Hz coefficients */
- static REAL_TYPE a0 = 0.445839214374383;
- static REAL_TYPE a1 = -1.227970132817902;
- static REAL_TYPE b0 = 0.094268996251840;
- static REAL_TYPE b1 = 0.123600085304640;
-
- if (debut) {
- debut = 0;
- x1 = h_eq * (1.0 + a1 - b1);
- x2 = h_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * h;
- x2_tmp = x1 - a1 * x2 + b1 * h;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of h filter 33 Hz */
-
- /* h filter 25 Hz*/
- REAL_TYPE
- h_filter_25(REAL_TYPE h){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE x1 = 0.0;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_tmp = 0.0;
- static REAL_TYPE x2_tmp = 0.0;
- static unsigned short debut = 1;
- /* 25 Hz coefficients */
- static REAL_TYPE a0 = 0.344282786628352;
- static REAL_TYPE a1 = -1.010643377701049;
- static REAL_TYPE b0 = 0.137177088974822;
- static REAL_TYPE b1 = 0.196462319952482; /**/
-
- if (debut) {
- debut = 0;
- x1 = h_eq * (1.0 + a1 - b1);
- x2 = h_eq;
- }
- // Output
- y = x2;
- // State
- x1_tmp = - a0 * x2 + b0 * h;
- x2_tmp = x1 - a1 * x2 + b1 * h;
- // Update
- x1 = x1_tmp;
- x2 = x2_tmp;
-
- return y;
- } /* end of h filter 25 Hz */
-
-
- /* Altitude hold controller 50 Hz */
-
- REAL_TYPE
- altitude_hold_50(REAL_TYPE h_f, REAL_TYPE h_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_h = 1.0/50.0;
- static REAL_TYPE integrator = 532.2730285;
-
- if ((h_f - h_c) < -50) {
- // Output
- y = Vz_c;
- }
- else if ((h_f - h_c) > 50) {
- // Output
- y = -Vz_c;
- }
- else {
- // Output
- y = Kp_h * (h_f - h_c) + Ki_h * integrator;
- // State
- integrator += Ts_h * (h_f - h_c);
- }
-
- return y;
- }
-
- /* Altitude hold controller 33 Hz */
-
- REAL_TYPE
- altitude_hold_33(REAL_TYPE h_f, REAL_TYPE h_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_h = 1.0/33.0;
- static REAL_TYPE integrator = 532.2730285;
-
- if ((h_f - h_c) < -50) {
- // Output
- y = Vz_c;
- }
- else if ((h_f - h_c) > 50) {
- // Output
- y = -Vz_c;
- }
- else {
- // Output
- y = Kp_h * (h_f - h_c) + Ki_h * integrator;
- // State
- integrator += Ts_h * (h_f - h_c);
- }
-
- return y;
- }
-
- /* Altitude hold controller 25 Hz */
-
- REAL_TYPE
- altitude_hold_25(REAL_TYPE h_f, REAL_TYPE h_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_h = 1.0/25.0;
- static REAL_TYPE integrator = 532.2730285;
-
- if ((h_f - h_c) < -50) {
- // Output
- y = Vz_c;
- }
- else if ((h_f - h_c) > 50) {
- // Output
- y = -Vz_c;
- }
- else {
- // Output
- y = Kp_h * (h_f - h_c) + Ki_h * integrator;
- // State
- integrator += Ts_h * (h_f - h_c);
- }
-
- return y;
- }
-
- /* Altitude hold controller 10 Hz */
-
- REAL_TYPE
- altitude_hold_10(REAL_TYPE h_f, REAL_TYPE h_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_h = 1.0/10.0;
- static REAL_TYPE integrator = 532.2730285;
-
- if ((h_f - h_c) < -50) {
- // Output
- y = Vz_c;
- }
- else if ((h_f - h_c) > 50) {
- // Output
- y = -Vz_c;
- }
- else {
- // Output
- y = Kp_h * (h_f - h_c) + Ki_h * integrator;
- // State
- integrator += Ts_h * (h_f - h_c);
- }
-
- return y;
- }
-
- /* Va Speed controller 50 Hz */
- REAL_TYPE
- Va_control_50(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K1 = 1.0/50.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
- // State
- integrator += Ts_K1 * (Va_c - Va_f + Va_eq);
-
- return y;
- }
-
- /* Va Speed controller 33 Hz */
- REAL_TYPE
- Va_control_33(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K1 = 1.0/33.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
- // State
- integrator += Ts_K1 * (Va_c - Va_f + Va_eq);
-
- return y;
- }
-
- /* Va Speed controller 25 Hz */
- REAL_TYPE
- Va_control_25(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K1 = 1.0/25.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
- // State
- integrator += Ts_K1 * (Va_c - Va_f + Va_eq);
-
- return y;
- }
-
- /* Va Speed controller 10 Hz */
- REAL_TYPE
- Va_control_10(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K1 = 1.0/10.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
- // State
- integrator += Ts_K1 * (Va_c - Va_f + Va_eq);
-
- return y;
- }
-
- /* Vz Speed controller 50 Hz */
- REAL_TYPE
- Vz_control_50(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K2 = 1.0/50.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
- // State
- integrator += Ts_K2 * (Vz_c - Vz_f);
-
- return y;
- }
-
- /* Vz Speed controller 33 Hz */
- REAL_TYPE
- Vz_control_33(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K2 = 1.0/33.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
- // State
- integrator += Ts_K2 * (Vz_c - Vz_f);
-
- return y;
- }
-
- /* Vz Speed controller 25 Hz */
- REAL_TYPE
- Vz_control_25(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K2 = 1.0/25.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
- // State
- integrator += Ts_K2 * (Vz_c - Vz_f);
-
- return y;
- }
-
- /* Vz Speed controller 10 Hz */
- REAL_TYPE
- Vz_control_10(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
- static REAL_TYPE y = 0.0;
- static REAL_TYPE Ts_K2 = 1.0/10.0;
- static REAL_TYPE integrator = 0.0;
-
- // Output
- y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
- // State
- integrator += Ts_K2 * (Vz_c - Vz_f);
-
- return y;
- }
-
- /* Engine */
- REAL_TYPE
- engine(REAL_TYPE delta_th_c) {
-
- static REAL_TYPE y = delta_th_eq;
- static REAL_TYPE x1 = delta_th_eq;
- static REAL_TYPE x1_dot = 0.0;
- static REAL_TYPE tau = 0.75;
-
- // Output
- y = 26350.0 * x1;
- // State Equation
- x1_dot = -tau * x1 + tau * delta_th_c;
- // Update State
- x1 += dt_dx * x1_dot;
-
- return y;
- }
-
- /* Elevator */
- REAL_TYPE
- elevator(REAL_TYPE delta_e_c) {
-
- static REAL_TYPE y = delta_e_eq;
- static REAL_TYPE x1 = delta_e_eq;
- static REAL_TYPE x2 = 0.0;
- static REAL_TYPE x1_dot = 0.0;
- static REAL_TYPE x2_dot = 0.0;
- static REAL_TYPE omega = 25.0;
- static REAL_TYPE xi = 0.85;
-
- // Output
- y = x1;
- // State Equation
- x1_dot = x2;
- x2_dot = -omega * omega * x1 - 2.0 * xi * omega * x2 + omega * omega * delta_e_c;
- // Update State
- x1 += dt_de * x1_dot;
- x2 += dt_de * x2_dot;
-
- return y;
- }
-
-
- /* Flight dynamics */
- void aircraft_dynamics (REAL_TYPE delta_e, REAL_TYPE T, struct aircraft_dynamics_outs_t *outputs){
-
- static int debut = 1;
-
- static REAL_TYPE u = 0.0;
- static REAL_TYPE w = 0.0;
- static REAL_TYPE q = 0.0;
- static REAL_TYPE theta = 0.0;
- static REAL_TYPE h = 0.0;
- static REAL_TYPE u_dot = 0.0;
- static REAL_TYPE w_dot = 0.0;
- static REAL_TYPE q_dot = 0.0;
- static REAL_TYPE theta_dot = 0.0;
- static REAL_TYPE h_dot = 0.0;
-
- static REAL_TYPE CD = 0.0;
- static REAL_TYPE CL = 0.0;
- static REAL_TYPE Cm = 0.0;
-
- static REAL_TYPE Xa = 0.0;
- static REAL_TYPE Za = 0.0;
- static REAL_TYPE Ma = 0.0;
-
- static REAL_TYPE alpha = 0.0;
- static REAL_TYPE qbar = 0.0;
- static REAL_TYPE V = 0.0;
- static REAL_TYPE rho = 0.0;
-
- if (debut) {
- debut = 0;
- u = Va_eq * MATH_COS(theta_eq);
- w = Va_eq * MATH_SIN(theta_eq);
- q = 0.0;
- theta = theta_eq;
- h = h_eq;
- }
-
- rho = rho0 * pow(1.0 + T0_h / T0_0 * h,- g0 / (Rs * T0_h) - 1.0);
- alpha = atan(w/u);
- V = sqrt(u * u + w * w);
- qbar = 0.5 * rho * V * V;
- CL = CL_deltae * delta_e + CL_alpha * (alpha - alpha_0);
- CD = CD_0 + CD_deltae * delta_e + CD_alpha * (alpha - alpha_0) * (alpha - alpha_0);
- Cm = Cm_0 + Cm_deltae * delta_e + Cm_alpha * alpha + 0.5 * Cm_q * q * cbar / V;
- Xa = - qbar * S * (CD * MATH_COS(alpha) - CL * MATH_SIN(alpha));
- Za = - qbar * S * (CD * MATH_SIN(alpha) + CL * MATH_COS(alpha));
- Ma = qbar * cbar * S * Cm;
-
- // Output
- outputs -> Va = V;
- outputs -> Vz = w * MATH_COS(theta) - u * MATH_SIN(theta);
- outputs -> q = q;
- outputs -> az = g0 * MATH_COS(theta) + Za / masse;
- outputs -> h = h;
- // State Equation
- u_dot = - g0 * sin (theta) - q * w + (Xa + T) / masse;
- w_dot = g0 * MATH_COS (theta) + q * u + Za / masse;
- q_dot = Ma / I_y;
- theta_dot = q;
- h_dot = u * MATH_SIN(theta) - w * MATH_COS(theta);
- // Update State
- u += dt * u_dot;
- w += dt * w_dot;
- q += dt * q_dot;
- theta += dt * theta_dot;
- h += dt * h_dot;
-
-
- static REAL_TYPE Time = 0.0;
-
-
- // instant++;
- Time = Time + dt;
- }
-
- /*
- * The commanded altitude
- */
- REAL_TYPE input_h_c() {
- return h_c;
- }
-
- /*
- * The commanded airspeed
- */
- REAL_TYPE input_Va_c() {
- return Va_c;
- }
-
- void output_delta_th_c(REAL_TYPE delta_th_c){
-
- }
-
- void output_delta_e_c(REAL_TYPE delta_e_c){
-
- }
|