Subversion Repositories FlightCtrl

/*
Copyright 2008, by Michael Walter

All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program 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, see <http://www.gnu.org/licenses/>.

Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/

/*****************************************************************************
  INCLUDES
**************************************************************************** */
#include "matmatrix.h"

/*****************************************************************************
  (SYMBOLIC) CONSTANTS
*****************************************************************************/

/*****************************************************************************
  VARIABLES
*****************************************************************************/
extern int sollGier;

/* *************************************************************************** */
/* Uses the Compass to Estimate Theta, Phi, Psi  */
//#define USE_Extended_MM3_Measurement_Model
/* *************************************************************************** */

/* kalman filter model extensions change number of state variables */
#define KAFI_DIM_X   (3)    /* number of rows of state vector */

#ifdef USE_Extended_MM3_Measurement_Model
    #define KAFI_DIM_Y   (6)    /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/
#else
    #define KAFI_DIM_Y   (4)   /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/
#endif

#define KAFI_DIM_U   (3)    /* number of rows of control vector */

#ifdef MELEXIS_GYRO
    #define fCycleTime 12.5F /* Use Different Cycle Times to account for Runtime / Gain Differences */
#else
#ifdef USE_Extended_MM3_Measurement_Model
    #define fCycleTime 16.0F
#else
    //#define fCycleTime 12.5F
    #define fCycleTime 16.0F
    #endif
#endif

/* Predict the Measurent */
void trPredictMeasurement(void);
/* Update the Jacobi Matrix */
void trUpdateJacobiMatrix(void);
/* Extract measurements and store them in vector matY.
   Measurement validity is indicated by fYValid[] */
void trMeasure(void);
/* Innovation: calculate Xd, and Pd  */
void trInnovate(void);
/* Update transition matrix Phi  */
void trUpdatePhi(void);
/* Update control matrix B */
void trUpdateBU(void);
/* Predict new state Xs and Ps */
void KAFIPrediction(void);
/* Main Kalman Filter Loop */
void FlightAttitudeEstimation(void);
/* Setup the Kalman Filter Parameter */
void Kafi_Init(void);
/* Not done yet */
void trEstimateVelocity(void);
/* Limit Angles to [-2Pi;...;+2Pi] */
void trLimitAngles(void);

typedef struct
{
f32_t Phi;
f32_t Theta;
f32_t Psi;
i32_t iPhi10;
i32_t iTheta10;
i32_t iPsi10;
f32_t dPhi;
f32_t dTheta;
f32_t dPsi;
f32_t du;
f32_t dv;
f32_t dw;
f32_t X;
f32_t Y;
f32_t Z;
} status_t;

status_t status;

/*typedef*/ enum
{
   _p = 0,
   _q,
   _r,
   c_control_variables                       /* number of control variables */
} /*trControlVariables_e*/;


#ifdef USE_Extended_MM3_Measurement_Model
/*typedef*/ enum
{
  _ax = 0,
  _ay,
  _az,
  _mx,
  _my,
  _mz,
  c_observation_variables                       /* number of observation variables */
} /*trObservationVariables_e*/;
#else
/*typedef*/ enum
{
  _ax = 0,
  _ay,
  _az,
  _compass,
  c_observation_variables                       /* number of observation variables */
} /*trObservationVariables_e*/;
#endif

/*typedef*/ enum
{
   _Phi = 0,
   _Theta,
   _Psi,
   //_du,
   //_dv,
   //_dw,
   c_state_variables                       /* number of state variables */
} /*trStateVariables_e*/;