You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1111 lines
24 KiB
1111 lines
24 KiB
#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){
|
|
|
|
}
|