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 "mat.h"

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


/* *************************************************************************** */
/* 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 USE_Extended_MM3_Measurement_Model
        #define fCycleTime 16.0F;
#else
        #define fCycleTime 10.5F;
#endif

/*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*/;


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;


void trUpdatePhi(void);
void trUpdateBU(void);
void trMeasure(void);
void trInnovate(void);

void trResetKalmanFilter(void);
void AttitudeEstimation(void);

int  KAFIInit(kafi_t *pkafi);
void Kafi_Init(void);
void trEstimateVelocity(void);
void trLimitAngles(void);
void trControl(void);