Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 836 | 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 | **************************************************************************** */ |
||
| 18 | #include "mat.h" |
||
| 19 | |||
| 20 | /***************************************************************************** |
||
| 21 | (SYMBOLIC) CONSTANTS |
||
| 22 | *****************************************************************************/ |
||
| 23 | |||
| 24 | |||
| 25 | /* *************************************************************************** */ |
||
| 26 | /* Uses the Compass to Estimate Theta, Phi, Psi */ |
||
| 27 | //#define USE_Extended_MM3_Measurement_Model |
||
| 28 | /* *************************************************************************** */ |
||
| 29 | |||
| 30 | /* kalman filter model extensions change number of state variables */ |
||
| 31 | #define KAFI_DIM_X (3) /* number of rows of state vector */ |
||
| 32 | |||
| 33 | #ifdef USE_Extended_MM3_Measurement_Model |
||
| 34 | #define KAFI_DIM_Y (6) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/ |
||
| 35 | #else |
||
| 36 | #define KAFI_DIM_Y (4) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/ |
||
| 37 | #endif |
||
| 38 | |||
| 39 | #define KAFI_DIM_U (3) /* number of rows of control vector */ |
||
| 40 | |||
| 41 | #ifdef USE_Extended_MM3_Measurement_Model |
||
| 42 | #define fCycleTime 16.0F; |
||
| 43 | #else |
||
| 44 | #define fCycleTime 10.5F; |
||
| 45 | #endif |
||
| 46 | |||
| 47 | /*typedef*/ enum |
||
| 48 | { |
||
| 49 | _p = 0, |
||
| 50 | _q, |
||
| 51 | _r, |
||
| 52 | c_control_variables /* number of control variables */ |
||
| 53 | } /*trControlVariables_e*/; |
||
| 54 | |||
| 55 | |||
| 56 | #ifdef USE_Extended_MM3_Measurement_Model |
||
| 57 | /*typedef*/ enum |
||
| 58 | { |
||
| 59 | _ax = 0, |
||
| 60 | _ay, |
||
| 61 | _az, |
||
| 62 | _mx, |
||
| 63 | _my, |
||
| 64 | _mz, |
||
| 65 | c_observation_variables /* number of observation variables */ |
||
| 66 | } /*trObservationVariables_e*/; |
||
| 67 | #else |
||
| 68 | /*typedef*/ enum |
||
| 69 | { |
||
| 70 | _ax = 0, |
||
| 71 | _ay, |
||
| 72 | _az, |
||
| 73 | _compass, |
||
| 74 | c_observation_variables /* number of observation variables */ |
||
| 75 | } /*trObservationVariables_e*/; |
||
| 76 | #endif |
||
| 77 | |||
| 78 | |||
| 79 | /*typedef*/ enum |
||
| 80 | { |
||
| 81 | _Phi = 0, |
||
| 82 | _Theta, |
||
| 83 | _Psi, |
||
| 84 | //_du, |
||
| 85 | //_dv, |
||
| 86 | //_dw, |
||
| 87 | c_state_variables /* number of state variables */ |
||
| 88 | } /*trStateVariables_e*/; |
||
| 89 | |||
| 90 | |||
| 91 | typedef struct |
||
| 92 | { |
||
| 93 | f32_t Phi; |
||
| 94 | f32_t Theta; |
||
| 95 | f32_t Psi; |
||
| 96 | i32_t iPhi10; |
||
| 97 | i32_t iTheta10; |
||
| 98 | i32_t iPsi10; |
||
| 99 | f32_t dPhi; |
||
| 100 | f32_t dTheta; |
||
| 101 | f32_t dPsi; |
||
| 102 | f32_t du; |
||
| 103 | f32_t dv; |
||
| 104 | f32_t dw; |
||
| 105 | f32_t X; |
||
| 106 | f32_t Y; |
||
| 107 | f32_t Z; |
||
| 108 | } status_t; |
||
| 109 | |||
| 110 | status_t status; |
||
| 111 | |||
| 112 | |||
| 113 | void trUpdatePhi(void); |
||
| 114 | void trUpdateBU(void); |
||
| 115 | void trMeasure(void); |
||
| 116 | void trInnovate(void); |
||
| 117 | |||
| 118 | void trResetKalmanFilter(void); |
||
| 119 | void AttitudeEstimation(void); |
||
| 120 | |||
| 121 | int KAFIInit(kafi_t *pkafi); |
||
| 122 | void Kafi_Init(void); |
||
| 123 | void trEstimateVelocity(void); |
||
| 124 | void trLimitAngles(void); |
||
| 125 | void trControl(void); |
||
| 126 | |||
| 127 |