Prof Greg Egan
UAVX Multicopter Flight Controller.
attitude.c@2:90292f8bd179, 2011-04-26 (annotated)
- Committer:
- gke
- Date:
- Tue Apr 26 12:12:29 2011 +0000
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
Not flightworthy. Posted for others to make use of the I2C SW code.
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| gke | 0:62a1c91a859a | 1 | // =============================================================================================== |
| gke | 0:62a1c91a859a | 2 | // = UAVXArm Quadrocopter Controller = |
| gke | 0:62a1c91a859a | 3 | // = Copyright (c) 2008 by Prof. Greg Egan = |
| gke | 0:62a1c91a859a | 4 | // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = |
| gke | 2:90292f8bd179 | 5 | // = http://code.google.com/p/uavp-mods/ = |
| gke | 0:62a1c91a859a | 6 | // =============================================================================================== |
| gke | 0:62a1c91a859a | 7 | |
| gke | 0:62a1c91a859a | 8 | // This is part of UAVXArm. |
| gke | 0:62a1c91a859a | 9 | |
| gke | 0:62a1c91a859a | 10 | // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU |
| gke | 0:62a1c91a859a | 11 | // General Public License as published by the Free Software Foundation, either version 3 of the |
| gke | 0:62a1c91a859a | 12 | // License, or (at your option) any later version. |
| gke | 0:62a1c91a859a | 13 | |
| gke | 0:62a1c91a859a | 14 | // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without |
| gke | 0:62a1c91a859a | 15 | // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
| gke | 0:62a1c91a859a | 16 | // See the GNU General Public License for more details. |
| gke | 0:62a1c91a859a | 17 | |
| gke | 0:62a1c91a859a | 18 | // You should have received a copy of the GNU General Public License along with this program. |
| gke | 0:62a1c91a859a | 19 | // If not, see http://www.gnu.org/licenses/ |
| gke | 0:62a1c91a859a | 20 | |
| gke | 0:62a1c91a859a | 21 | #include "UAVXArm.h" |
| gke | 0:62a1c91a859a | 22 | |
| gke | 0:62a1c91a859a | 23 | // Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW. |
| gke | 2:90292f8bd179 | 24 | // CAUTION: Because of the coordinate frame the LR Acc sense must be negated for roll compensation. |
| gke | 0:62a1c91a859a | 25 | |
| gke | 2:90292f8bd179 | 26 | void AdaptiveYawLPFreq(void); |
| gke | 2:90292f8bd179 | 27 | void DoLegacyYawComp(uint8); |
| gke | 2:90292f8bd179 | 28 | void NormaliseAccelerations(void); |
| gke | 0:62a1c91a859a | 29 | void AttitudeTest(void); |
| gke | 2:90292f8bd179 | 30 | void InitAttitude(void); |
| gke | 0:62a1c91a859a | 31 | |
| gke | 2:90292f8bd179 | 32 | real32 AccMagnitude; |
| gke | 2:90292f8bd179 | 33 | real32 EstAngle[3][MaxAttitudeScheme]; |
| gke | 2:90292f8bd179 | 34 | real32 EstRate[3][MaxAttitudeScheme]; |
| gke | 2:90292f8bd179 | 35 | real32 Correction[2]; |
| gke | 2:90292f8bd179 | 36 | real32 YawFilterLPFreq; |
| gke | 2:90292f8bd179 | 37 | real32 dT, dTOn2, dTR, dTmS; |
| gke | 0:62a1c91a859a | 38 | uint32 uSp; |
| gke | 0:62a1c91a859a | 39 | |
| gke | 2:90292f8bd179 | 40 | uint8 AttitudeMethod = Wolferl; //Integrator, Wolferl MadgwickIMU PremerlaniDCM MadgwickAHRS, MultiWii; |
| gke | 2:90292f8bd179 | 41 | |
| gke | 2:90292f8bd179 | 42 | void AdaptiveYawLPFreq(void) { // Filter LP freq is decreased with reduced yaw stick deflection |
| gke | 0:62a1c91a859a | 43 | |
| gke | 2:90292f8bd179 | 44 | YawFilterLPFreq = ( MAX_YAW_FREQ*abs(DesiredYaw) / RC_NEUTRAL ); |
| gke | 2:90292f8bd179 | 45 | YawFilterLPFreq = Limit(YawFilterLPFreq, 0.5, MAX_YAW_FREQ); |
| gke | 2:90292f8bd179 | 46 | |
| gke | 2:90292f8bd179 | 47 | } // AdaptiveYawFilterA |
| gke | 0:62a1c91a859a | 48 | |
| gke | 2:90292f8bd179 | 49 | real32 HE; |
| gke | 2:90292f8bd179 | 50 | |
| gke | 2:90292f8bd179 | 51 | void DoLegacyYawComp(uint8 S) { |
| gke | 0:62a1c91a859a | 52 | |
| gke | 2:90292f8bd179 | 53 | #define COMPASS_MIDDLE 10 // yaw stick neutral dead zone |
| gke | 2:90292f8bd179 | 54 | #define DRIFT_COMP_YAW_RATE QUARTERPI // Radians/Sec |
| gke | 0:62a1c91a859a | 55 | |
| gke | 2:90292f8bd179 | 56 | static int16 Temp; |
| gke | 2:90292f8bd179 | 57 | |
| gke | 2:90292f8bd179 | 58 | // Yaw Angle here is meant to be interpreted as the Heading Error |
| gke | 2:90292f8bd179 | 59 | |
| gke | 2:90292f8bd179 | 60 | Rate[Yaw] = Gyro[Yaw]; |
| gke | 0:62a1c91a859a | 61 | |
| gke | 2:90292f8bd179 | 62 | Temp = DesiredYaw - Trim[Yaw]; |
| gke | 2:90292f8bd179 | 63 | if ( F.CompassValid ) // CW+ |
| gke | 2:90292f8bd179 | 64 | if ( abs(Temp) > COMPASS_MIDDLE ) { |
| gke | 2:90292f8bd179 | 65 | DesiredHeading = Heading; // acquire new heading |
| gke | 2:90292f8bd179 | 66 | Angle[Yaw] = 0.0; |
| gke | 0:62a1c91a859a | 67 | } else { |
| gke | 2:90292f8bd179 | 68 | HE = MinimumTurn(DesiredHeading - Heading); |
| gke | 2:90292f8bd179 | 69 | HE = Limit1(HE, SIXTHPI); // 30 deg limit |
| gke | 2:90292f8bd179 | 70 | HE = HE*K[CompassKp]; |
| gke | 2:90292f8bd179 | 71 | Rate[Yaw] = -Limit1(HE, DRIFT_COMP_YAW_RATE); |
| gke | 0:62a1c91a859a | 72 | } |
| gke | 2:90292f8bd179 | 73 | else { |
| gke | 2:90292f8bd179 | 74 | DesiredHeading = Heading; |
| gke | 2:90292f8bd179 | 75 | Angle[Yaw] = 0.0; |
| gke | 0:62a1c91a859a | 76 | } |
| gke | 0:62a1c91a859a | 77 | |
| gke | 2:90292f8bd179 | 78 | Angle[Yaw] += Rate[Yaw]*dT; |
| gke | 2:90292f8bd179 | 79 | // Angle[Yaw] = Limit1(Angle[Yaw], K[YawIntLimit]); |
| gke | 0:62a1c91a859a | 80 | |
| gke | 0:62a1c91a859a | 81 | } // DoLegacyYawComp |
| gke | 0:62a1c91a859a | 82 | |
| gke | 2:90292f8bd179 | 83 | void NormaliseAccelerations(void) { |
| gke | 2:90292f8bd179 | 84 | |
| gke | 2:90292f8bd179 | 85 | const real32 MIN_ACC_MAGNITUDE = 0.7; // below this the accelerometers are deemed unreliable - falling? |
| gke | 2:90292f8bd179 | 86 | |
| gke | 2:90292f8bd179 | 87 | static real32 ReNorm; |
| gke | 2:90292f8bd179 | 88 | |
| gke | 2:90292f8bd179 | 89 | AccMagnitude = sqrt(Sqr(Acc[BF]) + Sqr(Acc[LR]) + Sqr(Acc[UD])); |
| gke | 2:90292f8bd179 | 90 | F.AccMagnitudeOK = AccMagnitude > MIN_ACC_MAGNITUDE; |
| gke | 2:90292f8bd179 | 91 | if ( F.AccMagnitudeOK ) { |
| gke | 2:90292f8bd179 | 92 | ReNorm = 1.0 / AccMagnitude; |
| gke | 2:90292f8bd179 | 93 | Acc[BF] *= ReNorm; |
| gke | 2:90292f8bd179 | 94 | Acc[LR] *= ReNorm; |
| gke | 2:90292f8bd179 | 95 | Acc[UD] *= ReNorm; |
| gke | 2:90292f8bd179 | 96 | } else { |
| gke | 2:90292f8bd179 | 97 | Acc[LR] = Acc[BF] = 0.0; |
| gke | 2:90292f8bd179 | 98 | Acc[UD] = 1.0; |
| gke | 2:90292f8bd179 | 99 | } |
| gke | 2:90292f8bd179 | 100 | } // NormaliseAccelerations |
| gke | 2:90292f8bd179 | 101 | |
| gke | 0:62a1c91a859a | 102 | void GetAttitude(void) { |
| gke | 0:62a1c91a859a | 103 | |
| gke | 0:62a1c91a859a | 104 | static uint32 Now; |
| gke | 0:62a1c91a859a | 105 | static uint8 i; |
| gke | 0:62a1c91a859a | 106 | |
| gke | 0:62a1c91a859a | 107 | if ( GyroType == IRSensors ) |
| gke | 0:62a1c91a859a | 108 | GetIRAttitude(); |
| gke | 0:62a1c91a859a | 109 | else { |
| gke | 0:62a1c91a859a | 110 | GetGyroRates(); |
| gke | 0:62a1c91a859a | 111 | GetAccelerations(); |
| gke | 0:62a1c91a859a | 112 | } |
| gke | 0:62a1c91a859a | 113 | |
| gke | 0:62a1c91a859a | 114 | Now = uSClock(); |
| gke | 2:90292f8bd179 | 115 | dT = ( Now - uSp)*0.000001; |
| gke | 2:90292f8bd179 | 116 | dTOn2 = 0.5 * dT; |
| gke | 0:62a1c91a859a | 117 | dTR = 1.0 / dT; |
| gke | 0:62a1c91a859a | 118 | uSp = Now; |
| gke | 0:62a1c91a859a | 119 | |
| gke | 2:90292f8bd179 | 120 | GetHeading(); // only updated every 50mS but read continuously anyway |
| gke | 2:90292f8bd179 | 121 | |
| gke | 0:62a1c91a859a | 122 | if ( GyroType == IRSensors ) { |
| gke | 0:62a1c91a859a | 123 | |
| gke | 0:62a1c91a859a | 124 | for ( i = 0; i < (uint8)2; i++ ) { |
| gke | 2:90292f8bd179 | 125 | Rate[i] = ( Angle[i] - Anglep[i] )*dT; |
| gke | 0:62a1c91a859a | 126 | Anglep[i] = Angle[i]; |
| gke | 0:62a1c91a859a | 127 | } |
| gke | 0:62a1c91a859a | 128 | |
| gke | 0:62a1c91a859a | 129 | Rate[Yaw] = 0.0; // need Yaw gyro! |
| gke | 0:62a1c91a859a | 130 | |
| gke | 0:62a1c91a859a | 131 | } else { |
| gke | 0:62a1c91a859a | 132 | DebugPin = true; |
| gke | 2:90292f8bd179 | 133 | |
| gke | 2:90292f8bd179 | 134 | NormaliseAccelerations(); // rudimentary check for free fall etc |
| gke | 2:90292f8bd179 | 135 | |
| gke | 2:90292f8bd179 | 136 | // Wolferl |
| gke | 2:90292f8bd179 | 137 | DoWolferl(); |
| gke | 2:90292f8bd179 | 138 | |
| gke | 2:90292f8bd179 | 139 | #ifdef INC_ALL_SCHEMES |
| gke | 2:90292f8bd179 | 140 | |
| gke | 2:90292f8bd179 | 141 | // Complementary |
| gke | 2:90292f8bd179 | 142 | DoCF(); |
| gke | 2:90292f8bd179 | 143 | |
| gke | 2:90292f8bd179 | 144 | // Kalman |
| gke | 2:90292f8bd179 | 145 | DoKalman(); |
| gke | 2:90292f8bd179 | 146 | |
| gke | 2:90292f8bd179 | 147 | //Premerlani DCM |
| gke | 2:90292f8bd179 | 148 | DoDCM(); |
| gke | 2:90292f8bd179 | 149 | |
| gke | 2:90292f8bd179 | 150 | // MultiWii |
| gke | 2:90292f8bd179 | 151 | DoMultiWii(); |
| gke | 2:90292f8bd179 | 152 | |
| gke | 2:90292f8bd179 | 153 | // Madgwick IMU |
| gke | 2:90292f8bd179 | 154 | // DoMadgwickIMU(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]); |
| gke | 2:90292f8bd179 | 155 | |
| gke | 2:90292f8bd179 | 156 | //#define INC_IMU2 |
| gke | 2:90292f8bd179 | 157 | |
| gke | 2:90292f8bd179 | 158 | #ifdef INC_IMU2 |
| gke | 2:90292f8bd179 | 159 | DoMadgwickIMU2(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]); |
| gke | 2:90292f8bd179 | 160 | #else |
| gke | 2:90292f8bd179 | 161 | Madgwick IMU April 30, 2010 Paper Version |
| gke | 2:90292f8bd179 | 162 | #endif |
| gke | 2:90292f8bd179 | 163 | // Madgwick AHRS BROKEN |
| gke | 2:90292f8bd179 | 164 | DoMadgwickAHRS(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD], Mag[BF].V, Mag[LR].V, -Mag[UD].V); |
| gke | 2:90292f8bd179 | 165 | |
| gke | 2:90292f8bd179 | 166 | // Integrator - REFERENCE/FALLBACK |
| gke | 2:90292f8bd179 | 167 | DoIntegrator(); |
| gke | 2:90292f8bd179 | 168 | |
| gke | 2:90292f8bd179 | 169 | #endif // INC_ALL_SCHEMES |
| gke | 2:90292f8bd179 | 170 | |
| gke | 2:90292f8bd179 | 171 | Angle[Roll] = EstAngle[Roll][AttitudeMethod]; |
| gke | 2:90292f8bd179 | 172 | Angle[Pitch] = EstAngle[Pitch][AttitudeMethod]; |
| gke | 0:62a1c91a859a | 173 | |
| gke | 0:62a1c91a859a | 174 | DebugPin = false; |
| gke | 0:62a1c91a859a | 175 | } |
| gke | 0:62a1c91a859a | 176 | |
| gke | 0:62a1c91a859a | 177 | F.NearLevel = Max(fabs(Angle[Roll]), fabs(Angle[Pitch])) < NAV_RTH_LOCKOUT; |
| gke | 0:62a1c91a859a | 178 | |
| gke | 0:62a1c91a859a | 179 | } // GetAttitude |
| gke | 0:62a1c91a859a | 180 | |
| gke | 2:90292f8bd179 | 181 | //____________________________________________________________________________________________ |
| gke | 0:62a1c91a859a | 182 | |
| gke | 2:90292f8bd179 | 183 | // Integrator |
| gke | 0:62a1c91a859a | 184 | |
| gke | 2:90292f8bd179 | 185 | void DoIntegrator(void) { |
| gke | 0:62a1c91a859a | 186 | |
| gke | 2:90292f8bd179 | 187 | static uint8 g; |
| gke | 0:62a1c91a859a | 188 | |
| gke | 2:90292f8bd179 | 189 | for ( g = 0; g < (uint8)3; g++ ) { |
| gke | 2:90292f8bd179 | 190 | EstRate[g][Integrator] = Gyro[g]; |
| gke | 2:90292f8bd179 | 191 | EstAngle[g][Integrator] += EstRate[g][Integrator]*dT; |
| gke | 2:90292f8bd179 | 192 | // EstAngle[g][Integrator] = DecayX(EstAngle[g][Integrator], 0.0001*dT); |
| gke | 0:62a1c91a859a | 193 | } |
| gke | 0:62a1c91a859a | 194 | |
| gke | 2:90292f8bd179 | 195 | } // DoIntegrator |
| gke | 2:90292f8bd179 | 196 | |
| gke | 2:90292f8bd179 | 197 | //____________________________________________________________________________________________ |
| gke | 2:90292f8bd179 | 198 | |
| gke | 2:90292f8bd179 | 199 | // Original simple accelerometer compensation of gyros developed for UAVP by Wolfgang Mahringer |
| gke | 2:90292f8bd179 | 200 | // and adapted for UAVXArm |
| gke | 2:90292f8bd179 | 201 | |
| gke | 2:90292f8bd179 | 202 | void DoWolferl(void) { // NO YAW ESTIMATE |
| gke | 2:90292f8bd179 | 203 | |
| gke | 2:90292f8bd179 | 204 | const real32 WKp = 0.13; // 0.1 |
| gke | 2:90292f8bd179 | 205 | |
| gke | 2:90292f8bd179 | 206 | static real32 Grav[2], Dyn[2]; |
| gke | 2:90292f8bd179 | 207 | static real32 CompStep; |
| gke | 2:90292f8bd179 | 208 | |
| gke | 2:90292f8bd179 | 209 | Rate[Roll] = Gyro[Roll]; |
| gke | 2:90292f8bd179 | 210 | Rate[Pitch] = Gyro[Pitch]; |
| gke | 2:90292f8bd179 | 211 | |
| gke | 2:90292f8bd179 | 212 | if ( F.AccMagnitudeOK ) { |
| gke | 2:90292f8bd179 | 213 | |
| gke | 2:90292f8bd179 | 214 | CompStep = WKp*dT; |
| gke | 2:90292f8bd179 | 215 | |
| gke | 2:90292f8bd179 | 216 | // Roll |
| gke | 2:90292f8bd179 | 217 | |
| gke | 2:90292f8bd179 | 218 | Grav[LR] = -sin(EstAngle[Roll][Wolferl]); // original used approximation for small angles |
| gke | 2:90292f8bd179 | 219 | Dyn[LR] = 0.0; //Rate[Roll]; // lateral acceleration due to rate - do later:). |
| gke | 2:90292f8bd179 | 220 | |
| gke | 2:90292f8bd179 | 221 | Correction[LR] = -Acc[LR] + Grav[LR] + Dyn[LR]; // Acc is reversed |
| gke | 2:90292f8bd179 | 222 | Correction[LR] = Limit1(Correction[LR], CompStep); |
| gke | 2:90292f8bd179 | 223 | |
| gke | 2:90292f8bd179 | 224 | EstAngle[Roll][Wolferl] += Rate[Roll]*dT; |
| gke | 2:90292f8bd179 | 225 | EstAngle[Roll][Wolferl] += Correction[LR]; |
| gke | 2:90292f8bd179 | 226 | |
| gke | 2:90292f8bd179 | 227 | // Pitch |
| gke | 2:90292f8bd179 | 228 | |
| gke | 2:90292f8bd179 | 229 | Grav[BF] = -sin(EstAngle[Pitch][Wolferl]); |
| gke | 2:90292f8bd179 | 230 | Dyn[BF] = 0.0; // Rate[Pitch]; |
| gke | 2:90292f8bd179 | 231 | |
| gke | 2:90292f8bd179 | 232 | Correction[BF] = Acc[BF] + Grav[BF] + Dyn[BF]; |
| gke | 2:90292f8bd179 | 233 | Correction[BF] = Limit1(Correction[BF], CompStep); |
| gke | 2:90292f8bd179 | 234 | |
| gke | 2:90292f8bd179 | 235 | EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT; |
| gke | 2:90292f8bd179 | 236 | EstAngle[Pitch][Wolferl] += Correction[BF]; |
| gke | 2:90292f8bd179 | 237 | |
| gke | 2:90292f8bd179 | 238 | } else { |
| gke | 2:90292f8bd179 | 239 | |
| gke | 2:90292f8bd179 | 240 | EstAngle[Roll][Wolferl] += Rate[Roll]*dT; |
| gke | 2:90292f8bd179 | 241 | EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT; |
| gke | 2:90292f8bd179 | 242 | |
| gke | 0:62a1c91a859a | 243 | } |
| gke | 0:62a1c91a859a | 244 | |
| gke | 2:90292f8bd179 | 245 | } // DoWolferl |
| gke | 2:90292f8bd179 | 246 | |
| gke | 2:90292f8bd179 | 247 | //_________________________________________________________________________________ |
| gke | 2:90292f8bd179 | 248 | |
| gke | 2:90292f8bd179 | 249 | // Complementary Filter originally authored by RoyLB |
| gke | 2:90292f8bd179 | 250 | // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286 |
| gke | 2:90292f8bd179 | 251 | |
| gke | 2:90292f8bd179 | 252 | const real32 TauCF = 1.1; |
| gke | 2:90292f8bd179 | 253 | |
| gke | 2:90292f8bd179 | 254 | real32 AngleCF[2] = {0,0}; |
| gke | 2:90292f8bd179 | 255 | real32 F0[2] = {0,0}; |
| gke | 2:90292f8bd179 | 256 | real32 F1[2] = {0,0}; |
| gke | 2:90292f8bd179 | 257 | real32 F2[2] = {0,0}; |
| gke | 2:90292f8bd179 | 258 | |
| gke | 2:90292f8bd179 | 259 | real32 CF(uint8 a, real32 NewAngle, real32 NewRate) { |
| gke | 0:62a1c91a859a | 260 | |
| gke | 2:90292f8bd179 | 261 | if ( F.AccMagnitudeOK ) { |
| gke | 2:90292f8bd179 | 262 | F0[a] = (NewAngle - AngleCF[a])*Sqr(TauCF); |
| gke | 2:90292f8bd179 | 263 | F2[a] += F0[a]*dT; |
| gke | 2:90292f8bd179 | 264 | F1[a] = F2[a] + (NewAngle - AngleCF[a])*2.0*TauCF + NewRate; |
| gke | 2:90292f8bd179 | 265 | AngleCF[a] = (F1[a]*dT) + AngleCF[a]; |
| gke | 2:90292f8bd179 | 266 | } else |
| gke | 2:90292f8bd179 | 267 | AngleCF[a] += NewRate*dT; |
| gke | 2:90292f8bd179 | 268 | |
| gke | 2:90292f8bd179 | 269 | return ( AngleCF[a] ); // This is actually the current angle, but is stored for the next iteration |
| gke | 2:90292f8bd179 | 270 | } // CF |
| gke | 2:90292f8bd179 | 271 | |
| gke | 2:90292f8bd179 | 272 | void DoCF(void) { // NO YAW ANGLE ESTIMATE |
| gke | 2:90292f8bd179 | 273 | |
| gke | 2:90292f8bd179 | 274 | EstAngle[Roll][Complementary] = CF(Roll, asin(-Acc[LR]), Gyro[Roll]); |
| gke | 2:90292f8bd179 | 275 | EstAngle[Pitch][Complementary] = CF(Pitch, asin(-Acc[BF]), Gyro[Pitch]); // zzz minus??? |
| gke | 2:90292f8bd179 | 276 | EstRate[Roll][Complementary] = Gyro[Roll]; |
| gke | 2:90292f8bd179 | 277 | EstRate[Pitch][Complementary] = Gyro[Pitch]; |
| gke | 2:90292f8bd179 | 278 | |
| gke | 2:90292f8bd179 | 279 | } // DoCF |
| gke | 0:62a1c91a859a | 280 | |
| gke | 0:62a1c91a859a | 281 | //____________________________________________________________________________________________ |
| gke | 0:62a1c91a859a | 282 | |
| gke | 0:62a1c91a859a | 283 | // The DCM formulation used here is due to W. Premerlani and P. Bizard in a paper entitled: |
| gke | 0:62a1c91a859a | 284 | // Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original |
| gke | 0:62a1c91a859a | 285 | // work by R. Mahony et al. - Thanks Rob! |
| gke | 0:62a1c91a859a | 286 | |
| gke | 2:90292f8bd179 | 287 | // SEEMS TO BE A FAIRLY LARGE PHASE DELAY OF 2 SAMPLE INTERVALS |
| gke | 2:90292f8bd179 | 288 | |
| gke | 0:62a1c91a859a | 289 | void DCMNormalise(void); |
| gke | 0:62a1c91a859a | 290 | void DCMDriftCorrection(void); |
| gke | 0:62a1c91a859a | 291 | void DCMMotionCompensation(void); |
| gke | 0:62a1c91a859a | 292 | void DCMUpdate(void); |
| gke | 0:62a1c91a859a | 293 | void DCMEulerAngles(void); |
| gke | 0:62a1c91a859a | 294 | |
| gke | 0:62a1c91a859a | 295 | real32 RollPitchError[3] = {0,0,0}; |
| gke | 0:62a1c91a859a | 296 | real32 OmegaV[3] = {0,0,0}; // corrected gyro data |
| gke | 0:62a1c91a859a | 297 | real32 OmegaP[3] = {0,0,0}; // proportional correction |
| gke | 0:62a1c91a859a | 298 | real32 OmegaI[3] = {0,0,0}; // integral correction |
| gke | 0:62a1c91a859a | 299 | real32 Omega[3] = {0,0,0}; |
| gke | 0:62a1c91a859a | 300 | real32 DCM[3][3] = {{1,0,0 },{0,1,0} ,{0,0,1}}; |
| gke | 0:62a1c91a859a | 301 | real32 U[3][3] = {{0,1,2},{ 3,4,5} ,{6,7,8}}; |
| gke | 0:62a1c91a859a | 302 | real32 TempM[3][3] = {{0,0,0},{0,0,0},{ 0,0,0}}; |
| gke | 2:90292f8bd179 | 303 | real32 AccV[3]; |
| gke | 0:62a1c91a859a | 304 | |
| gke | 0:62a1c91a859a | 305 | void DCMNormalise(void) { |
| gke | 0:62a1c91a859a | 306 | |
| gke | 0:62a1c91a859a | 307 | static real32 Error = 0; |
| gke | 0:62a1c91a859a | 308 | static real32 Renorm = 0.0; |
| gke | 0:62a1c91a859a | 309 | static boolean Problem; |
| gke | 0:62a1c91a859a | 310 | static uint8 r; |
| gke | 0:62a1c91a859a | 311 | |
| gke | 2:90292f8bd179 | 312 | Error = -VDot(&DCM[0][0], &DCM[1][0])*0.5; //eq.19 |
| gke | 0:62a1c91a859a | 313 | |
| gke | 0:62a1c91a859a | 314 | VScale(&TempM[0][0], &DCM[1][0], Error); //eq.19 |
| gke | 0:62a1c91a859a | 315 | VScale(&TempM[1][0], &DCM[0][0], Error); //eq.19 |
| gke | 0:62a1c91a859a | 316 | |
| gke | 0:62a1c91a859a | 317 | VAdd(&TempM[0][0], &TempM[0][0], &DCM[0][0]); //eq.19 |
| gke | 0:62a1c91a859a | 318 | VAdd(&TempM[1][0], &TempM[1][0], &DCM[1][0]); //eq.19 |
| gke | 0:62a1c91a859a | 319 | |
| gke | 2:90292f8bd179 | 320 | VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]); // c= a*b eq.20 |
| gke | 0:62a1c91a859a | 321 | |
| gke | 0:62a1c91a859a | 322 | Problem = false; |
| gke | 0:62a1c91a859a | 323 | for ( r = 0; r < (uint8)3; r++ ) { |
| gke | 2:90292f8bd179 | 324 | Renorm = VDot(&TempM[r][0], &TempM[r][0]); |
| gke | 0:62a1c91a859a | 325 | if ( (Renorm < 1.5625) && (Renorm > 0.64) ) |
| gke | 2:90292f8bd179 | 326 | Renorm = 0.5*(3.0 - Renorm); //eq.21 |
| gke | 0:62a1c91a859a | 327 | else |
| gke | 0:62a1c91a859a | 328 | if ( (Renorm < 100.0) && (Renorm > 0.01) ) |
| gke | 0:62a1c91a859a | 329 | Renorm = 1.0 / sqrt( Renorm ); |
| gke | 0:62a1c91a859a | 330 | else |
| gke | 0:62a1c91a859a | 331 | Problem = true; |
| gke | 0:62a1c91a859a | 332 | |
| gke | 0:62a1c91a859a | 333 | VScale(&DCM[r][0], &TempM[r][0], Renorm); |
| gke | 0:62a1c91a859a | 334 | } |
| gke | 0:62a1c91a859a | 335 | |
| gke | 0:62a1c91a859a | 336 | if ( Problem ) { // Divergent - force to initial conditions and hope! |
| gke | 0:62a1c91a859a | 337 | DCM[0][0] = 1.0; |
| gke | 0:62a1c91a859a | 338 | DCM[0][1] = 0.0; |
| gke | 0:62a1c91a859a | 339 | DCM[0][2] = 0.0; |
| gke | 0:62a1c91a859a | 340 | DCM[1][0] = 0.0; |
| gke | 0:62a1c91a859a | 341 | DCM[1][1] = 1.0; |
| gke | 0:62a1c91a859a | 342 | DCM[1][2] = 0.0; |
| gke | 0:62a1c91a859a | 343 | DCM[2][0] = 0.0; |
| gke | 0:62a1c91a859a | 344 | DCM[2][1] = 0.0; |
| gke | 0:62a1c91a859a | 345 | DCM[2][2] = 1.0; |
| gke | 0:62a1c91a859a | 346 | } |
| gke | 0:62a1c91a859a | 347 | |
| gke | 0:62a1c91a859a | 348 | } // DCMNormalise |
| gke | 0:62a1c91a859a | 349 | |
| gke | 0:62a1c91a859a | 350 | void DCMMotionCompensation(void) { |
| gke | 0:62a1c91a859a | 351 | // compensation for rate of change of velocity LR/BF needs GPS velocity but |
| gke | 0:62a1c91a859a | 352 | // updates probably too slow? |
| gke | 2:90292f8bd179 | 353 | AccV[LR ] += 0.0; |
| gke | 2:90292f8bd179 | 354 | AccV[BF] += 0.0; |
| gke | 0:62a1c91a859a | 355 | } // DCMMotionCompensation |
| gke | 0:62a1c91a859a | 356 | |
| gke | 0:62a1c91a859a | 357 | void DCMDriftCorrection(void) { |
| gke | 0:62a1c91a859a | 358 | |
| gke | 2:90292f8bd179 | 359 | static real32 ScaledOmegaI[3]; |
| gke | 0:62a1c91a859a | 360 | |
| gke | 2:90292f8bd179 | 361 | //DON'T USE #define USE_DCM_YAW_COMP |
| gke | 2:90292f8bd179 | 362 | #ifdef USE_DCM_YAW_COMP |
| gke | 2:90292f8bd179 | 363 | static real32 ScaledOmegaP[3]; |
| gke | 2:90292f8bd179 | 364 | static real32 YawError[3]; |
| gke | 2:90292f8bd179 | 365 | static real32 ErrorCourse; |
| gke | 2:90292f8bd179 | 366 | #endif // USE_DCM_YAW_COMP |
| gke | 0:62a1c91a859a | 367 | |
| gke | 2:90292f8bd179 | 368 | VCross(&RollPitchError[0], &AccV[0], &DCM[2][0]); //adjust the reference ground |
| gke | 2:90292f8bd179 | 369 | VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch); |
| gke | 0:62a1c91a859a | 370 | |
| gke | 2:90292f8bd179 | 371 | VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch); |
| gke | 0:62a1c91a859a | 372 | VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]); |
| gke | 0:62a1c91a859a | 373 | |
| gke | 2:90292f8bd179 | 374 | #ifdef USE_DCM_YAW_COMP |
| gke | 0:62a1c91a859a | 375 | // Yaw - drift correction based on compass/magnetometer heading |
| gke | 2:90292f8bd179 | 376 | HeadingCos = cos(Heading); |
| gke | 2:90292f8bd179 | 377 | HeadingSin = sin(Heading); |
| gke | 2:90292f8bd179 | 378 | ErrorCourse = ( U[0][0]*HeadingSin ) - ( U[1][0]*HeadingCos ); |
| gke | 0:62a1c91a859a | 379 | VScale(YawError, &U[2][0], ErrorCourse ); |
| gke | 0:62a1c91a859a | 380 | |
| gke | 0:62a1c91a859a | 381 | VScale(&ScaledOmegaP[0], &YawError[0], Kp_Yaw ); |
| gke | 0:62a1c91a859a | 382 | VAdd(&OmegaP[0], &OmegaP[0], &ScaledOmegaP[0]); |
| gke | 0:62a1c91a859a | 383 | |
| gke | 0:62a1c91a859a | 384 | VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw ); |
| gke | 0:62a1c91a859a | 385 | VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]); |
| gke | 2:90292f8bd179 | 386 | #endif // USE_DCM_YAW_COMP |
| gke | 2:90292f8bd179 | 387 | |
| gke | 0:62a1c91a859a | 388 | } // DCMDriftCorrection |
| gke | 0:62a1c91a859a | 389 | |
| gke | 0:62a1c91a859a | 390 | void DCMUpdate(void) { |
| gke | 0:62a1c91a859a | 391 | |
| gke | 0:62a1c91a859a | 392 | static uint8 i, j, k; |
| gke | 0:62a1c91a859a | 393 | static real32 op[3]; |
| gke | 0:62a1c91a859a | 394 | |
| gke | 2:90292f8bd179 | 395 | AccV[BF] = Acc[BF]; |
| gke | 2:90292f8bd179 | 396 | AccV[LR] = -Acc[LR]; |
| gke | 2:90292f8bd179 | 397 | AccV[UD] = -Acc[UD]; |
| gke | 2:90292f8bd179 | 398 | |
| gke | 0:62a1c91a859a | 399 | VAdd(&Omega[0], &Gyro[0], &OmegaI[0]); |
| gke | 0:62a1c91a859a | 400 | VAdd(&OmegaV[0], &Omega[0], &OmegaP[0]); |
| gke | 0:62a1c91a859a | 401 | |
| gke | 0:62a1c91a859a | 402 | // MotionCompensation(); |
| gke | 0:62a1c91a859a | 403 | |
| gke | 0:62a1c91a859a | 404 | U[0][0] = 0.0; |
| gke | 2:90292f8bd179 | 405 | U[0][1] = -dT*OmegaV[2]; //-z |
| gke | 2:90292f8bd179 | 406 | U[0][2] = dT*OmegaV[1]; // y |
| gke | 2:90292f8bd179 | 407 | U[1][0] = dT*OmegaV[2]; // z |
| gke | 0:62a1c91a859a | 408 | U[1][1] = 0.0; |
| gke | 2:90292f8bd179 | 409 | U[1][2] = -dT*OmegaV[0]; //-x |
| gke | 2:90292f8bd179 | 410 | U[2][0] = -dT*OmegaV[1]; //-y |
| gke | 2:90292f8bd179 | 411 | U[2][1] = dT*OmegaV[0]; // x |
| gke | 0:62a1c91a859a | 412 | U[2][2] = 0.0; |
| gke | 0:62a1c91a859a | 413 | |
| gke | 0:62a1c91a859a | 414 | for ( i = 0; i < (uint8)3; i++ ) |
| gke | 0:62a1c91a859a | 415 | for ( j = 0; j < (uint8)3; j++ ) { |
| gke | 0:62a1c91a859a | 416 | for ( k = 0; k < (uint8)3; k++ ) |
| gke | 2:90292f8bd179 | 417 | op[k] = DCM[i][k]*U[k][j]; |
| gke | 0:62a1c91a859a | 418 | |
| gke | 0:62a1c91a859a | 419 | TempM[i][j] = op[0] + op[1] + op[2]; |
| gke | 0:62a1c91a859a | 420 | } |
| gke | 0:62a1c91a859a | 421 | |
| gke | 0:62a1c91a859a | 422 | for ( i = 0; i < (uint8)3; i++ ) |
| gke | 0:62a1c91a859a | 423 | for (j = 0; j < (uint8)3; j++ ) |
| gke | 0:62a1c91a859a | 424 | DCM[i][j] += TempM[i][j]; |
| gke | 0:62a1c91a859a | 425 | |
| gke | 0:62a1c91a859a | 426 | } // DCMUpdate |
| gke | 0:62a1c91a859a | 427 | |
| gke | 0:62a1c91a859a | 428 | void DCMEulerAngles(void) { |
| gke | 0:62a1c91a859a | 429 | |
| gke | 0:62a1c91a859a | 430 | static uint8 g; |
| gke | 0:62a1c91a859a | 431 | |
| gke | 0:62a1c91a859a | 432 | for ( g = 0; g < (uint8)3; g++ ) |
| gke | 0:62a1c91a859a | 433 | Rate[g] = Gyro[g]; |
| gke | 0:62a1c91a859a | 434 | |
| gke | 2:90292f8bd179 | 435 | EstAngle[Roll][PremerlaniDCM]= atan2(DCM[2][1], DCM[2][2]); |
| gke | 2:90292f8bd179 | 436 | EstAngle[Pitch][PremerlaniDCM] = -asin(DCM[2][0]); |
| gke | 2:90292f8bd179 | 437 | EstAngle[Yaw][PremerlaniDCM] = atan2(DCM[1][0], DCM[0][0]); |
| gke | 0:62a1c91a859a | 438 | |
| gke | 2:90292f8bd179 | 439 | // Est. Rates ??? |
| gke | 0:62a1c91a859a | 440 | |
| gke | 0:62a1c91a859a | 441 | } // DCMEulerAngles |
| gke | 0:62a1c91a859a | 442 | |
| gke | 2:90292f8bd179 | 443 | void DoDCM(void) { |
| gke | 2:90292f8bd179 | 444 | DCMUpdate(); |
| gke | 2:90292f8bd179 | 445 | DCMNormalise(); |
| gke | 2:90292f8bd179 | 446 | DCMDriftCorrection(); |
| gke | 2:90292f8bd179 | 447 | DCMEulerAngles(); |
| gke | 2:90292f8bd179 | 448 | } // DoDCM |
| gke | 2:90292f8bd179 | 449 | |
| gke | 0:62a1c91a859a | 450 | //___________________________________________________________________________________ |
| gke | 0:62a1c91a859a | 451 | |
| gke | 0:62a1c91a859a | 452 | // IMU.c |
| gke | 0:62a1c91a859a | 453 | // S.O.H. Madgwick |
| gke | 0:62a1c91a859a | 454 | // 25th September 2010 |
| gke | 0:62a1c91a859a | 455 | |
| gke | 0:62a1c91a859a | 456 | // Description: |
| gke | 0:62a1c91a859a | 457 | |
| gke | 0:62a1c91a859a | 458 | // Quaternion implementation of the 'DCM filter' [Mahony et al.]. |
| gke | 0:62a1c91a859a | 459 | |
| gke | 0:62a1c91a859a | 460 | // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated |
| gke | 0:62a1c91a859a | 461 | // orientation. See my report for an overview of the use of quaternions in this application. |
| gke | 0:62a1c91a859a | 462 | |
| gke | 0:62a1c91a859a | 463 | // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz') |
| gke | 2:90292f8bd179 | 464 | // and accelerometer ('ax', 'ay', 'az') data. Gyroscope units are radians/second, accelerometer |
| gke | 0:62a1c91a859a | 465 | // units are irrelevant as the vector is normalised. |
| gke | 0:62a1c91a859a | 466 | |
| gke | 2:90292f8bd179 | 467 | const real32 MKp = 2.0; // proportional gain governs rate of convergence to accelerometer/magnetometer |
| gke | 2:90292f8bd179 | 468 | const real32 MKi = 1.0; // integral gain governs rate of convergence of gyroscope biases // 0.005 |
| gke | 0:62a1c91a859a | 469 | |
| gke | 2:90292f8bd179 | 470 | real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0; // scaled integral error |
| gke | 0:62a1c91a859a | 471 | real32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; // quaternion elements representing the estimated orientation |
| gke | 0:62a1c91a859a | 472 | |
| gke | 2:90292f8bd179 | 473 | void DoMadgwickIMU(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) { |
| gke | 0:62a1c91a859a | 474 | |
| gke | 2:90292f8bd179 | 475 | static uint8 g; |
| gke | 2:90292f8bd179 | 476 | static real32 ReNorm; |
| gke | 0:62a1c91a859a | 477 | static real32 vx, vy, vz; |
| gke | 0:62a1c91a859a | 478 | static real32 ex, ey, ez; |
| gke | 2:90292f8bd179 | 479 | |
| gke | 2:90292f8bd179 | 480 | if ( F.AccMagnitudeOK ) { |
| gke | 2:90292f8bd179 | 481 | |
| gke | 2:90292f8bd179 | 482 | // estimated direction of gravity |
| gke | 2:90292f8bd179 | 483 | vx = 2.0*(q1*q3 - q0*q2); |
| gke | 2:90292f8bd179 | 484 | vy = 2.0*(q0*q1 + q2*q3); |
| gke | 2:90292f8bd179 | 485 | vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3); |
| gke | 2:90292f8bd179 | 486 | |
| gke | 2:90292f8bd179 | 487 | // error is sum of cross product between reference direction of field and direction measured by sensor |
| gke | 2:90292f8bd179 | 488 | ex = (ay*vz - az*vy); |
| gke | 2:90292f8bd179 | 489 | ey = (az*vx - ax*vz); |
| gke | 2:90292f8bd179 | 490 | ez = (ax*vy - ay*vx); |
| gke | 2:90292f8bd179 | 491 | |
| gke | 2:90292f8bd179 | 492 | // integral error scaled integral gain |
| gke | 2:90292f8bd179 | 493 | exInt += ex*MKi*dT; |
| gke | 2:90292f8bd179 | 494 | eyInt += ey*MKi*dT; |
| gke | 2:90292f8bd179 | 495 | ezInt += ez*MKi*dT; |
| gke | 0:62a1c91a859a | 496 | |
| gke | 2:90292f8bd179 | 497 | // adjusted gyroscope measurements |
| gke | 2:90292f8bd179 | 498 | gx += MKp*ex + exInt; |
| gke | 2:90292f8bd179 | 499 | gy += MKp*ey + eyInt; |
| gke | 2:90292f8bd179 | 500 | gz += MKp*ez + ezInt; |
| gke | 2:90292f8bd179 | 501 | |
| gke | 2:90292f8bd179 | 502 | // integrate quaternion rate and normalise |
| gke | 2:90292f8bd179 | 503 | q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2; |
| gke | 2:90292f8bd179 | 504 | q1 += (q0*gx + q2*gz - q3*gy)*dTOn2; |
| gke | 2:90292f8bd179 | 505 | q2 += (q0*gy - q1*gz + q3*gx)*dTOn2; |
| gke | 2:90292f8bd179 | 506 | q3 += (q0*gz + q1*gy - q2*gx)*dTOn2; |
| gke | 2:90292f8bd179 | 507 | |
| gke | 2:90292f8bd179 | 508 | // normalise quaternion |
| gke | 2:90292f8bd179 | 509 | ReNorm = 1.0 /sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3)); |
| gke | 2:90292f8bd179 | 510 | q0 *= ReNorm; |
| gke | 2:90292f8bd179 | 511 | q1 *= ReNorm; |
| gke | 2:90292f8bd179 | 512 | q2 *= ReNorm; |
| gke | 2:90292f8bd179 | 513 | q3 *= ReNorm; |
| gke | 2:90292f8bd179 | 514 | |
| gke | 2:90292f8bd179 | 515 | MadgwickEulerAngles(MadgwickIMU); |
| gke | 0:62a1c91a859a | 516 | |
| gke | 2:90292f8bd179 | 517 | } else |
| gke | 2:90292f8bd179 | 518 | for ( g = 0; g <(uint8)3; g++) { |
| gke | 2:90292f8bd179 | 519 | EstRate[g][MadgwickIMU] = Gyro[g]; |
| gke | 2:90292f8bd179 | 520 | EstAngle[g][MadgwickIMU] += EstRate[g][MadgwickIMU]*dT; |
| gke | 2:90292f8bd179 | 521 | } |
| gke | 2:90292f8bd179 | 522 | |
| gke | 2:90292f8bd179 | 523 | } // DoMadgwickIMU |
| gke | 2:90292f8bd179 | 524 | |
| gke | 2:90292f8bd179 | 525 | //_________________________________________________________________________________ |
| gke | 2:90292f8bd179 | 526 | |
| gke | 2:90292f8bd179 | 527 | // IMU.c |
| gke | 2:90292f8bd179 | 528 | // S.O.H. Madgwick, |
| gke | 2:90292f8bd179 | 529 | // 'An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays', |
| gke | 2:90292f8bd179 | 530 | // April 30, 2010 |
| gke | 2:90292f8bd179 | 531 | |
| gke | 2:90292f8bd179 | 532 | #ifdef INC_IMU2 |
| gke | 2:90292f8bd179 | 533 | |
| gke | 2:90292f8bd179 | 534 | boolean FirstIMU2 = true; |
| gke | 2:90292f8bd179 | 535 | real32 BetaIMU2 = 0.033; |
| gke | 2:90292f8bd179 | 536 | // const real32 BetaAHRS = 0.041; |
| gke | 0:62a1c91a859a | 537 | |
| gke | 2:90292f8bd179 | 538 | //Quaternion orientation of earth frame relative to auxiliary frame. |
| gke | 2:90292f8bd179 | 539 | real32 AEq_1; |
| gke | 2:90292f8bd179 | 540 | real32 AEq_2; |
| gke | 2:90292f8bd179 | 541 | real32 AEq_3; |
| gke | 2:90292f8bd179 | 542 | real32 AEq_4; |
| gke | 2:90292f8bd179 | 543 | |
| gke | 2:90292f8bd179 | 544 | //Estimated orientation quaternion elements with initial conditions. |
| gke | 2:90292f8bd179 | 545 | real32 SEq_1; |
| gke | 2:90292f8bd179 | 546 | real32 SEq_2; |
| gke | 2:90292f8bd179 | 547 | real32 SEq_3; |
| gke | 2:90292f8bd179 | 548 | real32 SEq_4; |
| gke | 2:90292f8bd179 | 549 | |
| gke | 2:90292f8bd179 | 550 | void DoMadgwickIMU2(real32 w_x, real32 w_y, real32 w_z, real32 a_x, real32 a_y, real32 a_z) { |
| gke | 2:90292f8bd179 | 551 | |
| gke | 2:90292f8bd179 | 552 | static uint8 g; |
| gke | 2:90292f8bd179 | 553 | |
| gke | 2:90292f8bd179 | 554 | //Vector norm. |
| gke | 2:90292f8bd179 | 555 | static real32 Renorm; |
| gke | 2:90292f8bd179 | 556 | //Quaternion rate from gyroscope elements. |
| gke | 2:90292f8bd179 | 557 | static real32 SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; |
| gke | 2:90292f8bd179 | 558 | //Objective function elements. |
| gke | 2:90292f8bd179 | 559 | static real32 f_1, f_2, f_3; |
| gke | 2:90292f8bd179 | 560 | //Objective function Jacobian elements. |
| gke | 2:90292f8bd179 | 561 | static real32 J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; |
| gke | 2:90292f8bd179 | 562 | //Objective function gradient elements. |
| gke | 2:90292f8bd179 | 563 | static real32 SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; |
| gke | 0:62a1c91a859a | 564 | |
| gke | 2:90292f8bd179 | 565 | //Auxiliary variables to avoid reapeated calcualtions. |
| gke | 2:90292f8bd179 | 566 | static real32 halfSEq_1, halfSEq_2, halfSEq_3, halfSEq_4; |
| gke | 2:90292f8bd179 | 567 | static real32 twoSEq_1, twoSEq_2, twoSEq_3; |
| gke | 2:90292f8bd179 | 568 | |
| gke | 2:90292f8bd179 | 569 | if ( F.AccMagnitudeOK ) { |
| gke | 2:90292f8bd179 | 570 | |
| gke | 2:90292f8bd179 | 571 | halfSEq_1 = 0.5*SEq_1; |
| gke | 2:90292f8bd179 | 572 | halfSEq_2 = 0.5*SEq_2; |
| gke | 2:90292f8bd179 | 573 | halfSEq_3 = 0.5*SEq_3; |
| gke | 2:90292f8bd179 | 574 | halfSEq_4 = 0.5*SEq_4; |
| gke | 2:90292f8bd179 | 575 | twoSEq_1 = 2.0*SEq_1; |
| gke | 2:90292f8bd179 | 576 | twoSEq_2 = 2.0*SEq_2; |
| gke | 2:90292f8bd179 | 577 | twoSEq_3 = 2.0*SEq_3; |
| gke | 2:90292f8bd179 | 578 | |
| gke | 2:90292f8bd179 | 579 | //Compute the quaternion rate measured by gyroscopes. |
| gke | 2:90292f8bd179 | 580 | SEqDot_omega_1 = -halfSEq_2*w_x - halfSEq_3*w_y - halfSEq_4*w_z; |
| gke | 2:90292f8bd179 | 581 | SEqDot_omega_2 = halfSEq_1*w_x + halfSEq_3*w_z - halfSEq_4*w_y; |
| gke | 2:90292f8bd179 | 582 | SEqDot_omega_3 = halfSEq_1*w_y - halfSEq_2*w_z + halfSEq_4*w_x; |
| gke | 2:90292f8bd179 | 583 | SEqDot_omega_4 = halfSEq_1*w_z + halfSEq_2*w_y - halfSEq_3*w_x; |
| gke | 0:62a1c91a859a | 584 | |
| gke | 2:90292f8bd179 | 585 | /* |
| gke | 2:90292f8bd179 | 586 | //Normalise the accelerometer measurement. |
| gke | 2:90292f8bd179 | 587 | Renorm = 1.0 / sqrt(Sqr(a_x) + Sqr(a_y) + Sqr(a_z)); |
| gke | 2:90292f8bd179 | 588 | a_x *= Renorm; |
| gke | 2:90292f8bd179 | 589 | a_y *= Renorm; |
| gke | 2:90292f8bd179 | 590 | a_z *= Renorm; |
| gke | 2:90292f8bd179 | 591 | */ |
| gke | 0:62a1c91a859a | 592 | |
| gke | 2:90292f8bd179 | 593 | //Compute the objective function and Jacobian. |
| gke | 2:90292f8bd179 | 594 | f_1 = twoSEq_2*SEq_4 - twoSEq_1*SEq_3 - a_x; |
| gke | 2:90292f8bd179 | 595 | f_2 = twoSEq_1*SEq_2 + twoSEq_3*SEq_4 - a_y; |
| gke | 2:90292f8bd179 | 596 | f_3 = 1.0 - twoSEq_2*SEq_2 - twoSEq_3*SEq_3 - a_z; |
| gke | 2:90292f8bd179 | 597 | //J_11 negated in matrix multiplication. |
| gke | 2:90292f8bd179 | 598 | J_11or24 = twoSEq_3; |
| gke | 2:90292f8bd179 | 599 | J_12or23 = 2.0*SEq_4; |
| gke | 2:90292f8bd179 | 600 | //J_12 negated in matrix multiplication |
| gke | 2:90292f8bd179 | 601 | J_13or22 = twoSEq_1; |
| gke | 2:90292f8bd179 | 602 | J_14or21 = twoSEq_2; |
| gke | 2:90292f8bd179 | 603 | //Negated in matrix multiplication. |
| gke | 2:90292f8bd179 | 604 | J_32 = 2.0*J_14or21; |
| gke | 2:90292f8bd179 | 605 | //Negated in matrix multiplication. |
| gke | 2:90292f8bd179 | 606 | J_33 = 2.0*J_11or24; |
| gke | 0:62a1c91a859a | 607 | |
| gke | 2:90292f8bd179 | 608 | //Compute the gradient (matrix multiplication). |
| gke | 2:90292f8bd179 | 609 | SEqHatDot_1 = J_14or21*f_2 - J_11or24*f_1; |
| gke | 2:90292f8bd179 | 610 | SEqHatDot_2 = J_12or23*f_1 + J_13or22*f_2 - J_32*f_3; |
| gke | 2:90292f8bd179 | 611 | SEqHatDot_3 = J_12or23*f_2 - J_33*f_3 - J_13or22*f_1; |
| gke | 2:90292f8bd179 | 612 | SEqHatDot_4 = J_14or21*f_1 + J_11or24*f_2; |
| gke | 2:90292f8bd179 | 613 | |
| gke | 2:90292f8bd179 | 614 | //Normalise the gradient. |
| gke | 2:90292f8bd179 | 615 | Renorm = 1.0 / sqrt(Sqr(SEqHatDot_1) + Sqr(SEqHatDot_2) + Sqr(SEqHatDot_3) + Sqr(SEqHatDot_4)); |
| gke | 2:90292f8bd179 | 616 | SEqHatDot_1 *= Renorm; |
| gke | 2:90292f8bd179 | 617 | SEqHatDot_2 *= Renorm; |
| gke | 2:90292f8bd179 | 618 | SEqHatDot_3 *= Renorm; |
| gke | 2:90292f8bd179 | 619 | SEqHatDot_4 *= Renorm; |
| gke | 2:90292f8bd179 | 620 | |
| gke | 2:90292f8bd179 | 621 | //Compute then integrate the estimated quaternion rate. |
| gke | 2:90292f8bd179 | 622 | SEq_1 += (SEqDot_omega_1 - (BetaIMU2*SEqHatDot_1))*dT; |
| gke | 2:90292f8bd179 | 623 | SEq_2 += (SEqDot_omega_2 - (BetaIMU2*SEqHatDot_2))*dT; |
| gke | 2:90292f8bd179 | 624 | SEq_3 += (SEqDot_omega_3 - (BetaIMU2*SEqHatDot_3))*dT; |
| gke | 2:90292f8bd179 | 625 | SEq_4 += (SEqDot_omega_4 - (BetaIMU2*SEqHatDot_4))*dT; |
| gke | 0:62a1c91a859a | 626 | |
| gke | 2:90292f8bd179 | 627 | //Normalise quaternion |
| gke | 2:90292f8bd179 | 628 | Renorm = 1.0 / sqrt(Sqr(SEq_1) + Sqr(SEq_2) + Sqr(SEq_3) + Sqr(SEq_4)); |
| gke | 2:90292f8bd179 | 629 | SEq_1 *= Renorm; |
| gke | 2:90292f8bd179 | 630 | SEq_2 *= Renorm; |
| gke | 2:90292f8bd179 | 631 | SEq_3 *= Renorm; |
| gke | 2:90292f8bd179 | 632 | SEq_4 *= Renorm; |
| gke | 0:62a1c91a859a | 633 | |
| gke | 2:90292f8bd179 | 634 | if ( FirstIMU2 ) { |
| gke | 2:90292f8bd179 | 635 | //Store orientation of auxiliary frame. |
| gke | 2:90292f8bd179 | 636 | AEq_1 = SEq_1; |
| gke | 2:90292f8bd179 | 637 | AEq_2 = SEq_2; |
| gke | 2:90292f8bd179 | 638 | AEq_3 = SEq_3; |
| gke | 2:90292f8bd179 | 639 | AEq_4 = SEq_4; |
| gke | 2:90292f8bd179 | 640 | FirstIMU2 = false; |
| gke | 2:90292f8bd179 | 641 | } |
| gke | 2:90292f8bd179 | 642 | |
| gke | 2:90292f8bd179 | 643 | MadgwickEulerAngles(MadgwickIMU2); |
| gke | 2:90292f8bd179 | 644 | |
| gke | 2:90292f8bd179 | 645 | } else |
| gke | 2:90292f8bd179 | 646 | for ( g = 0; g <(uint8)3; g++) { |
| gke | 2:90292f8bd179 | 647 | EstRate[g][MadgwickIMU2] = Gyro[g]; |
| gke | 2:90292f8bd179 | 648 | EstAngle[g][MadgwickIMU2] += EstRate[g][MadgwickIMU2]*dT; |
| gke | 2:90292f8bd179 | 649 | } |
| gke | 2:90292f8bd179 | 650 | |
| gke | 2:90292f8bd179 | 651 | } // DoMadgwickIMU2 |
| gke | 2:90292f8bd179 | 652 | |
| gke | 2:90292f8bd179 | 653 | #endif // INC_IMU2 |
| gke | 0:62a1c91a859a | 654 | |
| gke | 0:62a1c91a859a | 655 | //_________________________________________________________________________________ |
| gke | 0:62a1c91a859a | 656 | |
| gke | 0:62a1c91a859a | 657 | // AHRS.c |
| gke | 0:62a1c91a859a | 658 | // S.O.H. Madgwick |
| gke | 0:62a1c91a859a | 659 | // 25th August 2010 |
| gke | 0:62a1c91a859a | 660 | |
| gke | 0:62a1c91a859a | 661 | // Description: |
| gke | 0:62a1c91a859a | 662 | |
| gke | 0:62a1c91a859a | 663 | // Quaternion implementation of the 'DCM filter' [Mahoney et al]. Incorporates the magnetic distortion |
| gke | 0:62a1c91a859a | 664 | // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference |
| gke | 0:62a1c91a859a | 665 | // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw |
| gke | 2:90292f8bd179 | 666 | // a only. |
| gke | 0:62a1c91a859a | 667 | |
| gke | 2:90292f8bd179 | 668 | // User must define 'dTOn2' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'. |
| gke | 0:62a1c91a859a | 669 | |
| gke | 0:62a1c91a859a | 670 | // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated |
| gke | 0:62a1c91a859a | 671 | // orientation. See my report for an overview of the use of quaternions in this application. |
| gke | 0:62a1c91a859a | 672 | |
| gke | 0:62a1c91a859a | 673 | // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'), |
| gke | 2:90292f8bd179 | 674 | // accelerometer ('ax', 'ay', 'az') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are |
| gke | 0:62a1c91a859a | 675 | // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised. |
| gke | 0:62a1c91a859a | 676 | |
| gke | 2:90292f8bd179 | 677 | void DoMadgwickAHRS(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) { |
| gke | 0:62a1c91a859a | 678 | |
| gke | 2:90292f8bd179 | 679 | static uint8 g; |
| gke | 2:90292f8bd179 | 680 | static real32 ReNorm; |
| gke | 0:62a1c91a859a | 681 | static real32 hx, hy, hz, bx2, bz2, mx2, my2, mz2; |
| gke | 0:62a1c91a859a | 682 | static real32 vx, vy, vz, wx, wy, wz; |
| gke | 0:62a1c91a859a | 683 | static real32 ex, ey, ez; |
| gke | 0:62a1c91a859a | 684 | static real32 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; |
| gke | 2:90292f8bd179 | 685 | |
| gke | 2:90292f8bd179 | 686 | if ( F.AccMagnitudeOK ) { |
| gke | 2:90292f8bd179 | 687 | |
| gke | 2:90292f8bd179 | 688 | // auxiliary variables to reduce number of repeated operations |
| gke | 2:90292f8bd179 | 689 | q0q0 = q0*q0; |
| gke | 2:90292f8bd179 | 690 | q0q1 = q0*q1; |
| gke | 2:90292f8bd179 | 691 | q0q2 = q0*q2; |
| gke | 2:90292f8bd179 | 692 | q0q3 = q0*q3; |
| gke | 2:90292f8bd179 | 693 | q1q1 = q1*q1; |
| gke | 2:90292f8bd179 | 694 | q1q2 = q1*q2; |
| gke | 2:90292f8bd179 | 695 | q1q3 = q1*q3; |
| gke | 2:90292f8bd179 | 696 | q2q2 = q2*q2; |
| gke | 2:90292f8bd179 | 697 | q2q3 = q2*q3; |
| gke | 2:90292f8bd179 | 698 | q3q3 = q3*q3; |
| gke | 2:90292f8bd179 | 699 | |
| gke | 2:90292f8bd179 | 700 | ReNorm = 1.0 / sqrt( Sqr( mx ) + Sqr( my ) + Sqr( mz ) ); |
| gke | 2:90292f8bd179 | 701 | mx *= ReNorm; |
| gke | 2:90292f8bd179 | 702 | my *= ReNorm; |
| gke | 2:90292f8bd179 | 703 | mz *= ReNorm; |
| gke | 2:90292f8bd179 | 704 | mx2 = 2.0*mx; |
| gke | 2:90292f8bd179 | 705 | my2 = 2.0*my; |
| gke | 2:90292f8bd179 | 706 | mz2 = 2.0*mz; |
| gke | 2:90292f8bd179 | 707 | |
| gke | 2:90292f8bd179 | 708 | // compute reference direction of flux |
| gke | 2:90292f8bd179 | 709 | hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2); |
| gke | 2:90292f8bd179 | 710 | hy = mx2*(q1q2 + q0q3) + my2*( 0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1); |
| gke | 2:90292f8bd179 | 711 | hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*( 0.5 - q1q1 - q2q2 ); |
| gke | 2:90292f8bd179 | 712 | bx2 = 2.0*sqrt( Sqr( hx ) + Sqr( hy ) ); |
| gke | 2:90292f8bd179 | 713 | bz2 = 2.0*hz; |
| gke | 2:90292f8bd179 | 714 | |
| gke | 2:90292f8bd179 | 715 | // estimated direction of gravity and flux (v and w) |
| gke | 2:90292f8bd179 | 716 | vx = 2.0*(q1q3 - q0q2); |
| gke | 2:90292f8bd179 | 717 | vy = 2.0*(q0q1 + q2q3); |
| gke | 2:90292f8bd179 | 718 | vz = q0q0 - q1q1 - q2q2 + q3q3; |
| gke | 2:90292f8bd179 | 719 | |
| gke | 2:90292f8bd179 | 720 | wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2); |
| gke | 2:90292f8bd179 | 721 | wy = bx2*(q1q2 - q0q3) + bz2*( q0q1 + q2q3 ); |
| gke | 2:90292f8bd179 | 722 | wz = bx2*(q0q2 + q1q3) + bz2*( 0.5 - q1q1 - q2q2 ); |
| gke | 2:90292f8bd179 | 723 | |
| gke | 2:90292f8bd179 | 724 | // error is sum of cross product between reference direction of fields and direction measured by sensors |
| gke | 2:90292f8bd179 | 725 | ex = (ay*vz - az*vy) + (my*wz - mz*wy); |
| gke | 2:90292f8bd179 | 726 | ey = (az*vx - ax*vz) + (mz*wx - mx*wz); |
| gke | 2:90292f8bd179 | 727 | ez = (ax*vy - ay*vx) + (mx*wy - my*wx); |
| gke | 2:90292f8bd179 | 728 | |
| gke | 2:90292f8bd179 | 729 | // integral error scaled integral gain |
| gke | 2:90292f8bd179 | 730 | exInt += ex*MKi*dT; |
| gke | 2:90292f8bd179 | 731 | eyInt += ey*MKi*dT; |
| gke | 2:90292f8bd179 | 732 | ezInt += ez*MKi*dT; |
| gke | 2:90292f8bd179 | 733 | |
| gke | 2:90292f8bd179 | 734 | // adjusted gyroscope measurements |
| gke | 2:90292f8bd179 | 735 | gx += MKp*ex + exInt; |
| gke | 2:90292f8bd179 | 736 | gy += MKp*ey + eyInt; |
| gke | 2:90292f8bd179 | 737 | gz += MKp*ez + ezInt; |
| gke | 2:90292f8bd179 | 738 | |
| gke | 2:90292f8bd179 | 739 | // integrate quaternion rate and normalise |
| gke | 2:90292f8bd179 | 740 | q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2; |
| gke | 2:90292f8bd179 | 741 | q1 += (q0*gx + q2*gz - q3*gy)*dTOn2; |
| gke | 2:90292f8bd179 | 742 | q2 += (q0*gy - q1*gz + q3*gx)*dTOn2; |
| gke | 2:90292f8bd179 | 743 | q3 += (q0*gz + q1*gy - q2*gx)*dTOn2; |
| gke | 2:90292f8bd179 | 744 | |
| gke | 2:90292f8bd179 | 745 | // normalise quaternion |
| gke | 2:90292f8bd179 | 746 | ReNorm = 1.0 / sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3)); |
| gke | 2:90292f8bd179 | 747 | q0 *= ReNorm; |
| gke | 2:90292f8bd179 | 748 | q1 *= ReNorm; |
| gke | 2:90292f8bd179 | 749 | q2 *= ReNorm; |
| gke | 2:90292f8bd179 | 750 | q3 *= ReNorm; |
| gke | 2:90292f8bd179 | 751 | |
| gke | 2:90292f8bd179 | 752 | MadgwickEulerAngles(MadgwickAHRS); |
| gke | 2:90292f8bd179 | 753 | |
| gke | 2:90292f8bd179 | 754 | } else |
| gke | 2:90292f8bd179 | 755 | for ( g = 0; g <(uint8)3; g++) { |
| gke | 2:90292f8bd179 | 756 | EstRate[g][MadgwickAHRS] = Gyro[g]; |
| gke | 2:90292f8bd179 | 757 | EstAngle[g][MadgwickAHRS] += EstRate[g][MadgwickAHRS]*dT; |
| gke | 2:90292f8bd179 | 758 | } |
| gke | 2:90292f8bd179 | 759 | |
| gke | 2:90292f8bd179 | 760 | } // DoMadgwickAHRS |
| gke | 2:90292f8bd179 | 761 | |
| gke | 2:90292f8bd179 | 762 | void MadgwickEulerAngles(uint8 S) { |
| gke | 2:90292f8bd179 | 763 | |
| gke | 2:90292f8bd179 | 764 | EstAngle[Roll][S] = atan2(2.0*q2*q3 - 2.0*q0*q1, 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0); |
| gke | 2:90292f8bd179 | 765 | EstAngle[Pitch][S] = asin(2.0*q1*q2 - 2.0*q0*q2); |
| gke | 2:90292f8bd179 | 766 | EstAngle[Yaw][S] = atan2(2.0*q1*q2 - 2.0*q0*q3, 2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0); |
| gke | 2:90292f8bd179 | 767 | |
| gke | 2:90292f8bd179 | 768 | } // MadgwickEulerAngles |
| gke | 2:90292f8bd179 | 769 | |
| gke | 2:90292f8bd179 | 770 | //_________________________________________________________________________________ |
| gke | 2:90292f8bd179 | 771 | |
| gke | 2:90292f8bd179 | 772 | // Kalman Filter originally authored by Tom Pycke |
| gke | 2:90292f8bd179 | 773 | // http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data |
| gke | 0:62a1c91a859a | 774 | |
| gke | 2:90292f8bd179 | 775 | real32 AngleKF[2] = {0,0}; |
| gke | 2:90292f8bd179 | 776 | real32 BiasKF[2] = {0,0}; |
| gke | 2:90292f8bd179 | 777 | real32 P00[2] = {0,0}; |
| gke | 2:90292f8bd179 | 778 | real32 P01[2] = {0,0}; |
| gke | 2:90292f8bd179 | 779 | real32 P10[2] = {0,0}; |
| gke | 2:90292f8bd179 | 780 | real32 P11[2] = {0,0}; |
| gke | 2:90292f8bd179 | 781 | |
| gke | 2:90292f8bd179 | 782 | real32 KalmanFilter(uint8 a, real32 NewAngle, real32 NewRate) { |
| gke | 2:90292f8bd179 | 783 | |
| gke | 2:90292f8bd179 | 784 | // Q is a 2x2 matrix that represents the process covariance noise. |
| gke | 2:90292f8bd179 | 785 | // In this case, it indicates how much we trust the accelerometer |
| gke | 2:90292f8bd179 | 786 | // relative to the gyros. |
| gke | 2:90292f8bd179 | 787 | const real32 AngleQ = 0.003; |
| gke | 2:90292f8bd179 | 788 | const real32 GyroQ = 0.009; |
| gke | 2:90292f8bd179 | 789 | |
| gke | 2:90292f8bd179 | 790 | // R represents the measurement covariance noise. In this case, |
| gke | 2:90292f8bd179 | 791 | // it is a 1x1 matrix that says that we expect AngleR rad jitter |
| gke | 2:90292f8bd179 | 792 | // from the accelerometer. |
| gke | 2:90292f8bd179 | 793 | const real32 AngleR = GYRO_PROP_NOISE; |
| gke | 2:90292f8bd179 | 794 | |
| gke | 2:90292f8bd179 | 795 | static real32 y, S; |
| gke | 2:90292f8bd179 | 796 | static real32 K0, K1; |
| gke | 2:90292f8bd179 | 797 | |
| gke | 2:90292f8bd179 | 798 | AngleKF[a] += (NewRate - BiasKF[a])*dT; |
| gke | 2:90292f8bd179 | 799 | P00[a] -= (( P10[a] + P01[a] ) + AngleQ )*dT; |
| gke | 2:90292f8bd179 | 800 | P01[a] -= P11[a]*dT; |
| gke | 2:90292f8bd179 | 801 | P10[a] -= P11[a]*dT; |
| gke | 2:90292f8bd179 | 802 | P11[a] += GyroQ*dT; |
| gke | 2:90292f8bd179 | 803 | |
| gke | 2:90292f8bd179 | 804 | y = NewAngle - AngleKF[a]; |
| gke | 2:90292f8bd179 | 805 | S = 1.0 / ( P00[a] + AngleR ); |
| gke | 2:90292f8bd179 | 806 | K0 = P00[a]*S; |
| gke | 2:90292f8bd179 | 807 | K1 = P10[a]*S; |
| gke | 2:90292f8bd179 | 808 | |
| gke | 2:90292f8bd179 | 809 | AngleKF[a] += K0*y; |
| gke | 2:90292f8bd179 | 810 | BiasKF[a] += K1*y; |
| gke | 2:90292f8bd179 | 811 | P00[a] -= K0*P00[a]; |
| gke | 2:90292f8bd179 | 812 | P01[a] -= K0*P01[a]; |
| gke | 2:90292f8bd179 | 813 | P10[a] -= K1*P00[a]; |
| gke | 2:90292f8bd179 | 814 | P11[a] -= K1*P01[a]; |
| gke | 2:90292f8bd179 | 815 | |
| gke | 2:90292f8bd179 | 816 | return ( AngleKF[a] ); |
| gke | 2:90292f8bd179 | 817 | |
| gke | 2:90292f8bd179 | 818 | } // KalmanFilter |
| gke | 0:62a1c91a859a | 819 | |
| gke | 2:90292f8bd179 | 820 | void DoKalman(void) { // NO YAW ANGLE ESTIMATE |
| gke | 2:90292f8bd179 | 821 | EstAngle[Roll][Kalman] = KalmanFilter(Roll, asin(-Acc[LR]), Gyro[Roll]); |
| gke | 2:90292f8bd179 | 822 | EstAngle[Pitch][Kalman] = KalmanFilter(Pitch, asin(Acc[BF]), Gyro[Pitch]); |
| gke | 2:90292f8bd179 | 823 | EstRate[Roll][Kalman] = Gyro[Roll]; |
| gke | 2:90292f8bd179 | 824 | EstRate[Pitch][Kalman] = Gyro[Pitch]; |
| gke | 2:90292f8bd179 | 825 | } // DoKalman |
| gke | 2:90292f8bd179 | 826 | |
| gke | 2:90292f8bd179 | 827 | |
| gke | 2:90292f8bd179 | 828 | //_________________________________________________________________________________ |
| gke | 2:90292f8bd179 | 829 | |
| gke | 2:90292f8bd179 | 830 | // MultWii |
| gke | 2:90292f8bd179 | 831 | // Original code by Alex at: http://radio-commande.com/international/triwiicopter-design/ |
| gke | 2:90292f8bd179 | 832 | // simplified IMU based on Kalman Filter |
| gke | 2:90292f8bd179 | 833 | // inspired from http://starlino.com/imu_guide.html |
| gke | 2:90292f8bd179 | 834 | // and http://www.starlino.com/imu_kalman_arduino.html |
| gke | 2:90292f8bd179 | 835 | // with this algorithm, we can get absolute angles for a stable mode integration |
| gke | 2:90292f8bd179 | 836 | |
| gke | 2:90292f8bd179 | 837 | real32 AccMW[3] = {0.0, 0.0, 1.0}; // init acc in stable mode |
| gke | 2:90292f8bd179 | 838 | real32 GyroMW[3] = {0.0, 0.0, 0.0}; // R obtained from last estimated value and gyro movement |
| gke | 2:90292f8bd179 | 839 | real32 Axz, Ayz; // angles between projection of R on XZ/YZ plane and Z axis |
| gke | 2:90292f8bd179 | 840 | |
| gke | 2:90292f8bd179 | 841 | void DoMultiWii(void) { // V1.6 NO YAW ANGLE ESTIMATE |
| gke | 2:90292f8bd179 | 842 | |
| gke | 2:90292f8bd179 | 843 | const real32 GyroWt = 50.0; // gyro weight/smoothing factor |
| gke | 2:90292f8bd179 | 844 | const real32 GyroWtR = 1.0 / GyroWt; |
| gke | 2:90292f8bd179 | 845 | |
| gke | 2:90292f8bd179 | 846 | if ( Acc[UD] < 0.0 ) { // check not inverted |
| gke | 2:90292f8bd179 | 847 | |
| gke | 2:90292f8bd179 | 848 | if ( F.AccMagnitudeOK ) { |
| gke | 2:90292f8bd179 | 849 | Ayz = atan2( AccMW[LR], AccMW[UD] ) + Gyro[Roll]*dT; |
| gke | 2:90292f8bd179 | 850 | Axz = atan2( AccMW[BF], AccMW[UD] ) + Gyro[Pitch]*dT; |
| gke | 2:90292f8bd179 | 851 | } else { |
| gke | 2:90292f8bd179 | 852 | Ayz += Gyro[Roll]*dT; |
| gke | 2:90292f8bd179 | 853 | Axz += Gyro[Pitch]*dT; |
| gke | 2:90292f8bd179 | 854 | } |
| gke | 2:90292f8bd179 | 855 | |
| gke | 2:90292f8bd179 | 856 | // reverse calculation of GyroMW from Awz angles, |
| gke | 2:90292f8bd179 | 857 | // for formulae deduction see http://starlino.com/imu_guide.html |
| gke | 2:90292f8bd179 | 858 | GyroMW[Roll] = sin( Ayz ) / sqrt( 1.0 + Sqr( cos( Ayz ) )*Sqr( tan( Axz ) ) ); |
| gke | 2:90292f8bd179 | 859 | GyroMW[Pitch] = sin( Axz ) / sqrt( 1.0 + Sqr( cos( Axz ) )*Sqr( tan( Ayz ) ) ); |
| gke | 2:90292f8bd179 | 860 | GyroMW[Yaw] = sqrt( fabs( 1.0 - Sqr( GyroMW[Roll] ) - Sqr( GyroMW[Pitch] ) ) ); |
| gke | 2:90292f8bd179 | 861 | |
| gke | 2:90292f8bd179 | 862 | //combine accelerometer and gyro readings |
| gke | 2:90292f8bd179 | 863 | AccMW[LR] = ( -Acc[LR] + GyroWt*GyroMW[Roll] )*GyroWtR; |
| gke | 2:90292f8bd179 | 864 | AccMW[BF] = ( Acc[BF] + GyroWt*GyroMW[Pitch] )*GyroWtR; |
| gke | 2:90292f8bd179 | 865 | AccMW[UD] = ( -Acc[UD] + GyroWt*GyroMW[Yaw] )*GyroWtR; |
| gke | 0:62a1c91a859a | 866 | } |
| gke | 0:62a1c91a859a | 867 | |
| gke | 2:90292f8bd179 | 868 | EstAngle[Roll][MultiWii] = Ayz; |
| gke | 2:90292f8bd179 | 869 | EstAngle[Pitch][MultiWii] = Axz; |
| gke | 0:62a1c91a859a | 870 | |
| gke | 2:90292f8bd179 | 871 | // EstRate[Roll][MultiWii] = GyroMW[Roll]; |
| gke | 2:90292f8bd179 | 872 | // EstRate[Pitch][MultiWii] = GyroMW[Pitch]; |
| gke | 2:90292f8bd179 | 873 | |
| gke | 2:90292f8bd179 | 874 | EstRate[Roll][MultiWii] = Gyro[Roll]; |
| gke | 2:90292f8bd179 | 875 | EstRate[Pitch][MultiWii] = Gyro[Pitch]; |
| gke | 2:90292f8bd179 | 876 | |
| gke | 2:90292f8bd179 | 877 | } // DoMultiWii |
| gke | 0:62a1c91a859a | 878 | |
| gke | 2:90292f8bd179 | 879 | //_________________________________________________________________________________ |
| gke | 0:62a1c91a859a | 880 | |
| gke | 2:90292f8bd179 | 881 | void AttitudeTest(void) { |
| gke | 2:90292f8bd179 | 882 | |
| gke | 2:90292f8bd179 | 883 | TxString("\r\nAttitude Test\r\n"); |
| gke | 0:62a1c91a859a | 884 | |
| gke | 2:90292f8bd179 | 885 | GetAttitude(); |
| gke | 0:62a1c91a859a | 886 | |
| gke | 2:90292f8bd179 | 887 | TxString("\r\ndT \t"); |
| gke | 2:90292f8bd179 | 888 | TxVal32(dT*1000.0, 3, 0); |
| gke | 2:90292f8bd179 | 889 | TxString(" Sec.\r\n\r\n"); |
| gke | 2:90292f8bd179 | 890 | |
| gke | 2:90292f8bd179 | 891 | if ( GyroType == IRSensors ) { |
| gke | 0:62a1c91a859a | 892 | |
| gke | 2:90292f8bd179 | 893 | TxString("IR Sensors:\r\n"); |
| gke | 2:90292f8bd179 | 894 | TxString("\tRoll \t"); |
| gke | 2:90292f8bd179 | 895 | TxVal32(IR[Roll]*100.0, 2, HT); |
| gke | 2:90292f8bd179 | 896 | TxNextLine(); |
| gke | 2:90292f8bd179 | 897 | TxString("\tPitch\t"); |
| gke | 2:90292f8bd179 | 898 | TxVal32(IR[Pitch]*100.0, 2, HT); |
| gke | 2:90292f8bd179 | 899 | TxNextLine(); |
| gke | 2:90292f8bd179 | 900 | TxString("\tZ \t"); |
| gke | 2:90292f8bd179 | 901 | TxVal32(IR[Yaw]*100.0, 2, HT); |
| gke | 2:90292f8bd179 | 902 | TxNextLine(); |
| gke | 2:90292f8bd179 | 903 | TxString("\tMin/Max\t"); |
| gke | 2:90292f8bd179 | 904 | TxVal32(IRMin*100.0, 2, HT); |
| gke | 2:90292f8bd179 | 905 | TxVal32(IRMax*100.0, 2, HT); |
| gke | 2:90292f8bd179 | 906 | TxString("\tSwing\t"); |
| gke | 2:90292f8bd179 | 907 | TxVal32(IRSwing*100.0, 2, HT); |
| gke | 2:90292f8bd179 | 908 | TxNextLine(); |
| gke | 0:62a1c91a859a | 909 | |
| gke | 2:90292f8bd179 | 910 | } else { |
| gke | 0:62a1c91a859a | 911 | |
| gke | 2:90292f8bd179 | 912 | TxString("Gyro, Compensated, Max Delta(Deg./Sec.):\r\n"); |
| gke | 2:90292f8bd179 | 913 | TxString("\tRoll \t"); |
| gke | 2:90292f8bd179 | 914 | TxVal32(Gyro[Roll]*MILLIANGLE, 3, HT); |
| gke | 2:90292f8bd179 | 915 | TxVal32(Rate[Roll]*MILLIANGLE, 3, HT); |
| gke | 2:90292f8bd179 | 916 | TxVal32(GyroNoise[Roll]*MILLIANGLE,3, 0); |
| gke | 2:90292f8bd179 | 917 | TxNextLine(); |
| gke | 2:90292f8bd179 | 918 | TxString("\tPitch\t"); |
| gke | 2:90292f8bd179 | 919 | TxVal32(Gyro[Pitch]*MILLIANGLE, 3, HT); |
| gke | 2:90292f8bd179 | 920 | TxVal32(Rate[Pitch]*MILLIANGLE, 3, HT); |
| gke | 2:90292f8bd179 | 921 | TxVal32(GyroNoise[Pitch]*MILLIANGLE,3, 0); |
| gke | 2:90292f8bd179 | 922 | TxNextLine(); |
| gke | 2:90292f8bd179 | 923 | TxString("\tYaw \t"); |
| gke | 2:90292f8bd179 | 924 | TxVal32(Gyro[Yaw]*MILLIANGLE, 3, HT); |
| gke | 2:90292f8bd179 | 925 | TxVal32(Rate[Yaw]*MILLIANGLE, 3, HT); |
| gke | 2:90292f8bd179 | 926 | TxVal32(GyroNoise[Yaw]*MILLIANGLE, 3, 0); |
| gke | 2:90292f8bd179 | 927 | TxNextLine(); |
| gke | 0:62a1c91a859a | 928 | |
| gke | 2:90292f8bd179 | 929 | TxString("Accelerations , peak change(G):\r\n"); |
| gke | 2:90292f8bd179 | 930 | TxString("\tB->F \t"); |
| gke | 2:90292f8bd179 | 931 | TxVal32(Acc[BF]*1000.0, 3, HT); |
| gke | 2:90292f8bd179 | 932 | TxVal32( AccNoise[BF]*1000.0, 3, 0); |
| gke | 2:90292f8bd179 | 933 | TxNextLine(); |
| gke | 2:90292f8bd179 | 934 | TxString("\tL->R \t"); |
| gke | 2:90292f8bd179 | 935 | TxVal32(Acc[LR]*1000.0, 3, HT); |
| gke | 2:90292f8bd179 | 936 | TxVal32( AccNoise[LR]*1000.0, 3, 0); |
| gke | 2:90292f8bd179 | 937 | TxNextLine(); |
| gke | 2:90292f8bd179 | 938 | TxString("\tU->D \t"); |
| gke | 2:90292f8bd179 | 939 | TxVal32(Acc[UD]*1000.0, 3, HT); |
| gke | 2:90292f8bd179 | 940 | TxVal32( AccNoise[UD]*1000.0, 3, 0); |
| gke | 2:90292f8bd179 | 941 | TxNextLine(); |
| gke | 0:62a1c91a859a | 942 | } |
| gke | 0:62a1c91a859a | 943 | |
| gke | 2:90292f8bd179 | 944 | if ( CompassType != HMC6352 ) { |
| gke | 2:90292f8bd179 | 945 | TxString("Magnetic:\r\n"); |
| gke | 2:90292f8bd179 | 946 | TxString("\tX \t"); |
| gke | 2:90292f8bd179 | 947 | TxVal32(Mag[Roll].V, 0, 0); |
| gke | 2:90292f8bd179 | 948 | TxNextLine(); |
| gke | 2:90292f8bd179 | 949 | TxString("\tY \t"); |
| gke | 2:90292f8bd179 | 950 | TxVal32(Mag[Pitch].V, 0, 0); |
| gke | 2:90292f8bd179 | 951 | TxNextLine(); |
| gke | 2:90292f8bd179 | 952 | TxString("\tZ \t"); |
| gke | 2:90292f8bd179 | 953 | TxVal32(Mag[Yaw].V, 0, 0); |
| gke | 2:90292f8bd179 | 954 | TxNextLine(); |
| gke | 2:90292f8bd179 | 955 | } |
| gke | 0:62a1c91a859a | 956 | |
| gke | 2:90292f8bd179 | 957 | TxString("Heading: \t"); |
| gke | 2:90292f8bd179 | 958 | TxVal32(Make2Pi(Heading)*MILLIANGLE, 3, 0); |
| gke | 2:90292f8bd179 | 959 | TxNextLine(); |
| gke | 2:90292f8bd179 | 960 | |
| gke | 2:90292f8bd179 | 961 | } // AttitudeTest |
| gke | 2:90292f8bd179 | 962 | |
| gke | 2:90292f8bd179 | 963 | void InitAttitude(void) { |
| gke | 2:90292f8bd179 | 964 | |
| gke | 2:90292f8bd179 | 965 | static uint8 a, s; |
| gke | 2:90292f8bd179 | 966 | |
| gke | 2:90292f8bd179 | 967 | #ifdef INC_IMU2 |
| gke | 0:62a1c91a859a | 968 | |
| gke | 2:90292f8bd179 | 969 | FirstIMU2 = true; |
| gke | 2:90292f8bd179 | 970 | BetaIMU2 = sqrt(0.75) * GyroNoiseRadian[GyroType]; |
| gke | 2:90292f8bd179 | 971 | |
| gke | 2:90292f8bd179 | 972 | //Quaternion orientation of earth frame relative to auxiliary frame. |
| gke | 2:90292f8bd179 | 973 | AEq_1 = 1.0; |
| gke | 2:90292f8bd179 | 974 | AEq_2 = 0.0; |
| gke | 2:90292f8bd179 | 975 | AEq_3 = 0.0; |
| gke | 2:90292f8bd179 | 976 | AEq_4 = 0.0; |
| gke | 0:62a1c91a859a | 977 | |
| gke | 2:90292f8bd179 | 978 | //Estimated orientation quaternion elements with initial conditions. |
| gke | 2:90292f8bd179 | 979 | SEq_1 = 1.0; |
| gke | 2:90292f8bd179 | 980 | SEq_2 = 0.0; |
| gke | 2:90292f8bd179 | 981 | SEq_3 = 0.0; |
| gke | 2:90292f8bd179 | 982 | SEq_4 = 0.0; |
| gke | 0:62a1c91a859a | 983 | |
| gke | 2:90292f8bd179 | 984 | #endif // INC_IMU2 |
| gke | 0:62a1c91a859a | 985 | |
| gke | 2:90292f8bd179 | 986 | for ( a = 0; a < (uint8)2; a++ ) |
| gke | 2:90292f8bd179 | 987 | AngleCF[a] = AngleKF[a] = BiasKF[a] = F0[a] = F1[a] = F2[a] = 0.0; |
| gke | 2:90292f8bd179 | 988 | |
| gke | 2:90292f8bd179 | 989 | for ( a = 0; a < (uint8)3; a++ ) |
| gke | 2:90292f8bd179 | 990 | for ( s = 0; s < MaxAttitudeScheme; s++ ) |
| gke | 2:90292f8bd179 | 991 | EstAngle[a][s] = EstRate[a][s] = 0.0; |
| gke | 2:90292f8bd179 | 992 | |
| gke | 2:90292f8bd179 | 993 | } // InitAttitude |
| gke | 2:90292f8bd179 | 994 |