Subversion Repositories FlightCtrl

Rev

Rev 981 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
838 MikeW 1
/*
2
Copyright 2008, by Michael Walter
3
 
4
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
5
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
6
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
7
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
8
License along with this program. If not, see <http://www.gnu.org/licenses/>.
9
 
10
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
11
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
12
unless it is stated otherwise.
13
*/
14
 
15
/*****************************************************************************
16
  INCLUDES
17
**************************************************************************** */
966 MikeW 18
#include "matmatrix.h"
838 MikeW 19
 
20
/*****************************************************************************
21
  (SYMBOLIC) CONSTANTS
22
*****************************************************************************/
23
 
966 MikeW 24
/*****************************************************************************
25
  VARIABLES
26
*****************************************************************************/
27
extern int sollGier;
838 MikeW 28
 
29
/* *************************************************************************** */
30
/* Uses the Compass to Estimate Theta, Phi, Psi  */
31
//#define USE_Extended_MM3_Measurement_Model
32
/* *************************************************************************** */
33
 
34
/* kalman filter model extensions change number of state variables */
35
#define KAFI_DIM_X   (3)    /* number of rows of state vector */
36
 
37
#ifdef USE_Extended_MM3_Measurement_Model
966 MikeW 38
    #define KAFI_DIM_Y   (6)    /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/
838 MikeW 39
#else
966 MikeW 40
    #define KAFI_DIM_Y   (4)   /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/
838 MikeW 41
#endif
42
 
43
#define KAFI_DIM_U   (3)    /* number of rows of control vector */
44
 
966 MikeW 45
#ifdef MELEXIS_GYRO
1041 MikeW 46
    #define fCycleTime 12.5F /* Use Different Cycle Times to account for Runtime / Gain Differences */
966 MikeW 47
#else
838 MikeW 48
#ifdef USE_Extended_MM3_Measurement_Model
1041 MikeW 49
    #define fCycleTime 16.0F
838 MikeW 50
#else
1041 MikeW 51
    //#define fCycleTime 12.5F
52
    #define fCycleTime 16.0F
966 MikeW 53
    #endif
838 MikeW 54
#endif
55
 
966 MikeW 56
/* Predict the Measurent */
57
void trPredictMeasurement(void);
58
/* Update the Jacobi Matrix */
59
void trUpdateJacobiMatrix(void);
60
/* Extract measurements and store them in vector matY.
61
   Measurement validity is indicated by fYValid[] */
62
void trMeasure(void);
63
/* Innovation: calculate Xd, and Pd  */
64
void trInnovate(void);
65
/* Update transition matrix Phi  */
66
void trUpdatePhi(void);
67
/* Update control matrix B */
68
void trUpdateBU(void);
69
/* Predict new state Xs and Ps */
70
void KAFIPrediction(void);
71
/* Main Kalman Filter Loop */
72
void FlightAttitudeEstimation(void);
73
/* Setup the Kalman Filter Parameter */
74
void Kafi_Init(void);
75
/* Not done yet */
76
void trEstimateVelocity(void);
77
/* Limit Angles to [-2Pi;...;+2Pi] */
78
void trLimitAngles(void);
79
 
80
typedef struct
81
{
82
f32_t Phi;
83
f32_t Theta;
84
f32_t Psi;
85
i32_t iPhi10;
86
i32_t iTheta10;
87
i32_t iPsi10;
88
f32_t dPhi;
89
f32_t dTheta;
90
f32_t dPsi;
91
f32_t du;
92
f32_t dv;
93
f32_t dw;
94
f32_t X;
95
f32_t Y;
96
f32_t Z;
97
} status_t;
98
 
99
status_t status;
100
 
838 MikeW 101
/*typedef*/ enum
102
{
103
   _p = 0,
104
   _q,
105
   _r,
106
   c_control_variables                       /* number of control variables */
107
} /*trControlVariables_e*/;
108
 
109
 
110
#ifdef USE_Extended_MM3_Measurement_Model
111
/*typedef*/ enum
112
{
113
  _ax = 0,
114
  _ay,
115
  _az,
116
  _mx,
117
  _my,
118
  _mz,
119
  c_observation_variables                       /* number of observation variables */
120
} /*trObservationVariables_e*/;
121
#else
122
/*typedef*/ enum
123
{
124
  _ax = 0,
125
  _ay,
126
  _az,
127
  _compass,
128
  c_observation_variables                       /* number of observation variables */
129
} /*trObservationVariables_e*/;
130
#endif
131
 
132
/*typedef*/ enum
133
{
134
   _Phi = 0,
135
   _Theta,
136
   _Psi,
137
   //_du,
138
   //_dv,
139
   //_dw,
140
   c_state_variables                       /* number of state variables */
141
} /*trStateVariables_e*/;
142
 
143
 
144
 
145
 
146
 
147