christoph/failnix
1
Fork 0
Code Releases Activity
Files
main
failnix/targets/wasm-tacle/parallel/rosace/thread2/assemblage_includes.c

1213 lines
26 KiB
C
Raw Permalink Blame History

/* ----------------------------------------------------------------------------
SchedMCore - A MultiCore Scheduling Framework
Copyright (C) 2009-2011, ONERA, Toulouse, FRANCE - LIFL, Lille, FRANCE
This file is part of Prelude
Prelude is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation ; either version 2 of
the License, or (at your option) any later version.
Prelude is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this program ; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
USA
---------------------------------------------------------------------------- */
/*
This program is part of the TACLeBench benchmark suite.
Version V 1.9
Name: assemblage_includes.c
Author: unknown
Function:
Source: https://svn.onera.fr/schedmcore/branches/ROSACE_CaseStudy/
c_posix_implementation/
Original name: assemblage_includes.c
Changes: no major functional changes
License: see above
*/
#include "wcclibm.h"
#include "assemblage_includes.h"
#define MATH_COS cosf
#define MATH_SIN sinf
#define pow powf
#define atan atanf
#define sin sinf
#define sqrt sqrtf
/*
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.
*/
/* Period/Frequency of the nodes */
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.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.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;
#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( void )
{
return h_c;
}
/*
The commanded airspeed
*/
REAL_TYPE input_Va_c( void )
{
return Va_c;
}
void output_delta_th_c( REAL_TYPE delta_th_c )
{
}
void output_delta_e_c( REAL_TYPE delta_e_c )
{
}
View Git Blame Copy Permalink