attitude.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 // Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW.
00024 // CAUTION: Because of the coordinate frame the LR Acc sense must be negated for roll compensation.
00025 
00026 void AdaptiveYawLPFreq(void);
00027 void DoLegacyYawComp(uint8);
00028 void NormaliseAccelerations(void);
00029 void AttitudeTest(void);
00030 void InitAttitude(void);
00031 
00032 real32 AccMagnitude;
00033 real32 EstAngle[3][MaxAttitudeScheme];
00034 real32 EstRate[3][MaxAttitudeScheme];
00035 real32 Correction[2];
00036 real32 YawFilterLPFreq;
00037 real32 dT, dTOn2, dTR, dTmS;
00038 uint32 uSp;
00039 
00040 uint8 AttitudeMethod = Wolferl; //Integrator, Wolferl MadgwickIMU PremerlaniDCM MadgwickAHRS, MultiWii;
00041 
00042 void AdaptiveYawLPFreq(void) { // Filter LP freq is decreased with reduced yaw stick deflection
00043 
00044     YawFilterLPFreq = ( MAX_YAW_FREQ*abs(DesiredYaw) / RC_NEUTRAL );
00045     YawFilterLPFreq = Limit(YawFilterLPFreq, 0.5, MAX_YAW_FREQ);
00046 
00047 } // AdaptiveYawFilterA
00048 
00049 real32 HE;
00050     
00051 void DoLegacyYawComp(uint8 S) {
00052 
00053 #define COMPASS_MIDDLE          10                 // yaw stick neutral dead zone
00054 #define DRIFT_COMP_YAW_RATE     QUARTERPI          // Radians/Sec
00055 
00056     static int16 Temp;
00057 
00058     // Yaw Angle here is meant to be interpreted as the Heading Error
00059 
00060     Rate[Yaw] = Gyro[Yaw];
00061 
00062     Temp = DesiredYaw - Trim[Yaw];
00063     if ( F.CompassValid )  // CW+
00064         if ( abs(Temp) > COMPASS_MIDDLE ) {
00065             DesiredHeading = Heading; // acquire new heading
00066             Angle[Yaw] = 0.0;
00067         } else {
00068             HE = MinimumTurn(DesiredHeading - Heading);
00069             HE = Limit1(HE, SIXTHPI); // 30 deg limit
00070             HE = HE*K[CompassKp];
00071             Rate[Yaw] = -Limit1(HE, DRIFT_COMP_YAW_RATE);
00072         }
00073     else {
00074         DesiredHeading = Heading;
00075         Angle[Yaw] = 0.0;
00076     }
00077 
00078     Angle[Yaw] += Rate[Yaw]*dT;
00079  //   Angle[Yaw] = Limit1(Angle[Yaw], K[YawIntLimit]);
00080 
00081 } // DoLegacyYawComp
00082 
00083 void NormaliseAccelerations(void) {
00084 
00085     const real32 MIN_ACC_MAGNITUDE = 0.7; // below this the accelerometers are deemed unreliable - falling?
00086 
00087     static real32 ReNorm;
00088 
00089     AccMagnitude = sqrt(Sqr(Acc[BF]) + Sqr(Acc[LR]) + Sqr(Acc[UD]));
00090     F.AccMagnitudeOK = AccMagnitude > MIN_ACC_MAGNITUDE;
00091     if ( F.AccMagnitudeOK ) {
00092         ReNorm = 1.0 / AccMagnitude;
00093         Acc[BF] *= ReNorm;
00094         Acc[LR] *= ReNorm;
00095         Acc[UD] *= ReNorm;
00096     } else {
00097         Acc[LR] = Acc[BF]  = 0.0;
00098         Acc[UD] = 1.0;
00099     }
00100 } // NormaliseAccelerations
00101 
00102 void GetAttitude(void) {
00103 
00104     static uint32 Now;
00105     static uint8 i;
00106 
00107     if ( GyroType == IRSensors )
00108         GetIRAttitude();
00109     else {
00110         GetGyroRates();
00111         GetAccelerations();
00112     }
00113 
00114     Now = uSClock();
00115     dT = ( Now - uSp)*0.000001;
00116     dTOn2 = 0.5 * dT;
00117     dTR = 1.0 / dT;
00118     uSp = Now;
00119 
00120     GetHeading(); // only updated every 50mS but read continuously anyway
00121 
00122     if ( GyroType == IRSensors ) {
00123 
00124         for ( i = 0; i < (uint8)2; i++ ) {
00125             Rate[i] = ( Angle[i] - Anglep[i] )*dT;
00126             Anglep[i] = Angle[i];
00127         }
00128 
00129         Rate[Yaw] = 0.0; // need Yaw gyro!
00130 
00131     } else {
00132         DebugPin = true;
00133 
00134         NormaliseAccelerations(); // rudimentary check for free fall etc
00135 
00136         // Wolferl
00137         DoWolferl();
00138 
00139 #ifdef INC_ALL_SCHEMES
00140 
00141         // Complementary
00142         DoCF();
00143 
00144         // Kalman
00145         DoKalman();
00146 
00147         //Premerlani DCM
00148         DoDCM();
00149 
00150         // MultiWii
00151         DoMultiWii();
00152 
00153         // Madgwick IMU
00154         // DoMadgwickIMU(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]);
00155 
00156         //#define INC_IMU2
00157 
00158 #ifdef INC_IMU2
00159         DoMadgwickIMU2(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]);    
00160 #else
00161         Madgwick IMU April 30, 2010 Paper Version
00162 #endif
00163         // Madgwick AHRS BROKEN
00164          DoMadgwickAHRS(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD], Mag[BF].V, Mag[LR].V, -Mag[UD].V);
00165 
00166         // Integrator - REFERENCE/FALLBACK
00167         DoIntegrator();
00168 
00169 #endif // INC_ALL_SCHEMES
00170 
00171         Angle[Roll] = EstAngle[Roll][AttitudeMethod];
00172         Angle[Pitch] = EstAngle[Pitch][AttitudeMethod];
00173 
00174         DebugPin = false;
00175     }
00176 
00177     F.NearLevel = Max(fabs(Angle[Roll]), fabs(Angle[Pitch])) < NAV_RTH_LOCKOUT;
00178 
00179 } // GetAttitude
00180 
00181 //____________________________________________________________________________________________
00182 
00183 // Integrator
00184 
00185 void DoIntegrator(void) {
00186 
00187     static uint8 g;
00188 
00189     for ( g = 0; g < (uint8)3; g++ ) {
00190         EstRate[g][Integrator] = Gyro[g];
00191         EstAngle[g][Integrator] +=  EstRate[g][Integrator]*dT;
00192         //  EstAngle[g][Integrator] = DecayX(EstAngle[g][Integrator], 0.0001*dT);
00193     }
00194 
00195 } // DoIntegrator
00196 
00197 //____________________________________________________________________________________________
00198 
00199 // Original simple accelerometer compensation of gyros developed for UAVP by Wolfgang Mahringer
00200 // and adapted for UAVXArm
00201 
00202 void DoWolferl(void) { // NO YAW ESTIMATE
00203 
00204     const real32 WKp = 0.13; // 0.1
00205 
00206     static real32 Grav[2], Dyn[2];
00207     static real32 CompStep;
00208 
00209     Rate[Roll] = Gyro[Roll];
00210     Rate[Pitch] = Gyro[Pitch];
00211 
00212     if ( F.AccMagnitudeOK ) {
00213 
00214         CompStep = WKp*dT;
00215 
00216         // Roll
00217 
00218         Grav[LR] = -sin(EstAngle[Roll][Wolferl]);   // original used approximation for small angles
00219         Dyn[LR] = 0.0; //Rate[Roll];                // lateral acceleration due to rate - do later:).
00220 
00221         Correction[LR] = -Acc[LR] + Grav[LR] + Dyn[LR]; // Acc is reversed
00222         Correction[LR] = Limit1(Correction[LR], CompStep);
00223 
00224         EstAngle[Roll][Wolferl] += Rate[Roll]*dT;
00225         EstAngle[Roll][Wolferl] += Correction[LR];
00226 
00227         // Pitch
00228 
00229         Grav[BF] = -sin(EstAngle[Pitch][Wolferl]);
00230         Dyn[BF] = 0.0; // Rate[Pitch];
00231 
00232         Correction[BF] = Acc[BF] + Grav[BF] + Dyn[BF];
00233         Correction[BF] = Limit1(Correction[BF], CompStep);
00234 
00235         EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT;
00236         EstAngle[Pitch][Wolferl] += Correction[BF];
00237         
00238     } else {
00239     
00240         EstAngle[Roll][Wolferl] += Rate[Roll]*dT;
00241         EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT;
00242         
00243     }
00244 
00245 } // DoWolferl
00246 
00247 //_________________________________________________________________________________
00248 
00249 // Complementary Filter originally authored by RoyLB
00250 // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
00251 
00252 const real32 TauCF = 1.1;
00253 
00254 real32 AngleCF[2] = {0,0};
00255 real32 F0[2] = {0,0};
00256 real32 F1[2] = {0,0};
00257 real32 F2[2] = {0,0};
00258 
00259 real32 CF(uint8 a, real32 NewAngle, real32 NewRate) {
00260 
00261     if ( F.AccMagnitudeOK ) {
00262         F0[a] = (NewAngle - AngleCF[a])*Sqr(TauCF);
00263         F2[a] += F0[a]*dT;
00264         F1[a] = F2[a] + (NewAngle - AngleCF[a])*2.0*TauCF + NewRate;
00265         AngleCF[a] = (F1[a]*dT) + AngleCF[a];
00266     } else
00267         AngleCF[a] += NewRate*dT;
00268 
00269     return ( AngleCF[a] ); // This is actually the current angle, but is stored for the next iteration
00270 } // CF
00271 
00272 void DoCF(void) { // NO YAW ANGLE ESTIMATE
00273 
00274     EstAngle[Roll][Complementary] = CF(Roll, asin(-Acc[LR]), Gyro[Roll]);
00275     EstAngle[Pitch][Complementary] = CF(Pitch, asin(-Acc[BF]), Gyro[Pitch]); // zzz minus???
00276     EstRate[Roll][Complementary] = Gyro[Roll];
00277     EstRate[Pitch][Complementary] = Gyro[Pitch];
00278 
00279 } // DoCF
00280 
00281 //____________________________________________________________________________________________
00282 
00283 // The DCM formulation used here is due to W. Premerlani and P. Bizard in a paper entitled:
00284 // Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original
00285 // work by R. Mahony et al. - Thanks Rob!
00286 
00287 // SEEMS TO BE A FAIRLY LARGE PHASE DELAY OF 2 SAMPLE INTERVALS
00288 
00289 void DCMNormalise(void);
00290 void DCMDriftCorrection(void);
00291 void DCMMotionCompensation(void);
00292 void DCMUpdate(void);
00293 void DCMEulerAngles(void);
00294 
00295 real32   RollPitchError[3] = {0,0,0};
00296 real32   OmegaV[3] = {0,0,0}; // corrected gyro data
00297 real32   OmegaP[3] = {0,0,0}; // proportional correction
00298 real32   OmegaI[3] = {0,0,0}; // integral correction
00299 real32   Omega[3] = {0,0,0};
00300 real32   DCM[3][3] = {{1,0,0 },{0,1,0} ,{0,0,1}};
00301 real32   U[3][3] = {{0,1,2},{ 3,4,5} ,{6,7,8}};
00302 real32   TempM[3][3] = {{0,0,0},{0,0,0},{ 0,0,0}};
00303 real32   AccV[3];
00304 
00305 void DCMNormalise(void) {
00306 
00307     static real32 Error = 0;
00308     static real32 Renorm = 0.0;
00309     static boolean Problem;
00310     static uint8 r;
00311 
00312     Error = -VDot(&DCM[0][0], &DCM[1][0])*0.5;        //eq.19
00313 
00314     VScale(&TempM[0][0], &DCM[1][0], Error);            //eq.19
00315     VScale(&TempM[1][0], &DCM[0][0], Error);            //eq.19
00316 
00317     VAdd(&TempM[0][0], &TempM[0][0], &DCM[0][0]);       //eq.19
00318     VAdd(&TempM[1][0], &TempM[1][0], &DCM[1][0]);       //eq.19
00319 
00320     VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]);    // c= a*b eq.20
00321 
00322     Problem = false;
00323     for ( r = 0; r < (uint8)3; r++ ) {
00324         Renorm = VDot(&TempM[r][0], &TempM[r][0]);
00325         if ( (Renorm <  1.5625) && (Renorm > 0.64) )
00326             Renorm = 0.5*(3.0 - Renorm);               //eq.21
00327         else
00328             if ( (Renorm < 100.0) && (Renorm > 0.01) )
00329                 Renorm = 1.0 / sqrt( Renorm );
00330             else
00331                 Problem = true;
00332 
00333         VScale(&DCM[r][0], &TempM[r][0], Renorm);
00334     }
00335 
00336     if ( Problem ) { // Divergent - force to initial conditions and hope!
00337         DCM[0][0] = 1.0;
00338         DCM[0][1] = 0.0;
00339         DCM[0][2] = 0.0;
00340         DCM[1][0] = 0.0;
00341         DCM[1][1] = 1.0;
00342         DCM[1][2] = 0.0;
00343         DCM[2][0] = 0.0;
00344         DCM[2][1] = 0.0;
00345         DCM[2][2] = 1.0;
00346     }
00347 
00348 } // DCMNormalise
00349 
00350 void DCMMotionCompensation(void) {
00351     // compensation for rate of change of velocity LR/BF needs GPS velocity but
00352     // updates probably too slow?
00353     AccV[LR ] += 0.0;
00354     AccV[BF] += 0.0;
00355 } // DCMMotionCompensation
00356 
00357 void DCMDriftCorrection(void) {
00358 
00359     static real32 ScaledOmegaI[3];
00360 
00361     //DON'T USE  #define USE_DCM_YAW_COMP
00362 #ifdef USE_DCM_YAW_COMP
00363     static real32 ScaledOmegaP[3];
00364     static real32 YawError[3];
00365     static real32 ErrorCourse;
00366 #endif // USE_DCM_YAW_COMP
00367 
00368     VCross(&RollPitchError[0], &AccV[0], &DCM[2][0]); //adjust the reference ground
00369     VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch);
00370 
00371     VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch);
00372     VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
00373 
00374 #ifdef USE_DCM_YAW_COMP
00375     // Yaw - drift correction based on compass/magnetometer heading
00376     HeadingCos = cos(Heading);
00377     HeadingSin = sin(Heading);
00378     ErrorCourse = ( U[0][0]*HeadingSin ) - ( U[1][0]*HeadingCos );
00379     VScale(YawError, &U[2][0], ErrorCourse );
00380 
00381     VScale(&ScaledOmegaP[0], &YawError[0], Kp_Yaw );
00382     VAdd(&OmegaP[0], &OmegaP[0], &ScaledOmegaP[0]);
00383 
00384     VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw );
00385     VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
00386 #endif // USE_DCM_YAW_COMP
00387 
00388 } // DCMDriftCorrection
00389 
00390 void DCMUpdate(void) {
00391 
00392     static uint8 i, j, k;
00393     static real32 op[3];
00394 
00395     AccV[BF] = Acc[BF];
00396     AccV[LR] = -Acc[LR];
00397     AccV[UD] = -Acc[UD];
00398 
00399     VAdd(&Omega[0], &Gyro[0], &OmegaI[0]);
00400     VAdd(&OmegaV[0], &Omega[0], &OmegaP[0]);
00401 
00402 //   MotionCompensation();
00403 
00404     U[0][0] =  0.0;
00405     U[0][1] = -dT*OmegaV[2];   //-z
00406     U[0][2] =  dT*OmegaV[1];   // y
00407     U[1][0] =  dT*OmegaV[2];   // z
00408     U[1][1] =  0.0;
00409     U[1][2] = -dT*OmegaV[0];   //-x
00410     U[2][0] = -dT*OmegaV[1];   //-y
00411     U[2][1] =  dT*OmegaV[0];   // x
00412     U[2][2] =  0.0;
00413 
00414     for ( i = 0; i < (uint8)3; i++ )
00415         for ( j = 0; j < (uint8)3; j++ ) {
00416             for ( k = 0; k < (uint8)3; k++ )
00417                 op[k] = DCM[i][k]*U[k][j];
00418 
00419             TempM[i][j] = op[0] + op[1] + op[2];
00420         }
00421 
00422     for ( i = 0; i < (uint8)3; i++ )
00423         for (j = 0; j < (uint8)3; j++ )
00424             DCM[i][j] += TempM[i][j];
00425 
00426 } // DCMUpdate
00427 
00428 void DCMEulerAngles(void) {
00429 
00430     static uint8 g;
00431 
00432     for ( g = 0; g < (uint8)3; g++ )
00433         Rate[g] = Gyro[g];
00434 
00435     EstAngle[Roll][PremerlaniDCM]= atan2(DCM[2][1], DCM[2][2]);
00436     EstAngle[Pitch][PremerlaniDCM] = -asin(DCM[2][0]);
00437     EstAngle[Yaw][PremerlaniDCM] = atan2(DCM[1][0], DCM[0][0]);
00438 
00439     // Est. Rates ???
00440 
00441 } // DCMEulerAngles
00442 
00443 void DoDCM(void) {
00444     DCMUpdate();
00445     DCMNormalise();
00446     DCMDriftCorrection();
00447     DCMEulerAngles();
00448 } // DoDCM
00449 
00450 //___________________________________________________________________________________
00451 
00452 // IMU.c
00453 // S.O.H. Madgwick
00454 // 25th September 2010
00455 
00456 // Description:
00457 
00458 // Quaternion implementation of the 'DCM filter' [Mahony et al.].
00459 
00460 // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
00461 // orientation.  See my report for an overview of the use of quaternions in this application.
00462 
00463 // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
00464 // and accelerometer ('ax', 'ay', 'az') data.  Gyroscope units are radians/second, accelerometer
00465 // units are irrelevant as the vector is normalised.
00466 
00467 const real32 MKp = 2.0;           // proportional gain governs rate of convergence to accelerometer/magnetometer
00468 const real32 MKi = 1.0;          // integral gain governs rate of convergence of gyroscope biases // 0.005
00469 
00470 real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0;   // scaled integral error
00471 real32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0;  // quaternion elements representing the estimated orientation
00472 
00473 void DoMadgwickIMU(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) {
00474 
00475     static uint8 g;
00476     static real32 ReNorm;
00477     static real32 vx, vy, vz;
00478     static real32 ex, ey, ez;
00479 
00480     if ( F.AccMagnitudeOK ) {
00481 
00482         // estimated direction of gravity
00483         vx = 2.0*(q1*q3 - q0*q2);
00484         vy = 2.0*(q0*q1 + q2*q3);
00485         vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3);
00486 
00487         // error is sum of cross product between reference direction of field and direction measured by sensor
00488         ex = (ay*vz - az*vy);
00489         ey = (az*vx - ax*vz);
00490         ez = (ax*vy - ay*vx);
00491 
00492         // integral error scaled integral gain
00493         exInt += ex*MKi*dT;
00494         eyInt += ey*MKi*dT;
00495         ezInt += ez*MKi*dT;
00496 
00497         // adjusted gyroscope measurements
00498         gx += MKp*ex + exInt;
00499         gy += MKp*ey + eyInt;
00500         gz += MKp*ez + ezInt;
00501 
00502         // integrate quaternion rate and normalise
00503         q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2;
00504         q1 += (q0*gx + q2*gz - q3*gy)*dTOn2;
00505         q2 += (q0*gy - q1*gz + q3*gx)*dTOn2;
00506         q3 += (q0*gz + q1*gy - q2*gx)*dTOn2;
00507 
00508         // normalise quaternion
00509         ReNorm = 1.0  /sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
00510         q0 *= ReNorm;
00511         q1 *= ReNorm;
00512         q2 *= ReNorm;
00513         q3 *= ReNorm;
00514 
00515         MadgwickEulerAngles(MadgwickIMU);
00516 
00517     } else
00518         for ( g = 0; g <(uint8)3; g++) {
00519             EstRate[g][MadgwickIMU] = Gyro[g];
00520             EstAngle[g][MadgwickIMU] += EstRate[g][MadgwickIMU]*dT;
00521         }
00522 
00523 }  // DoMadgwickIMU
00524 
00525 //_________________________________________________________________________________
00526 
00527 // IMU.c
00528 // S.O.H. Madgwick, 
00529 // 'An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays',
00530 // April 30, 2010
00531 
00532 #ifdef INC_IMU2
00533 
00534 boolean FirstIMU2 = true;
00535 real32 BetaIMU2 = 0.033;
00536 // const real32 BetaAHRS = 0.041;
00537 
00538 //Quaternion orientation of earth frame relative to auxiliary frame.
00539 real32 AEq_1;
00540 real32 AEq_2;
00541 real32 AEq_3;
00542 real32 AEq_4;
00543 
00544 //Estimated orientation quaternion elements with initial conditions.
00545 real32 SEq_1;
00546 real32 SEq_2;
00547 real32 SEq_3;
00548 real32 SEq_4;
00549 
00550 void DoMadgwickIMU2(real32 w_x, real32 w_y, real32 w_z, real32 a_x, real32 a_y, real32 a_z) {
00551 
00552     static uint8 g;
00553 
00554     //Vector norm.
00555     static real32 Renorm;
00556     //Quaternion rate from gyroscope elements.
00557     static real32 SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4;
00558     //Objective function elements.
00559     static real32 f_1, f_2, f_3;
00560     //Objective function Jacobian elements.
00561     static real32 J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33;
00562     //Objective function gradient elements.
00563     static real32 SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4;
00564 
00565     //Auxiliary variables to avoid reapeated calcualtions.
00566     static real32 halfSEq_1, halfSEq_2, halfSEq_3, halfSEq_4;
00567     static real32 twoSEq_1, twoSEq_2, twoSEq_3;
00568 
00569     if ( F.AccMagnitudeOK ) {
00570 
00571         halfSEq_1 = 0.5*SEq_1;
00572         halfSEq_2 = 0.5*SEq_2;
00573         halfSEq_3 = 0.5*SEq_3;
00574         halfSEq_4 = 0.5*SEq_4;
00575         twoSEq_1 = 2.0*SEq_1;
00576         twoSEq_2 = 2.0*SEq_2;
00577         twoSEq_3 = 2.0*SEq_3;
00578 
00579         //Compute the quaternion rate measured by gyroscopes.
00580         SEqDot_omega_1 = -halfSEq_2*w_x - halfSEq_3*w_y - halfSEq_4*w_z;
00581         SEqDot_omega_2 = halfSEq_1*w_x + halfSEq_3*w_z - halfSEq_4*w_y;
00582         SEqDot_omega_3 = halfSEq_1*w_y - halfSEq_2*w_z + halfSEq_4*w_x;
00583         SEqDot_omega_4 = halfSEq_1*w_z + halfSEq_2*w_y - halfSEq_3*w_x;
00584 
00585         /*
00586         //Normalise the accelerometer measurement.
00587         Renorm = 1.0 / sqrt(Sqr(a_x) + Sqr(a_y) + Sqr(a_z));
00588         a_x *= Renorm;
00589         a_y *= Renorm;
00590         a_z *= Renorm;
00591         */
00592 
00593         //Compute the objective function and Jacobian.
00594         f_1 = twoSEq_2*SEq_4 - twoSEq_1*SEq_3 - a_x;
00595         f_2 = twoSEq_1*SEq_2 + twoSEq_3*SEq_4 - a_y;
00596         f_3 = 1.0 - twoSEq_2*SEq_2 - twoSEq_3*SEq_3 - a_z;
00597         //J_11 negated in matrix multiplication.
00598         J_11or24 = twoSEq_3;
00599         J_12or23 = 2.0*SEq_4;
00600         //J_12 negated in matrix multiplication
00601         J_13or22 = twoSEq_1;
00602         J_14or21 = twoSEq_2;
00603         //Negated in matrix multiplication.
00604         J_32 = 2.0*J_14or21;
00605         //Negated in matrix multiplication.
00606         J_33 = 2.0*J_11or24;
00607 
00608         //Compute the gradient (matrix multiplication).
00609         SEqHatDot_1 = J_14or21*f_2 - J_11or24*f_1;
00610         SEqHatDot_2 = J_12or23*f_1 + J_13or22*f_2 - J_32*f_3;
00611         SEqHatDot_3 = J_12or23*f_2 - J_33*f_3 - J_13or22*f_1;
00612         SEqHatDot_4 = J_14or21*f_1 + J_11or24*f_2;
00613 
00614         //Normalise the gradient.
00615         Renorm = 1.0 / sqrt(Sqr(SEqHatDot_1) + Sqr(SEqHatDot_2) + Sqr(SEqHatDot_3) + Sqr(SEqHatDot_4));
00616         SEqHatDot_1 *= Renorm;
00617         SEqHatDot_2 *= Renorm;
00618         SEqHatDot_3 *= Renorm;
00619         SEqHatDot_4 *= Renorm;
00620 
00621         //Compute then integrate the estimated quaternion rate.
00622         SEq_1 += (SEqDot_omega_1 - (BetaIMU2*SEqHatDot_1))*dT;
00623         SEq_2 += (SEqDot_omega_2 - (BetaIMU2*SEqHatDot_2))*dT;
00624         SEq_3 += (SEqDot_omega_3 - (BetaIMU2*SEqHatDot_3))*dT;
00625         SEq_4 += (SEqDot_omega_4 - (BetaIMU2*SEqHatDot_4))*dT;
00626 
00627         //Normalise quaternion
00628         Renorm = 1.0 / sqrt(Sqr(SEq_1) + Sqr(SEq_2) + Sqr(SEq_3) + Sqr(SEq_4));
00629         SEq_1 *= Renorm;
00630         SEq_2 *= Renorm;
00631         SEq_3 *= Renorm;
00632         SEq_4 *= Renorm;
00633 
00634         if ( FirstIMU2 ) {
00635             //Store orientation of auxiliary frame.
00636             AEq_1 = SEq_1;
00637             AEq_2 = SEq_2;
00638             AEq_3 = SEq_3;
00639             AEq_4 = SEq_4;
00640             FirstIMU2 = false;
00641         }
00642 
00643         MadgwickEulerAngles(MadgwickIMU2);
00644 
00645     } else
00646         for ( g = 0; g <(uint8)3; g++) {
00647             EstRate[g][MadgwickIMU2] = Gyro[g];
00648             EstAngle[g][MadgwickIMU2] += EstRate[g][MadgwickIMU2]*dT;
00649         }
00650 
00651 } // DoMadgwickIMU2
00652 
00653 #endif // INC_IMU2
00654 
00655 //_________________________________________________________________________________
00656 
00657 // AHRS.c
00658 // S.O.H. Madgwick
00659 // 25th August 2010
00660 
00661 // Description:
00662 
00663 // Quaternion implementation of the 'DCM filter' [Mahoney et al].  Incorporates the magnetic distortion
00664 // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
00665 // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
00666 // a only.
00667 
00668 // User must define 'dTOn2' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
00669 
00670 // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
00671 // orientation.  See my report for an overview of the use of quaternions in this application.
00672 
00673 // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
00674 // accelerometer ('ax', 'ay', 'az') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
00675 // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
00676 
00677 void DoMadgwickAHRS(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) {
00678 
00679     static uint8 g;
00680     static real32 ReNorm;
00681     static real32 hx, hy, hz, bx2, bz2, mx2, my2, mz2;
00682     static real32 vx, vy, vz, wx, wy, wz;
00683     static real32 ex, ey, ez;
00684     static real32 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00685 
00686     if ( F.AccMagnitudeOK ) {
00687 
00688         // auxiliary variables to reduce number of repeated operations
00689         q0q0 = q0*q0;
00690         q0q1 = q0*q1;
00691         q0q2 = q0*q2;
00692         q0q3 = q0*q3;
00693         q1q1 = q1*q1;
00694         q1q2 = q1*q2;
00695         q1q3 = q1*q3;
00696         q2q2 = q2*q2;
00697         q2q3 = q2*q3;
00698         q3q3 = q3*q3;
00699 
00700         ReNorm = 1.0 / sqrt( Sqr( mx ) + Sqr( my ) + Sqr( mz ) );
00701         mx *= ReNorm;
00702         my *= ReNorm;
00703         mz *= ReNorm;
00704         mx2 = 2.0*mx;
00705         my2 = 2.0*my;
00706         mz2 = 2.0*mz;
00707 
00708         // compute reference direction of flux
00709         hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2);
00710         hy = mx2*(q1q2 + q0q3) + my2*( 0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1);
00711         hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*( 0.5 - q1q1 - q2q2 );
00712         bx2 = 2.0*sqrt( Sqr( hx ) + Sqr( hy ) );
00713         bz2 = 2.0*hz;
00714 
00715         // estimated direction of gravity and flux (v and w)
00716         vx = 2.0*(q1q3 - q0q2);
00717         vy = 2.0*(q0q1 + q2q3);
00718         vz = q0q0 - q1q1 - q2q2 + q3q3;
00719 
00720         wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2);
00721         wy = bx2*(q1q2 - q0q3) + bz2*( q0q1 + q2q3 );
00722         wz = bx2*(q0q2 + q1q3) + bz2*( 0.5 - q1q1 - q2q2 );
00723 
00724         // error is sum of cross product between reference direction of fields and direction measured by sensors
00725         ex = (ay*vz - az*vy) + (my*wz - mz*wy);
00726         ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
00727         ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
00728 
00729         // integral error scaled integral gain
00730         exInt += ex*MKi*dT;
00731         eyInt += ey*MKi*dT;
00732         ezInt += ez*MKi*dT;
00733 
00734         // adjusted gyroscope measurements
00735         gx += MKp*ex + exInt;
00736         gy += MKp*ey + eyInt;
00737         gz += MKp*ez + ezInt;
00738 
00739         // integrate quaternion rate and normalise
00740         q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2;
00741         q1 += (q0*gx + q2*gz - q3*gy)*dTOn2;
00742         q2 += (q0*gy - q1*gz + q3*gx)*dTOn2;
00743         q3 += (q0*gz + q1*gy - q2*gx)*dTOn2;
00744 
00745         // normalise quaternion
00746         ReNorm = 1.0 / sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
00747         q0 *= ReNorm;
00748         q1 *= ReNorm;
00749         q2 *= ReNorm;
00750         q3 *= ReNorm;
00751 
00752         MadgwickEulerAngles(MadgwickAHRS);
00753 
00754     } else
00755         for ( g = 0; g <(uint8)3; g++) {
00756             EstRate[g][MadgwickAHRS] = Gyro[g];
00757             EstAngle[g][MadgwickAHRS] += EstRate[g][MadgwickAHRS]*dT;
00758         }
00759 
00760 } // DoMadgwickAHRS
00761 
00762 void  MadgwickEulerAngles(uint8 S) {
00763 
00764     EstAngle[Roll][S] = atan2(2.0*q2*q3 - 2.0*q0*q1, 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0);
00765     EstAngle[Pitch][S] = asin(2.0*q1*q2 - 2.0*q0*q2);
00766     EstAngle[Yaw][S] = atan2(2.0*q1*q2 - 2.0*q0*q3,  2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);
00767 
00768 } // MadgwickEulerAngles
00769 
00770 //_________________________________________________________________________________
00771 
00772 // Kalman Filter originally authored by Tom Pycke
00773 // http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
00774 
00775 real32 AngleKF[2] = {0,0};
00776 real32 BiasKF[2] = {0,0};
00777 real32 P00[2] = {0,0};
00778 real32 P01[2] = {0,0};
00779 real32 P10[2] = {0,0};
00780 real32 P11[2] = {0,0};
00781 
00782 real32 KalmanFilter(uint8 a, real32 NewAngle, real32 NewRate) {
00783 
00784     // Q is a 2x2 matrix that represents the process covariance noise.
00785     // In this case, it indicates how much we trust the accelerometer
00786     // relative to the gyros.
00787     const real32 AngleQ = 0.003;
00788     const real32 GyroQ = 0.009;
00789 
00790     // R represents the measurement covariance noise. In this case,
00791     // it is a 1x1 matrix that says that we expect AngleR rad jitter
00792     // from the accelerometer.
00793     const real32 AngleR = GYRO_PROP_NOISE;
00794 
00795     static real32 y, S;
00796     static real32 K0, K1;
00797 
00798     AngleKF[a] += (NewRate - BiasKF[a])*dT;
00799     P00[a] -=  (( P10[a] + P01[a] ) + AngleQ )*dT;
00800     P01[a] -=  P11[a]*dT;
00801     P10[a] -=  P11[a]*dT;
00802     P11[a] +=  GyroQ*dT;
00803 
00804     y = NewAngle - AngleKF[a];
00805     S = 1.0 / ( P00[a] + AngleR );
00806     K0 = P00[a]*S;
00807     K1 = P10[a]*S;
00808 
00809     AngleKF[a] +=  K0*y;
00810     BiasKF[a]  +=  K1*y;
00811     P00[a] -= K0*P00[a];
00812     P01[a] -= K0*P01[a];
00813     P10[a] -= K1*P00[a];
00814     P11[a] -= K1*P01[a];
00815 
00816     return ( AngleKF[a] );
00817 
00818 }  // KalmanFilter
00819 
00820 void DoKalman(void) { // NO YAW ANGLE ESTIMATE
00821     EstAngle[Roll][Kalman] = KalmanFilter(Roll, asin(-Acc[LR]), Gyro[Roll]);
00822     EstAngle[Pitch][Kalman] = KalmanFilter(Pitch, asin(Acc[BF]), Gyro[Pitch]);
00823     EstRate[Roll][Kalman] = Gyro[Roll];
00824     EstRate[Pitch][Kalman] = Gyro[Pitch];
00825 } // DoKalman
00826 
00827 
00828 //_________________________________________________________________________________
00829 
00830 // MultWii
00831 // Original code by Alex at: http://radio-commande.com/international/triwiicopter-design/
00832 // simplified IMU based on Kalman Filter
00833 // inspired from http://starlino.com/imu_guide.html
00834 // and http://www.starlino.com/imu_kalman_arduino.html
00835 // with this algorithm, we can get absolute angles for a stable mode integration
00836 
00837 real32 AccMW[3] = {0.0, 0.0, 1.0};   // init acc in stable mode
00838 real32 GyroMW[3] = {0.0, 0.0, 0.0};  // R obtained from last estimated value and gyro movement
00839 real32 Axz, Ayz;                     // angles between projection of R on XZ/YZ plane and Z axis
00840 
00841 void DoMultiWii(void) { // V1.6  NO YAW ANGLE ESTIMATE
00842 
00843     const real32 GyroWt = 50.0;  // gyro weight/smoothing factor
00844     const real32 GyroWtR = 1.0 / GyroWt;
00845 
00846     if ( Acc[UD] < 0.0 ) { // check not inverted
00847 
00848         if ( F.AccMagnitudeOK ) {
00849             Ayz = atan2( AccMW[LR], AccMW[UD] ) + Gyro[Roll]*dT;
00850             Axz = atan2( AccMW[BF], AccMW[UD] ) + Gyro[Pitch]*dT;
00851         } else {
00852             Ayz += Gyro[Roll]*dT;
00853             Axz += Gyro[Pitch]*dT;
00854         }
00855 
00856         // reverse calculation of GyroMW from Awz angles,
00857         // for formulae deduction see  http://starlino.com/imu_guide.html
00858         GyroMW[Roll] = sin( Ayz ) / sqrt( 1.0 + Sqr( cos( Ayz ) )*Sqr( tan( Axz ) ) );
00859         GyroMW[Pitch] = sin( Axz ) / sqrt( 1.0 + Sqr( cos( Axz ) )*Sqr( tan( Ayz ) ) );
00860         GyroMW[Yaw] = sqrt( fabs( 1.0 - Sqr( GyroMW[Roll] ) - Sqr( GyroMW[Pitch] ) ) );
00861 
00862         //combine accelerometer and gyro readings
00863         AccMW[LR] = ( -Acc[LR] + GyroWt*GyroMW[Roll] )*GyroWtR;
00864         AccMW[BF] = ( Acc[BF] + GyroWt*GyroMW[Pitch] )*GyroWtR;
00865         AccMW[UD] = ( -Acc[UD] + GyroWt*GyroMW[Yaw] )*GyroWtR;
00866     }
00867 
00868     EstAngle[Roll][MultiWii] =  Ayz;
00869     EstAngle[Pitch][MultiWii] =  Axz;
00870 
00871 //   EstRate[Roll][MultiWii] = GyroMW[Roll];
00872 //   EstRate[Pitch][MultiWii] = GyroMW[Pitch];
00873 
00874     EstRate[Roll][MultiWii] = Gyro[Roll];
00875     EstRate[Pitch][MultiWii] = Gyro[Pitch];
00876 
00877 } // DoMultiWii
00878 
00879 //_________________________________________________________________________________
00880 
00881 void AttitudeTest(void) {
00882 
00883     TxString("\r\nAttitude Test\r\n");
00884 
00885     GetAttitude();
00886 
00887     TxString("\r\ndT \t");
00888     TxVal32(dT*1000.0, 3, 0);
00889     TxString(" Sec.\r\n\r\n");
00890 
00891     if ( GyroType == IRSensors ) {
00892 
00893         TxString("IR Sensors:\r\n");
00894         TxString("\tRoll \t");
00895         TxVal32(IR[Roll]*100.0, 2, HT);
00896         TxNextLine();
00897         TxString("\tPitch\t");
00898         TxVal32(IR[Pitch]*100.0, 2, HT);
00899         TxNextLine();
00900         TxString("\tZ    \t");
00901         TxVal32(IR[Yaw]*100.0, 2, HT);
00902         TxNextLine();
00903         TxString("\tMin/Max\t");
00904         TxVal32(IRMin*100.0, 2, HT);
00905         TxVal32(IRMax*100.0, 2, HT);
00906         TxString("\tSwing\t");
00907         TxVal32(IRSwing*100.0, 2, HT);
00908         TxNextLine();
00909 
00910     } else {
00911 
00912         TxString("Gyro, Compensated, Max Delta(Deg./Sec.):\r\n");
00913         TxString("\tRoll \t");
00914         TxVal32(Gyro[Roll]*MILLIANGLE, 3, HT);
00915         TxVal32(Rate[Roll]*MILLIANGLE, 3, HT);
00916         TxVal32(GyroNoise[Roll]*MILLIANGLE,3, 0);
00917         TxNextLine();
00918         TxString("\tPitch\t");
00919         TxVal32(Gyro[Pitch]*MILLIANGLE, 3, HT);
00920         TxVal32(Rate[Pitch]*MILLIANGLE, 3, HT);
00921         TxVal32(GyroNoise[Pitch]*MILLIANGLE,3, 0);
00922         TxNextLine();
00923         TxString("\tYaw  \t");
00924         TxVal32(Gyro[Yaw]*MILLIANGLE, 3, HT);
00925         TxVal32(Rate[Yaw]*MILLIANGLE, 3, HT);
00926         TxVal32(GyroNoise[Yaw]*MILLIANGLE, 3, 0);
00927         TxNextLine();
00928 
00929         TxString("Accelerations , peak change(G):\r\n");
00930         TxString("\tB->F \t");
00931         TxVal32(Acc[BF]*1000.0, 3, HT);
00932         TxVal32( AccNoise[BF]*1000.0, 3, 0);
00933         TxNextLine();
00934         TxString("\tL->R \t");
00935         TxVal32(Acc[LR]*1000.0, 3, HT);
00936         TxVal32( AccNoise[LR]*1000.0, 3, 0);
00937         TxNextLine();
00938         TxString("\tU->D \t");
00939         TxVal32(Acc[UD]*1000.0, 3, HT);
00940         TxVal32( AccNoise[UD]*1000.0, 3, 0);
00941         TxNextLine();
00942     }
00943 
00944     if ( CompassType != HMC6352 ) {
00945         TxString("Magnetic:\r\n");
00946         TxString("\tX    \t");
00947         TxVal32(Mag[Roll].V, 0, 0);
00948         TxNextLine();
00949         TxString("\tY    \t");
00950         TxVal32(Mag[Pitch].V, 0, 0);
00951         TxNextLine();
00952         TxString("\tZ    \t");
00953         TxVal32(Mag[Yaw].V, 0, 0);
00954         TxNextLine();
00955     }
00956 
00957     TxString("Heading: \t");
00958     TxVal32(Make2Pi(Heading)*MILLIANGLE, 3, 0);
00959     TxNextLine();
00960 
00961 } // AttitudeTest
00962 
00963 void InitAttitude(void) {
00964 
00965     static uint8 a, s;
00966 
00967 #ifdef INC_IMU2
00968 
00969     FirstIMU2 = true;
00970     BetaIMU2 = sqrt(0.75) * GyroNoiseRadian[GyroType];
00971 
00972     //Quaternion orientation of earth frame relative to auxiliary frame.
00973     AEq_1 = 1.0;
00974     AEq_2 = 0.0;
00975     AEq_3 = 0.0;
00976     AEq_4 = 0.0;
00977 
00978     //Estimated orientation quaternion elements with initial conditions.
00979     SEq_1 = 1.0;
00980     SEq_2 = 0.0;
00981     SEq_3 = 0.0;
00982     SEq_4 = 0.0;
00983 
00984 #endif // INC_IMU2
00985 
00986     for ( a = 0; a < (uint8)2; a++ )
00987         AngleCF[a] = AngleKF[a] = BiasKF[a] = F0[a] = F1[a] = F2[a] = 0.0;
00988 
00989     for ( a = 0; a < (uint8)3; a++ )
00990         for ( s = 0; s < MaxAttitudeScheme; s++ )
00991             EstAngle[a][s] = EstRate[a][s] = 0.0;
00992 
00993 } // InitAttitude
00994