Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
attitude.c@0:62a1c91a859a, 2011-02-18 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 18 22:28:05 2011 +0000
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
First release
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 | 0:62a1c91a859a | 5 | // = http://code.google.com/p/uavp-mods/ http://uavp.ch = |
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 | //#define USE_GYRO_RATE |
gke | 0:62a1c91a859a | 24 | |
gke | 0:62a1c91a859a | 25 | // Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW. |
gke | 0:62a1c91a859a | 26 | |
gke | 0:62a1c91a859a | 27 | void AttitudeFailsafeEstimate(void); |
gke | 0:62a1c91a859a | 28 | void DoLegacyYawComp(void); |
gke | 0:62a1c91a859a | 29 | void AttitudeTest(void); |
gke | 0:62a1c91a859a | 30 | |
gke | 0:62a1c91a859a | 31 | real32 dT, HalfdT, dTR, dTmS; |
gke | 0:62a1c91a859a | 32 | uint32 uSp; |
gke | 0:62a1c91a859a | 33 | uint8 AttitudeMethod = PremerlaniDCM; //MadgwickIMU PremerlaniDCM MadgwickAHRS; |
gke | 0:62a1c91a859a | 34 | |
gke | 0:62a1c91a859a | 35 | void AttitudeFailsafeEstimate(void) { |
gke | 0:62a1c91a859a | 36 | |
gke | 0:62a1c91a859a | 37 | static uint8 i; |
gke | 0:62a1c91a859a | 38 | |
gke | 0:62a1c91a859a | 39 | for ( i = 0; i < (uint8)3; i++ ) { |
gke | 0:62a1c91a859a | 40 | Rate[i] = Gyro[i]; |
gke | 0:62a1c91a859a | 41 | Angle[i] += Rate[i] * dTmS; |
gke | 0:62a1c91a859a | 42 | } |
gke | 0:62a1c91a859a | 43 | } // AttitudeFailsafeEstimate |
gke | 0:62a1c91a859a | 44 | |
gke | 0:62a1c91a859a | 45 | void DoLegacyYawComp(void) { |
gke | 0:62a1c91a859a | 46 | |
gke | 0:62a1c91a859a | 47 | static real32 Temp, HE; |
gke | 0:62a1c91a859a | 48 | |
gke | 0:62a1c91a859a | 49 | if ( F.CompassValid ) { |
gke | 0:62a1c91a859a | 50 | // + CCW |
gke | 0:62a1c91a859a | 51 | Temp = DesiredYaw - Trim[Yaw]; |
gke | 0:62a1c91a859a | 52 | if ( fabs(Temp) > COMPASS_MIDDLE ) { // acquire new heading |
gke | 0:62a1c91a859a | 53 | DesiredHeading = Heading; |
gke | 0:62a1c91a859a | 54 | HE = 0.0; |
gke | 0:62a1c91a859a | 55 | } else { |
gke | 0:62a1c91a859a | 56 | HE = MakePi(DesiredHeading - Heading); |
gke | 0:62a1c91a859a | 57 | HE = Limit(HE, -SIXTHPI, SIXTHPI); // 30 deg limit |
gke | 0:62a1c91a859a | 58 | HE = HE * K[CompassKp]; |
gke | 0:62a1c91a859a | 59 | Rate[Yaw] -= Limit(HE, -COMPASS_MAXDEV, COMPASS_MAXDEV); |
gke | 0:62a1c91a859a | 60 | } |
gke | 0:62a1c91a859a | 61 | } |
gke | 0:62a1c91a859a | 62 | |
gke | 0:62a1c91a859a | 63 | Angle[Yaw] += Rate[Yaw]; |
gke | 0:62a1c91a859a | 64 | Angle[Yaw] = Limit(Angle[Yaw], -K[YawIntLimit], K[YawIntLimit]); |
gke | 0:62a1c91a859a | 65 | |
gke | 0:62a1c91a859a | 66 | Angle[Yaw] = DecayX(Angle[Yaw], 0.0002); // GKE added to kill gyro drift |
gke | 0:62a1c91a859a | 67 | |
gke | 0:62a1c91a859a | 68 | } // DoLegacyYawComp |
gke | 0:62a1c91a859a | 69 | |
gke | 0:62a1c91a859a | 70 | void GetAttitude(void) { |
gke | 0:62a1c91a859a | 71 | |
gke | 0:62a1c91a859a | 72 | static uint32 Now; |
gke | 0:62a1c91a859a | 73 | static uint8 i; |
gke | 0:62a1c91a859a | 74 | |
gke | 0:62a1c91a859a | 75 | if ( GyroType == IRSensors ) |
gke | 0:62a1c91a859a | 76 | GetIRAttitude(); |
gke | 0:62a1c91a859a | 77 | else { |
gke | 0:62a1c91a859a | 78 | GetGyroRates(); |
gke | 0:62a1c91a859a | 79 | GetAccelerations(); |
gke | 0:62a1c91a859a | 80 | } |
gke | 0:62a1c91a859a | 81 | |
gke | 0:62a1c91a859a | 82 | if ( mSClock() > mS[CompassUpdate] ) { |
gke | 0:62a1c91a859a | 83 | mS[CompassUpdate] = mSClock() + COMPASS_UPDATE_MS; |
gke | 0:62a1c91a859a | 84 | GetHeading(); |
gke | 0:62a1c91a859a | 85 | #ifndef USE_DCM_YAW |
gke | 0:62a1c91a859a | 86 | DoLegacyYawComp(); |
gke | 0:62a1c91a859a | 87 | #endif // !USE_DCM_YAW |
gke | 0:62a1c91a859a | 88 | } |
gke | 0:62a1c91a859a | 89 | |
gke | 0:62a1c91a859a | 90 | Now = uSClock(); |
gke | 0:62a1c91a859a | 91 | dT = ( Now - uSp) * 0.000001; |
gke | 0:62a1c91a859a | 92 | HalfdT = 0.5 * dT; |
gke | 0:62a1c91a859a | 93 | dTR = 1.0 / dT; |
gke | 0:62a1c91a859a | 94 | uSp = Now; |
gke | 0:62a1c91a859a | 95 | |
gke | 0:62a1c91a859a | 96 | if ( GyroType == IRSensors ) { |
gke | 0:62a1c91a859a | 97 | |
gke | 0:62a1c91a859a | 98 | for ( i = 0; i < (uint8)2; i++ ) { |
gke | 0:62a1c91a859a | 99 | Rate[i] = ( Angle[i] - Anglep[i] ) * dT; |
gke | 0:62a1c91a859a | 100 | Anglep[i] = Angle[i]; |
gke | 0:62a1c91a859a | 101 | } |
gke | 0:62a1c91a859a | 102 | |
gke | 0:62a1c91a859a | 103 | Rate[Yaw] = 0.0; // need Yaw gyro! |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | } else { |
gke | 0:62a1c91a859a | 106 | DebugPin = true; |
gke | 0:62a1c91a859a | 107 | switch ( AttitudeMethod ) { |
gke | 0:62a1c91a859a | 108 | case PremerlaniDCM: |
gke | 0:62a1c91a859a | 109 | DCMUpdate(); |
gke | 0:62a1c91a859a | 110 | DCMNormalise(); |
gke | 0:62a1c91a859a | 111 | DCMDriftCorrection(); |
gke | 0:62a1c91a859a | 112 | DCMEulerAngles(); |
gke | 0:62a1c91a859a | 113 | break; |
gke | 0:62a1c91a859a | 114 | case MadgwickIMU: |
gke | 0:62a1c91a859a | 115 | IMUupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD]); |
gke | 0:62a1c91a859a | 116 | EulerAngles(); |
gke | 0:62a1c91a859a | 117 | // DoLegacyYawComp(); |
gke | 0:62a1c91a859a | 118 | break; |
gke | 0:62a1c91a859a | 119 | case MadgwickAHRS: // must have magnetometer |
gke | 0:62a1c91a859a | 120 | AHRSupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD], 1,0,0);//Mag[BF].V, Mag[LR].V, Mag[UD].V); |
gke | 0:62a1c91a859a | 121 | EulerAngles(); |
gke | 0:62a1c91a859a | 122 | break; |
gke | 0:62a1c91a859a | 123 | } // switch |
gke | 0:62a1c91a859a | 124 | |
gke | 0:62a1c91a859a | 125 | DebugPin = false; |
gke | 0:62a1c91a859a | 126 | } |
gke | 0:62a1c91a859a | 127 | |
gke | 0:62a1c91a859a | 128 | F.NearLevel = Max(fabs(Angle[Roll]), fabs(Angle[Pitch])) < NAV_RTH_LOCKOUT; |
gke | 0:62a1c91a859a | 129 | |
gke | 0:62a1c91a859a | 130 | } // GetAttitude |
gke | 0:62a1c91a859a | 131 | |
gke | 0:62a1c91a859a | 132 | |
gke | 0:62a1c91a859a | 133 | void AttitudeTest(void) { |
gke | 0:62a1c91a859a | 134 | |
gke | 0:62a1c91a859a | 135 | TxString("\r\nAttitude Test\r\n"); |
gke | 0:62a1c91a859a | 136 | |
gke | 0:62a1c91a859a | 137 | GetAttitude(); |
gke | 0:62a1c91a859a | 138 | |
gke | 0:62a1c91a859a | 139 | TxString("\r\ndT \t"); |
gke | 0:62a1c91a859a | 140 | TxVal32(dT * 1000.0, 3, 0); |
gke | 0:62a1c91a859a | 141 | TxString(" Sec.\r\n\r\n"); |
gke | 0:62a1c91a859a | 142 | |
gke | 0:62a1c91a859a | 143 | if ( GyroType == IRSensors ) { |
gke | 0:62a1c91a859a | 144 | |
gke | 0:62a1c91a859a | 145 | TxString("IR Sensors:\r\n"); |
gke | 0:62a1c91a859a | 146 | TxString("\tRoll \t"); |
gke | 0:62a1c91a859a | 147 | TxVal32(IR[Roll] * 100.0, 2, HT); |
gke | 0:62a1c91a859a | 148 | TxNextLine(); |
gke | 0:62a1c91a859a | 149 | TxString("\tPitch\t"); |
gke | 0:62a1c91a859a | 150 | TxVal32(IR[Pitch] * 100.0, 2, HT); |
gke | 0:62a1c91a859a | 151 | TxNextLine(); |
gke | 0:62a1c91a859a | 152 | TxString("\tZ \t"); |
gke | 0:62a1c91a859a | 153 | TxVal32(IR[Yaw] * 100.0, 2, HT); |
gke | 0:62a1c91a859a | 154 | TxNextLine(); |
gke | 0:62a1c91a859a | 155 | TxString("\tMin/Max\t"); |
gke | 0:62a1c91a859a | 156 | TxVal32(IRMin * 100.0, 2, HT); |
gke | 0:62a1c91a859a | 157 | TxVal32(IRMax * 100.0, 2, HT); |
gke | 0:62a1c91a859a | 158 | TxString("\tSwing\t"); |
gke | 0:62a1c91a859a | 159 | TxVal32(IRSwing * 100.0, 2, HT); |
gke | 0:62a1c91a859a | 160 | TxNextLine(); |
gke | 0:62a1c91a859a | 161 | |
gke | 0:62a1c91a859a | 162 | } else { |
gke | 0:62a1c91a859a | 163 | |
gke | 0:62a1c91a859a | 164 | TxString("Rates (Raw and Compensated):\r\n"); |
gke | 0:62a1c91a859a | 165 | TxString("\tRoll \t"); |
gke | 0:62a1c91a859a | 166 | TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT); |
gke | 0:62a1c91a859a | 167 | TxVal32(Rate[Roll] * MILLIANGLE, 3, 0); |
gke | 0:62a1c91a859a | 168 | TxNextLine(); |
gke | 0:62a1c91a859a | 169 | TxString("\tPitch\t"); |
gke | 0:62a1c91a859a | 170 | TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT); |
gke | 0:62a1c91a859a | 171 | TxVal32(Rate[Pitch] * MILLIANGLE, 3, 0); |
gke | 0:62a1c91a859a | 172 | TxNextLine(); |
gke | 0:62a1c91a859a | 173 | TxString("\tYaw \t"); |
gke | 0:62a1c91a859a | 174 | TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT); |
gke | 0:62a1c91a859a | 175 | TxVal32(Rate[Yaw] * MILLIANGLE, 3, 0); |
gke | 0:62a1c91a859a | 176 | TxNextLine(); |
gke | 0:62a1c91a859a | 177 | |
gke | 0:62a1c91a859a | 178 | TxString("Accelerations:\r\n"); |
gke | 0:62a1c91a859a | 179 | TxString("\tB->F \t"); |
gke | 0:62a1c91a859a | 180 | TxVal32(Acc[BF] * 1000.0, 3, 0); |
gke | 0:62a1c91a859a | 181 | TxNextLine(); |
gke | 0:62a1c91a859a | 182 | TxString("\tL->R \t"); |
gke | 0:62a1c91a859a | 183 | TxVal32(Acc[LR] * 1000.0, 3, 0); |
gke | 0:62a1c91a859a | 184 | TxNextLine(); |
gke | 0:62a1c91a859a | 185 | TxString("\tU->D \t"); |
gke | 0:62a1c91a859a | 186 | TxVal32(Acc[UD] * 1000.0, 3, 0); |
gke | 0:62a1c91a859a | 187 | TxNextLine(); |
gke | 0:62a1c91a859a | 188 | } |
gke | 0:62a1c91a859a | 189 | |
gke | 0:62a1c91a859a | 190 | if ( CompassType != HMC6352 ) { |
gke | 0:62a1c91a859a | 191 | TxString("Magnetic:\r\n"); |
gke | 0:62a1c91a859a | 192 | TxString("\tX \t"); |
gke | 0:62a1c91a859a | 193 | TxVal32(Mag[Roll].V, 0, 0); |
gke | 0:62a1c91a859a | 194 | TxNextLine(); |
gke | 0:62a1c91a859a | 195 | TxString("\tY \t"); |
gke | 0:62a1c91a859a | 196 | TxVal32(Mag[Pitch].V, 0, 0); |
gke | 0:62a1c91a859a | 197 | TxNextLine(); |
gke | 0:62a1c91a859a | 198 | TxString("\tZ \t"); |
gke | 0:62a1c91a859a | 199 | TxVal32(Mag[Yaw].V, 0, 0); |
gke | 0:62a1c91a859a | 200 | TxNextLine(); |
gke | 0:62a1c91a859a | 201 | } |
gke | 0:62a1c91a859a | 202 | |
gke | 0:62a1c91a859a | 203 | TxString("Compass: \t"); |
gke | 0:62a1c91a859a | 204 | TxVal32(Make2Pi(MagHeading) * MILLIANGLE, 3, 0); |
gke | 0:62a1c91a859a | 205 | TxNextLine(); |
gke | 0:62a1c91a859a | 206 | |
gke | 0:62a1c91a859a | 207 | } // AttitudeTest |
gke | 0:62a1c91a859a | 208 | |
gke | 0:62a1c91a859a | 209 | //____________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 210 | |
gke | 0:62a1c91a859a | 211 | // The DCM formulation used here is due to W. Premerlani and P. Bizard in a paper entitled: |
gke | 0:62a1c91a859a | 212 | // Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original |
gke | 0:62a1c91a859a | 213 | // work by R. Mahony et al. - Thanks Rob! |
gke | 0:62a1c91a859a | 214 | |
gke | 0:62a1c91a859a | 215 | void DCMNormalise(void); |
gke | 0:62a1c91a859a | 216 | void DCMDriftCorrection(void); |
gke | 0:62a1c91a859a | 217 | void DCMMotionCompensation(void); |
gke | 0:62a1c91a859a | 218 | void DCMUpdate(void); |
gke | 0:62a1c91a859a | 219 | void DCMEulerAngles(void); |
gke | 0:62a1c91a859a | 220 | |
gke | 0:62a1c91a859a | 221 | // requires rescaling for the much faster PID cycle in UAVXArm |
gke | 0:62a1c91a859a | 222 | // guess x5 initially |
gke | 0:62a1c91a859a | 223 | #define Kp_RollPitch 25.0 // 5.0 |
gke | 0:62a1c91a859a | 224 | #define Ki_RollPitch 0.025 // 0.005 |
gke | 0:62a1c91a859a | 225 | #define Kp_Yaw 1.2 |
gke | 0:62a1c91a859a | 226 | #define Ki_Yaw 0.00002 |
gke | 0:62a1c91a859a | 227 | |
gke | 0:62a1c91a859a | 228 | real32 RollPitchError[3] = {0,0,0}; |
gke | 0:62a1c91a859a | 229 | real32 OmegaV[3] = {0,0,0}; // corrected gyro data |
gke | 0:62a1c91a859a | 230 | real32 OmegaP[3] = {0,0,0}; // proportional correction |
gke | 0:62a1c91a859a | 231 | real32 OmegaI[3] = {0,0,0}; // integral correction |
gke | 0:62a1c91a859a | 232 | real32 Omega[3] = {0,0,0}; |
gke | 0:62a1c91a859a | 233 | real32 DCM[3][3] = {{1,0,0 },{0,1,0} ,{0,0,1}}; |
gke | 0:62a1c91a859a | 234 | real32 U[3][3] = {{0,1,2},{ 3,4,5} ,{6,7,8}}; |
gke | 0:62a1c91a859a | 235 | real32 TempM[3][3] = {{0,0,0},{0,0,0},{ 0,0,0}}; |
gke | 0:62a1c91a859a | 236 | |
gke | 0:62a1c91a859a | 237 | void DCMNormalise(void) { |
gke | 0:62a1c91a859a | 238 | |
gke | 0:62a1c91a859a | 239 | static real32 Error = 0; |
gke | 0:62a1c91a859a | 240 | static real32 Renorm = 0.0; |
gke | 0:62a1c91a859a | 241 | static boolean Problem; |
gke | 0:62a1c91a859a | 242 | static uint8 r; |
gke | 0:62a1c91a859a | 243 | |
gke | 0:62a1c91a859a | 244 | Error = -VDot(&DCM[0][0], &DCM[1][0]) * 0.5; //eq.19 |
gke | 0:62a1c91a859a | 245 | |
gke | 0:62a1c91a859a | 246 | VScale(&TempM[0][0], &DCM[1][0], Error); //eq.19 |
gke | 0:62a1c91a859a | 247 | VScale(&TempM[1][0], &DCM[0][0], Error); //eq.19 |
gke | 0:62a1c91a859a | 248 | |
gke | 0:62a1c91a859a | 249 | VAdd(&TempM[0][0], &TempM[0][0], &DCM[0][0]); //eq.19 |
gke | 0:62a1c91a859a | 250 | VAdd(&TempM[1][0], &TempM[1][0], &DCM[1][0]); //eq.19 |
gke | 0:62a1c91a859a | 251 | |
gke | 0:62a1c91a859a | 252 | VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]); // c= a * b eq.20 |
gke | 0:62a1c91a859a | 253 | |
gke | 0:62a1c91a859a | 254 | Problem = false; |
gke | 0:62a1c91a859a | 255 | for ( r = 0; r < (uint8)3; r++ ) { |
gke | 0:62a1c91a859a | 256 | Renorm = VDot(&TempM[r][0],&TempM[r][0]); |
gke | 0:62a1c91a859a | 257 | if ( (Renorm < 1.5625) && (Renorm > 0.64) ) |
gke | 0:62a1c91a859a | 258 | Renorm = 0.5 * (3.0 - Renorm); //eq.21 |
gke | 0:62a1c91a859a | 259 | else |
gke | 0:62a1c91a859a | 260 | if ( (Renorm < 100.0) && (Renorm > 0.01) ) |
gke | 0:62a1c91a859a | 261 | Renorm = 1.0 / sqrt( Renorm ); |
gke | 0:62a1c91a859a | 262 | else |
gke | 0:62a1c91a859a | 263 | Problem = true; |
gke | 0:62a1c91a859a | 264 | |
gke | 0:62a1c91a859a | 265 | VScale(&DCM[r][0], &TempM[r][0], Renorm); |
gke | 0:62a1c91a859a | 266 | } |
gke | 0:62a1c91a859a | 267 | |
gke | 0:62a1c91a859a | 268 | if ( Problem ) { // Divergent - force to initial conditions and hope! |
gke | 0:62a1c91a859a | 269 | DCM[0][0] = 1.0; |
gke | 0:62a1c91a859a | 270 | DCM[0][1] = 0.0; |
gke | 0:62a1c91a859a | 271 | DCM[0][2] = 0.0; |
gke | 0:62a1c91a859a | 272 | DCM[1][0] = 0.0; |
gke | 0:62a1c91a859a | 273 | DCM[1][1] = 1.0; |
gke | 0:62a1c91a859a | 274 | DCM[1][2] = 0.0; |
gke | 0:62a1c91a859a | 275 | DCM[2][0] = 0.0; |
gke | 0:62a1c91a859a | 276 | DCM[2][1] = 0.0; |
gke | 0:62a1c91a859a | 277 | DCM[2][2] = 1.0; |
gke | 0:62a1c91a859a | 278 | } |
gke | 0:62a1c91a859a | 279 | |
gke | 0:62a1c91a859a | 280 | } // DCMNormalise |
gke | 0:62a1c91a859a | 281 | |
gke | 0:62a1c91a859a | 282 | void DCMMotionCompensation(void) { |
gke | 0:62a1c91a859a | 283 | // compensation for rate of change of velocity LR/BF needs GPS velocity but |
gke | 0:62a1c91a859a | 284 | // updates probably too slow? |
gke | 0:62a1c91a859a | 285 | Acc[LR ] += 0.0; |
gke | 0:62a1c91a859a | 286 | Acc[BF] += 0.0; |
gke | 0:62a1c91a859a | 287 | } // DCMMotionCompensation |
gke | 0:62a1c91a859a | 288 | |
gke | 0:62a1c91a859a | 289 | void DCMDriftCorrection(void) { |
gke | 0:62a1c91a859a | 290 | |
gke | 0:62a1c91a859a | 291 | static real32 ScaledOmegaP[3], ScaledOmegaI[3]; |
gke | 0:62a1c91a859a | 292 | static real32 YawError[3]; |
gke | 0:62a1c91a859a | 293 | static real32 AccMagnitude, AccWeight; |
gke | 0:62a1c91a859a | 294 | static real32 ErrorCourse; |
gke | 0:62a1c91a859a | 295 | |
gke | 0:62a1c91a859a | 296 | AccMagnitude = sqrt( Sqr(Acc[0]) + Sqr(Acc[1]) + Sqr(Acc[2]) ); |
gke | 0:62a1c91a859a | 297 | |
gke | 0:62a1c91a859a | 298 | // dynamic weighting of Accelerometer info (reliability filter) |
gke | 0:62a1c91a859a | 299 | // weight for Accelerometer info ( < 0.5G = 0.0, 1G = 1.0 , > 1.5G = 0.0) |
gke | 0:62a1c91a859a | 300 | AccWeight = Limit(1.0 - 2.0 * fabs(1.0 - AccMagnitude), 0.0, 1.0); |
gke | 0:62a1c91a859a | 301 | |
gke | 0:62a1c91a859a | 302 | VCross(&RollPitchError[0], &Acc[0], &DCM[2][0]); //adjust the reference ground |
gke | 0:62a1c91a859a | 303 | VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch * AccWeight); |
gke | 0:62a1c91a859a | 304 | |
gke | 0:62a1c91a859a | 305 | VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch * AccWeight); |
gke | 0:62a1c91a859a | 306 | VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]); |
gke | 0:62a1c91a859a | 307 | |
gke | 0:62a1c91a859a | 308 | #ifdef USE_DCM_YAW |
gke | 0:62a1c91a859a | 309 | // Yaw - drift correction based on compass/magnetometer heading |
gke | 0:62a1c91a859a | 310 | HeadingCos = cos( Heading ); |
gke | 0:62a1c91a859a | 311 | HeadingSin = sin( Heading ); |
gke | 0:62a1c91a859a | 312 | ErrorCourse = ( U[0][0] * HeadingSin ) - ( U[1][0] * HeadingCos ); |
gke | 0:62a1c91a859a | 313 | VScale(YawError, &U[2][0], ErrorCourse ); |
gke | 0:62a1c91a859a | 314 | |
gke | 0:62a1c91a859a | 315 | VScale(&ScaledOmegaP[0], &YawError[0], Kp_Yaw ); |
gke | 0:62a1c91a859a | 316 | VAdd(&OmegaP[0], &OmegaP[0], &ScaledOmegaP[0]); |
gke | 0:62a1c91a859a | 317 | |
gke | 0:62a1c91a859a | 318 | VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw ); |
gke | 0:62a1c91a859a | 319 | VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]); |
gke | 0:62a1c91a859a | 320 | #endif // USE_DCM_YAW |
gke | 0:62a1c91a859a | 321 | } // DCMDriftCorrection |
gke | 0:62a1c91a859a | 322 | |
gke | 0:62a1c91a859a | 323 | void DCMUpdate(void) { |
gke | 0:62a1c91a859a | 324 | |
gke | 0:62a1c91a859a | 325 | static uint8 i, j, k; |
gke | 0:62a1c91a859a | 326 | static real32 op[3]; |
gke | 0:62a1c91a859a | 327 | |
gke | 0:62a1c91a859a | 328 | VAdd(&Omega[0], &Gyro[0], &OmegaI[0]); |
gke | 0:62a1c91a859a | 329 | VAdd(&OmegaV[0], &Omega[0], &OmegaP[0]); |
gke | 0:62a1c91a859a | 330 | |
gke | 0:62a1c91a859a | 331 | // MotionCompensation(); |
gke | 0:62a1c91a859a | 332 | |
gke | 0:62a1c91a859a | 333 | U[0][0] = 0.0; |
gke | 0:62a1c91a859a | 334 | U[0][1] = -dT * OmegaV[2]; //-z |
gke | 0:62a1c91a859a | 335 | U[0][2] = dT * OmegaV[1]; // y |
gke | 0:62a1c91a859a | 336 | U[1][0] = dT * OmegaV[2]; // z |
gke | 0:62a1c91a859a | 337 | U[1][1] = 0.0; |
gke | 0:62a1c91a859a | 338 | U[1][2] = -dT * OmegaV[0]; //-x |
gke | 0:62a1c91a859a | 339 | U[2][0] = -dT * OmegaV[1]; //-y |
gke | 0:62a1c91a859a | 340 | U[2][1] = dT * OmegaV[0]; // x |
gke | 0:62a1c91a859a | 341 | U[2][2] = 0.0; |
gke | 0:62a1c91a859a | 342 | |
gke | 0:62a1c91a859a | 343 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 344 | for ( j = 0; j < (uint8)3; j++ ) { |
gke | 0:62a1c91a859a | 345 | for ( k = 0; k < (uint8)3; k++ ) |
gke | 0:62a1c91a859a | 346 | op[k] = DCM[i][k] * U[k][j]; |
gke | 0:62a1c91a859a | 347 | |
gke | 0:62a1c91a859a | 348 | TempM[i][j] = op[0] + op[1] + op[2]; |
gke | 0:62a1c91a859a | 349 | } |
gke | 0:62a1c91a859a | 350 | |
gke | 0:62a1c91a859a | 351 | for ( i = 0; i < (uint8)3; i++ ) |
gke | 0:62a1c91a859a | 352 | for (j = 0; j < (uint8)3; j++ ) |
gke | 0:62a1c91a859a | 353 | DCM[i][j] += TempM[i][j]; |
gke | 0:62a1c91a859a | 354 | |
gke | 0:62a1c91a859a | 355 | } // DCMUpdate |
gke | 0:62a1c91a859a | 356 | |
gke | 0:62a1c91a859a | 357 | void DCMEulerAngles(void) { |
gke | 0:62a1c91a859a | 358 | |
gke | 0:62a1c91a859a | 359 | static uint8 g; |
gke | 0:62a1c91a859a | 360 | |
gke | 0:62a1c91a859a | 361 | for ( g = 0; g < (uint8)3; g++ ) |
gke | 0:62a1c91a859a | 362 | Rate[g] = Gyro[g]; |
gke | 0:62a1c91a859a | 363 | |
gke | 0:62a1c91a859a | 364 | Angle[Pitch] = asin(DCM[2][0]); |
gke | 0:62a1c91a859a | 365 | Angle[Roll] = -atan2(DCM[2][1], DCM[2][2]); |
gke | 0:62a1c91a859a | 366 | |
gke | 0:62a1c91a859a | 367 | #ifdef USE_DCM_YAW |
gke | 0:62a1c91a859a | 368 | Angle[Yaw] = atan2(DCM[1][0], DCM[0][0]); |
gke | 0:62a1c91a859a | 369 | #endif // USE_DCM_YAW |
gke | 0:62a1c91a859a | 370 | |
gke | 0:62a1c91a859a | 371 | } // DCMEulerAngles |
gke | 0:62a1c91a859a | 372 | |
gke | 0:62a1c91a859a | 373 | //___________________________________________________________________________________ |
gke | 0:62a1c91a859a | 374 | |
gke | 0:62a1c91a859a | 375 | // IMU.c |
gke | 0:62a1c91a859a | 376 | // S.O.H. Madgwick |
gke | 0:62a1c91a859a | 377 | // 25th September 2010 |
gke | 0:62a1c91a859a | 378 | |
gke | 0:62a1c91a859a | 379 | // Description: |
gke | 0:62a1c91a859a | 380 | |
gke | 0:62a1c91a859a | 381 | // Quaternion implementation of the 'DCM filter' [Mahony et al.]. |
gke | 0:62a1c91a859a | 382 | |
gke | 0:62a1c91a859a | 383 | // User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'. |
gke | 0:62a1c91a859a | 384 | |
gke | 0:62a1c91a859a | 385 | // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated |
gke | 0:62a1c91a859a | 386 | // orientation. See my report for an overview of the use of quaternions in this application. |
gke | 0:62a1c91a859a | 387 | |
gke | 0:62a1c91a859a | 388 | // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz') |
gke | 0:62a1c91a859a | 389 | // and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer |
gke | 0:62a1c91a859a | 390 | // units are irrelevant as the vector is normalised. |
gke | 0:62a1c91a859a | 391 | |
gke | 0:62a1c91a859a | 392 | #include <math.h> |
gke | 0:62a1c91a859a | 393 | |
gke | 0:62a1c91a859a | 394 | void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az); |
gke | 0:62a1c91a859a | 395 | |
gke | 0:62a1c91a859a | 396 | #define MKp 2.0 // proportional gain governs rate of convergence to accelerometer/magnetometer |
gke | 0:62a1c91a859a | 397 | #define MKi 0.005f // integral gain governs rate of convergence of gyroscope biases |
gke | 0:62a1c91a859a | 398 | |
gke | 0:62a1c91a859a | 399 | real32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; // quaternion elements representing the estimated orientation |
gke | 0:62a1c91a859a | 400 | real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0; // scaled integral error |
gke | 0:62a1c91a859a | 401 | |
gke | 0:62a1c91a859a | 402 | void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) { |
gke | 0:62a1c91a859a | 403 | |
gke | 0:62a1c91a859a | 404 | static real32 rnorm; |
gke | 0:62a1c91a859a | 405 | static real32 vx, vy, vz; |
gke | 0:62a1c91a859a | 406 | static real32 ex, ey, ez; |
gke | 0:62a1c91a859a | 407 | static real32 aMag; |
gke | 0:62a1c91a859a | 408 | |
gke | 0:62a1c91a859a | 409 | //swap z and y? |
gke | 0:62a1c91a859a | 410 | |
gke | 0:62a1c91a859a | 411 | // normalise the measurements |
gke | 0:62a1c91a859a | 412 | aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero Acc values into AHRS is FATAL |
gke | 0:62a1c91a859a | 413 | if ( aMag < 0.9 ) { |
gke | 0:62a1c91a859a | 414 | ax = ay = 0.0; |
gke | 0:62a1c91a859a | 415 | az = 1.0; |
gke | 0:62a1c91a859a | 416 | } else { |
gke | 0:62a1c91a859a | 417 | rnorm = 1.0/aMag; |
gke | 0:62a1c91a859a | 418 | ax *= rnorm; |
gke | 0:62a1c91a859a | 419 | ay *= rnorm; |
gke | 0:62a1c91a859a | 420 | az *= rnorm; |
gke | 0:62a1c91a859a | 421 | } |
gke | 0:62a1c91a859a | 422 | |
gke | 0:62a1c91a859a | 423 | // estimated direction of gravity |
gke | 0:62a1c91a859a | 424 | vx = 2.0*(q1*q3 - q0*q2); |
gke | 0:62a1c91a859a | 425 | vy = 2.0*(q0*q1 + q2*q3); |
gke | 0:62a1c91a859a | 426 | vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3); |
gke | 0:62a1c91a859a | 427 | |
gke | 0:62a1c91a859a | 428 | // error is sum of cross product between reference direction of field and direction measured by sensor |
gke | 0:62a1c91a859a | 429 | ex = (ay*vz - az*vy); |
gke | 0:62a1c91a859a | 430 | ey = (az*vx - ax*vz); |
gke | 0:62a1c91a859a | 431 | ez = (ax*vy - ay*vx); |
gke | 0:62a1c91a859a | 432 | |
gke | 0:62a1c91a859a | 433 | // integral error scaled integral gain |
gke | 0:62a1c91a859a | 434 | exInt += ex*MKi; |
gke | 0:62a1c91a859a | 435 | eyInt += ey*MKi; |
gke | 0:62a1c91a859a | 436 | ezInt += ez*MKi; |
gke | 0:62a1c91a859a | 437 | |
gke | 0:62a1c91a859a | 438 | // adjusted gyroscope measurements |
gke | 0:62a1c91a859a | 439 | gx += MKp*ex + exInt; |
gke | 0:62a1c91a859a | 440 | gy += MKp*ey + eyInt; |
gke | 0:62a1c91a859a | 441 | gz += MKp*ez + ezInt; |
gke | 0:62a1c91a859a | 442 | |
gke | 0:62a1c91a859a | 443 | // integrate quaternion rate and normalise |
gke | 0:62a1c91a859a | 444 | q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT; |
gke | 0:62a1c91a859a | 445 | q1 += (q0*gx + q2*gz - q3*gy)*HalfdT; |
gke | 0:62a1c91a859a | 446 | q2 += (q0*gy - q1*gz + q3*gx)*HalfdT; |
gke | 0:62a1c91a859a | 447 | q3 += (q0*gz + q1*gy - q2*gx)*HalfdT; |
gke | 0:62a1c91a859a | 448 | |
gke | 0:62a1c91a859a | 449 | // normalise quaternion |
gke | 0:62a1c91a859a | 450 | rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3)); |
gke | 0:62a1c91a859a | 451 | q0 *= rnorm; |
gke | 0:62a1c91a859a | 452 | q1 *= rnorm; |
gke | 0:62a1c91a859a | 453 | q2 *= rnorm; |
gke | 0:62a1c91a859a | 454 | q3 *= rnorm; |
gke | 0:62a1c91a859a | 455 | |
gke | 0:62a1c91a859a | 456 | } // IMUupdate |
gke | 0:62a1c91a859a | 457 | |
gke | 0:62a1c91a859a | 458 | //_________________________________________________________________________________ |
gke | 0:62a1c91a859a | 459 | |
gke | 0:62a1c91a859a | 460 | // AHRS.c |
gke | 0:62a1c91a859a | 461 | // S.O.H. Madgwick |
gke | 0:62a1c91a859a | 462 | // 25th August 2010 |
gke | 0:62a1c91a859a | 463 | |
gke | 0:62a1c91a859a | 464 | // Description: |
gke | 0:62a1c91a859a | 465 | |
gke | 0:62a1c91a859a | 466 | // Quaternion implementation of the 'DCM filter' [Mahoney et al]. Incorporates the magnetic distortion |
gke | 0:62a1c91a859a | 467 | // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference |
gke | 0:62a1c91a859a | 468 | // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw |
gke | 0:62a1c91a859a | 469 | // axis only. |
gke | 0:62a1c91a859a | 470 | |
gke | 0:62a1c91a859a | 471 | // User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'. |
gke | 0:62a1c91a859a | 472 | |
gke | 0:62a1c91a859a | 473 | // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated |
gke | 0:62a1c91a859a | 474 | // orientation. See my report for an overview of the use of quaternions in this application. |
gke | 0:62a1c91a859a | 475 | |
gke | 0:62a1c91a859a | 476 | // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'), |
gke | 0:62a1c91a859a | 477 | // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are |
gke | 0:62a1c91a859a | 478 | // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised. |
gke | 0:62a1c91a859a | 479 | |
gke | 0:62a1c91a859a | 480 | void AHRSupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) { |
gke | 0:62a1c91a859a | 481 | |
gke | 0:62a1c91a859a | 482 | static real32 rnorm; |
gke | 0:62a1c91a859a | 483 | static real32 hx, hy, hz, bx2, bz2, mx2, my2, mz2; |
gke | 0:62a1c91a859a | 484 | static real32 vx, vy, vz, wx, wy, wz; |
gke | 0:62a1c91a859a | 485 | static real32 ex, ey, ez; |
gke | 0:62a1c91a859a | 486 | static real32 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; |
gke | 0:62a1c91a859a | 487 | static real32 aMag; |
gke | 0:62a1c91a859a | 488 | |
gke | 0:62a1c91a859a | 489 | // auxiliary variables to reduce number of repeated operations |
gke | 0:62a1c91a859a | 490 | q0q0 = q0*q0; |
gke | 0:62a1c91a859a | 491 | q0q1 = q0*q1; |
gke | 0:62a1c91a859a | 492 | q0q2 = q0*q2; |
gke | 0:62a1c91a859a | 493 | q0q3 = q0*q3; |
gke | 0:62a1c91a859a | 494 | q1q1 = q1*q1; |
gke | 0:62a1c91a859a | 495 | q1q2 = q1*q2; |
gke | 0:62a1c91a859a | 496 | q1q3 = q1*q3; |
gke | 0:62a1c91a859a | 497 | q2q2 = q2*q2; |
gke | 0:62a1c91a859a | 498 | q2q3 = q2*q3; |
gke | 0:62a1c91a859a | 499 | q3q3 = q3*q3; |
gke | 0:62a1c91a859a | 500 | |
gke | 0:62a1c91a859a | 501 | aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero values into AHRS is FATAL |
gke | 0:62a1c91a859a | 502 | if ( aMag < 0.9 ) { |
gke | 0:62a1c91a859a | 503 | ax = ay = 0.0; |
gke | 0:62a1c91a859a | 504 | az = 1.0; |
gke | 0:62a1c91a859a | 505 | } else { |
gke | 0:62a1c91a859a | 506 | // normalise the measurements |
gke | 0:62a1c91a859a | 507 | rnorm = 1.0/aMag; |
gke | 0:62a1c91a859a | 508 | ax *= rnorm; |
gke | 0:62a1c91a859a | 509 | ay *= rnorm; |
gke | 0:62a1c91a859a | 510 | az *= rnorm; |
gke | 0:62a1c91a859a | 511 | } |
gke | 0:62a1c91a859a | 512 | |
gke | 0:62a1c91a859a | 513 | rnorm = 1.0/sqrt(Sqr(mx) + Sqr(my) + Sqr(mz)); |
gke | 0:62a1c91a859a | 514 | mx *= rnorm; |
gke | 0:62a1c91a859a | 515 | my *= rnorm; |
gke | 0:62a1c91a859a | 516 | mz *= rnorm; |
gke | 0:62a1c91a859a | 517 | mx2 = 2.0 * mx; |
gke | 0:62a1c91a859a | 518 | my2 = 2.0 * my; |
gke | 0:62a1c91a859a | 519 | mz2 = 2.0 * mz; |
gke | 0:62a1c91a859a | 520 | |
gke | 0:62a1c91a859a | 521 | // compute reference direction of flux |
gke | 0:62a1c91a859a | 522 | hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2); |
gke | 0:62a1c91a859a | 523 | hy = mx2*(q1q2 + q0q3) + my2*(0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1); |
gke | 0:62a1c91a859a | 524 | hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*(0.5 - q1q1 - q2q2); |
gke | 0:62a1c91a859a | 525 | bx2 = 2.0*sqrt(Sqr(hx) + Sqr(hy)); |
gke | 0:62a1c91a859a | 526 | bz2 = 2.0*hz; |
gke | 0:62a1c91a859a | 527 | |
gke | 0:62a1c91a859a | 528 | // estimated direction of gravity and flux (v and w) |
gke | 0:62a1c91a859a | 529 | vx = 2.0*(q1q3 - q0q2); |
gke | 0:62a1c91a859a | 530 | vy = 2.0*(q0q1 + q2q3); |
gke | 0:62a1c91a859a | 531 | vz = q0q0 - q1q1 - q2q2 + q3q3; |
gke | 0:62a1c91a859a | 532 | |
gke | 0:62a1c91a859a | 533 | wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2); |
gke | 0:62a1c91a859a | 534 | wy = bx2*(q1q2 - q0q3) + bz2*(q0q1 + q2q3); |
gke | 0:62a1c91a859a | 535 | wz = bx2*(q0q2 + q1q3) + bz2*(0.5 - q1q1 - q2q2); |
gke | 0:62a1c91a859a | 536 | |
gke | 0:62a1c91a859a | 537 | // error is sum of cross product between reference direction of fields and direction measured by sensors |
gke | 0:62a1c91a859a | 538 | ex = (ay*vz - az*vy) + (my*wz - mz*wy); |
gke | 0:62a1c91a859a | 539 | ey = (az*vx - ax*vz) + (mz*wx - mx*wz); |
gke | 0:62a1c91a859a | 540 | ez = (ax*vy - ay*vx) + (mx*wy - my*wx); |
gke | 0:62a1c91a859a | 541 | |
gke | 0:62a1c91a859a | 542 | // integral error scaled integral gain |
gke | 0:62a1c91a859a | 543 | exInt += ex*MKi; |
gke | 0:62a1c91a859a | 544 | eyInt += ey*MKi; |
gke | 0:62a1c91a859a | 545 | ezInt += ez*MKi; |
gke | 0:62a1c91a859a | 546 | |
gke | 0:62a1c91a859a | 547 | // adjusted gyroscope measurements |
gke | 0:62a1c91a859a | 548 | gx += MKp*ex + exInt; |
gke | 0:62a1c91a859a | 549 | gy += MKp*ey + eyInt; |
gke | 0:62a1c91a859a | 550 | gz += MKp*ez + ezInt; |
gke | 0:62a1c91a859a | 551 | |
gke | 0:62a1c91a859a | 552 | // integrate quaternion rate and normalise |
gke | 0:62a1c91a859a | 553 | q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT; |
gke | 0:62a1c91a859a | 554 | q1 += (q0*gx + q2*gz - q3*gy)*HalfdT; |
gke | 0:62a1c91a859a | 555 | q2 += (q0*gy - q1*gz + q3*gx)*HalfdT; |
gke | 0:62a1c91a859a | 556 | q3 += (q0*gz + q1*gy - q2*gx)*HalfdT; |
gke | 0:62a1c91a859a | 557 | |
gke | 0:62a1c91a859a | 558 | // normalise quaternion |
gke | 0:62a1c91a859a | 559 | rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3)); |
gke | 0:62a1c91a859a | 560 | q0 *= rnorm; |
gke | 0:62a1c91a859a | 561 | q1 *= rnorm; |
gke | 0:62a1c91a859a | 562 | q2 *= rnorm; |
gke | 0:62a1c91a859a | 563 | q3 *= rnorm; |
gke | 0:62a1c91a859a | 564 | |
gke | 0:62a1c91a859a | 565 | } // AHRSupdate |
gke | 0:62a1c91a859a | 566 | |
gke | 0:62a1c91a859a | 567 | void EulerAngles(void) { |
gke | 0:62a1c91a859a | 568 | |
gke | 0:62a1c91a859a | 569 | static uint8 g; |
gke | 0:62a1c91a859a | 570 | |
gke | 0:62a1c91a859a | 571 | Angle[Roll] = atan2(2.0*q2*q3 - 2.0*q0*q1 , 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0); |
gke | 0:62a1c91a859a | 572 | Angle[Pitch] = asin(2.0*q1*q2 - 2.0*q0*q2); |
gke | 0:62a1c91a859a | 573 | Angle[Yaw] = -atan2(2.0*q1*q2 - 2.0*q0*q3 , 2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0); |
gke | 0:62a1c91a859a | 574 | |
gke | 0:62a1c91a859a | 575 | for ( g = 0; g < (uint8)3; g++ ) { |
gke | 0:62a1c91a859a | 576 | #ifdef USE_GYRO_RATE |
gke | 0:62a1c91a859a | 577 | Rate[g] = Gyro[g]; |
gke | 0:62a1c91a859a | 578 | #else |
gke | 0:62a1c91a859a | 579 | Rate[g] = ( Angle[g] - Anglep[g] ) * dTR; |
gke | 0:62a1c91a859a | 580 | Anglep[g] = Angle[g]; |
gke | 0:62a1c91a859a | 581 | #endif // USE_GYRO_RATE |
gke | 0:62a1c91a859a | 582 | } |
gke | 0:62a1c91a859a | 583 | |
gke | 0:62a1c91a859a | 584 | } // EulerAngles |
gke | 0:62a1c91a859a | 585 | |
gke | 0:62a1c91a859a | 586 | /* |
gke | 0:62a1c91a859a | 587 | heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2) |
gke | 0:62a1c91a859a | 588 | attitude = asin(2*qx*qy + 2*qz*qw) |
gke | 0:62a1c91a859a | 589 | bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2) |
gke | 0:62a1c91a859a | 590 | |
gke | 0:62a1c91a859a | 591 | except when qx*qy + qz*qw = 0.5 (north pole) |
gke | 0:62a1c91a859a | 592 | which gives: |
gke | 0:62a1c91a859a | 593 | heading = 2 * atan2(x,w) |
gke | 0:62a1c91a859a | 594 | bank = 0 |
gke | 0:62a1c91a859a | 595 | and when qx*qy + qz*qw = -0.5 (south pole) |
gke | 0:62a1c91a859a | 596 | which gives: |
gke | 0:62a1c91a859a | 597 | heading = -2 * atan2(x,w) |
gke | 0:62a1c91a859a | 598 | bank = 0 |
gke | 0:62a1c91a859a | 599 | */ |
gke | 0:62a1c91a859a | 600 | |
gke | 0:62a1c91a859a | 601 | |
gke | 0:62a1c91a859a | 602 |