Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

attitude.c@1:1e3318a30ddd, 2011-02-25 (annotated)

Committer:
gke
Date:
Fri Feb 25 01:35:24 2011 +0000
Revision:
1:1e3318a30ddd
Parent:
0:62a1c91a859a
Child:
2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.

Who changed what in which revision?

UserRevisionLine numberNew 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 1:1e3318a30ddd 203 TxString("Heading: \t");
gke 1:1e3318a30ddd 204 TxVal32(Make2Pi(Heading) * 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