0,0 → 1,127 |
/* |
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); |
|
|