/* ----------------------------------------------------------------------------
   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 )
{

}