Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: attitude.c
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/attitude.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,602 @@
+// ===============================================================================================
+// = UAVXArm Quadrocopter Controller =
+// = Copyright (c) 2008 by Prof. Greg Egan =
+// = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
+// = http://code.google.com/p/uavp-mods/ http://uavp.ch =
+// ===============================================================================================
+
+// This is part of UAVXArm.
+
+// UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
+// General Public License as published by the Free Software Foundation, either version 3 of the
+// License, or (at your option) any later version.
+
+// UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
+// even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+// See the GNU General Public License for more details.
+
+// You should have received a copy of the GNU General Public License along with this program.
+// If not, see http://www.gnu.org/licenses/
+
+#include "UAVXArm.h"
+
+//#define USE_GYRO_RATE
+
+// Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW.
+
+void AttitudeFailsafeEstimate(void);
+void DoLegacyYawComp(void);
+void AttitudeTest(void);
+
+real32 dT, HalfdT, dTR, dTmS;
+uint32 uSp;
+uint8 AttitudeMethod = PremerlaniDCM; //MadgwickIMU PremerlaniDCM MadgwickAHRS;
+
+void AttitudeFailsafeEstimate(void) {
+
+ static uint8 i;
+
+ for ( i = 0; i < (uint8)3; i++ ) {
+ Rate[i] = Gyro[i];
+ Angle[i] += Rate[i] * dTmS;
+ }
+} // AttitudeFailsafeEstimate
+
+void DoLegacyYawComp(void) {
+
+ static real32 Temp, HE;
+
+ if ( F.CompassValid ) {
+ // + CCW
+ Temp = DesiredYaw - Trim[Yaw];
+ if ( fabs(Temp) > COMPASS_MIDDLE ) { // acquire new heading
+ DesiredHeading = Heading;
+ HE = 0.0;
+ } else {
+ HE = MakePi(DesiredHeading - Heading);
+ HE = Limit(HE, -SIXTHPI, SIXTHPI); // 30 deg limit
+ HE = HE * K[CompassKp];
+ Rate[Yaw] -= Limit(HE, -COMPASS_MAXDEV, COMPASS_MAXDEV);
+ }
+ }
+
+ Angle[Yaw] += Rate[Yaw];
+ Angle[Yaw] = Limit(Angle[Yaw], -K[YawIntLimit], K[YawIntLimit]);
+
+ Angle[Yaw] = DecayX(Angle[Yaw], 0.0002); // GKE added to kill gyro drift
+
+} // DoLegacyYawComp
+
+void GetAttitude(void) {
+
+ static uint32 Now;
+ static uint8 i;
+
+ if ( GyroType == IRSensors )
+ GetIRAttitude();
+ else {
+ GetGyroRates();
+ GetAccelerations();
+ }
+
+ if ( mSClock() > mS[CompassUpdate] ) {
+ mS[CompassUpdate] = mSClock() + COMPASS_UPDATE_MS;
+ GetHeading();
+#ifndef USE_DCM_YAW
+ DoLegacyYawComp();
+#endif // !USE_DCM_YAW
+ }
+
+ Now = uSClock();
+ dT = ( Now - uSp) * 0.000001;
+ HalfdT = 0.5 * dT;
+ dTR = 1.0 / dT;
+ uSp = Now;
+
+ if ( GyroType == IRSensors ) {
+
+ for ( i = 0; i < (uint8)2; i++ ) {
+ Rate[i] = ( Angle[i] - Anglep[i] ) * dT;
+ Anglep[i] = Angle[i];
+ }
+
+ Rate[Yaw] = 0.0; // need Yaw gyro!
+
+ } else {
+ DebugPin = true;
+ switch ( AttitudeMethod ) {
+ case PremerlaniDCM:
+ DCMUpdate();
+ DCMNormalise();
+ DCMDriftCorrection();
+ DCMEulerAngles();
+ break;
+ case MadgwickIMU:
+ IMUupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD]);
+ EulerAngles();
+ // DoLegacyYawComp();
+ break;
+ case MadgwickAHRS: // must have magnetometer
+ 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);
+ EulerAngles();
+ break;
+ } // switch
+
+ DebugPin = false;
+ }
+
+ F.NearLevel = Max(fabs(Angle[Roll]), fabs(Angle[Pitch])) < NAV_RTH_LOCKOUT;
+
+} // GetAttitude
+
+
+void AttitudeTest(void) {
+
+ TxString("\r\nAttitude Test\r\n");
+
+ GetAttitude();
+
+ TxString("\r\ndT \t");
+ TxVal32(dT * 1000.0, 3, 0);
+ TxString(" Sec.\r\n\r\n");
+
+ if ( GyroType == IRSensors ) {
+
+ TxString("IR Sensors:\r\n");
+ TxString("\tRoll \t");
+ TxVal32(IR[Roll] * 100.0, 2, HT);
+ TxNextLine();
+ TxString("\tPitch\t");
+ TxVal32(IR[Pitch] * 100.0, 2, HT);
+ TxNextLine();
+ TxString("\tZ \t");
+ TxVal32(IR[Yaw] * 100.0, 2, HT);
+ TxNextLine();
+ TxString("\tMin/Max\t");
+ TxVal32(IRMin * 100.0, 2, HT);
+ TxVal32(IRMax * 100.0, 2, HT);
+ TxString("\tSwing\t");
+ TxVal32(IRSwing * 100.0, 2, HT);
+ TxNextLine();
+
+ } else {
+
+ TxString("Rates (Raw and Compensated):\r\n");
+ TxString("\tRoll \t");
+ TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
+ TxVal32(Rate[Roll] * MILLIANGLE, 3, 0);
+ TxNextLine();
+ TxString("\tPitch\t");
+ TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
+ TxVal32(Rate[Pitch] * MILLIANGLE, 3, 0);
+ TxNextLine();
+ TxString("\tYaw \t");
+ TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
+ TxVal32(Rate[Yaw] * MILLIANGLE, 3, 0);
+ TxNextLine();
+
+ TxString("Accelerations:\r\n");
+ TxString("\tB->F \t");
+ TxVal32(Acc[BF] * 1000.0, 3, 0);
+ TxNextLine();
+ TxString("\tL->R \t");
+ TxVal32(Acc[LR] * 1000.0, 3, 0);
+ TxNextLine();
+ TxString("\tU->D \t");
+ TxVal32(Acc[UD] * 1000.0, 3, 0);
+ TxNextLine();
+ }
+
+ if ( CompassType != HMC6352 ) {
+ TxString("Magnetic:\r\n");
+ TxString("\tX \t");
+ TxVal32(Mag[Roll].V, 0, 0);
+ TxNextLine();
+ TxString("\tY \t");
+ TxVal32(Mag[Pitch].V, 0, 0);
+ TxNextLine();
+ TxString("\tZ \t");
+ TxVal32(Mag[Yaw].V, 0, 0);
+ TxNextLine();
+ }
+
+ TxString("Compass: \t");
+ TxVal32(Make2Pi(MagHeading) * MILLIANGLE, 3, 0);
+ TxNextLine();
+
+} // AttitudeTest
+
+//____________________________________________________________________________________________
+
+// The DCM formulation used here is due to W. Premerlani and P. Bizard in a paper entitled:
+// Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original
+// work by R. Mahony et al. - Thanks Rob!
+
+void DCMNormalise(void);
+void DCMDriftCorrection(void);
+void DCMMotionCompensation(void);
+void DCMUpdate(void);
+void DCMEulerAngles(void);
+
+// requires rescaling for the much faster PID cycle in UAVXArm
+// guess x5 initially
+#define Kp_RollPitch 25.0 // 5.0
+#define Ki_RollPitch 0.025 // 0.005
+#define Kp_Yaw 1.2
+#define Ki_Yaw 0.00002
+
+real32 RollPitchError[3] = {0,0,0};
+real32 OmegaV[3] = {0,0,0}; // corrected gyro data
+real32 OmegaP[3] = {0,0,0}; // proportional correction
+real32 OmegaI[3] = {0,0,0}; // integral correction
+real32 Omega[3] = {0,0,0};
+real32 DCM[3][3] = {{1,0,0 },{0,1,0} ,{0,0,1}};
+real32 U[3][3] = {{0,1,2},{ 3,4,5} ,{6,7,8}};
+real32 TempM[3][3] = {{0,0,0},{0,0,0},{ 0,0,0}};
+
+void DCMNormalise(void) {
+
+ static real32 Error = 0;
+ static real32 Renorm = 0.0;
+ static boolean Problem;
+ static uint8 r;
+
+ Error = -VDot(&DCM[0][0], &DCM[1][0]) * 0.5; //eq.19
+
+ VScale(&TempM[0][0], &DCM[1][0], Error); //eq.19
+ VScale(&TempM[1][0], &DCM[0][0], Error); //eq.19
+
+ VAdd(&TempM[0][0], &TempM[0][0], &DCM[0][0]); //eq.19
+ VAdd(&TempM[1][0], &TempM[1][0], &DCM[1][0]); //eq.19
+
+ VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]); // c= a * b eq.20
+
+ Problem = false;
+ for ( r = 0; r < (uint8)3; r++ ) {
+ Renorm = VDot(&TempM[r][0],&TempM[r][0]);
+ if ( (Renorm < 1.5625) && (Renorm > 0.64) )
+ Renorm = 0.5 * (3.0 - Renorm); //eq.21
+ else
+ if ( (Renorm < 100.0) && (Renorm > 0.01) )
+ Renorm = 1.0 / sqrt( Renorm );
+ else
+ Problem = true;
+
+ VScale(&DCM[r][0], &TempM[r][0], Renorm);
+ }
+
+ if ( Problem ) { // Divergent - force to initial conditions and hope!
+ DCM[0][0] = 1.0;
+ DCM[0][1] = 0.0;
+ DCM[0][2] = 0.0;
+ DCM[1][0] = 0.0;
+ DCM[1][1] = 1.0;
+ DCM[1][2] = 0.0;
+ DCM[2][0] = 0.0;
+ DCM[2][1] = 0.0;
+ DCM[2][2] = 1.0;
+ }
+
+} // DCMNormalise
+
+void DCMMotionCompensation(void) {
+ // compensation for rate of change of velocity LR/BF needs GPS velocity but
+ // updates probably too slow?
+ Acc[LR ] += 0.0;
+ Acc[BF] += 0.0;
+} // DCMMotionCompensation
+
+void DCMDriftCorrection(void) {
+
+ static real32 ScaledOmegaP[3], ScaledOmegaI[3];
+ static real32 YawError[3];
+ static real32 AccMagnitude, AccWeight;
+ static real32 ErrorCourse;
+
+ AccMagnitude = sqrt( Sqr(Acc[0]) + Sqr(Acc[1]) + Sqr(Acc[2]) );
+
+ // dynamic weighting of Accelerometer info (reliability filter)
+ // weight for Accelerometer info ( < 0.5G = 0.0, 1G = 1.0 , > 1.5G = 0.0)
+ AccWeight = Limit(1.0 - 2.0 * fabs(1.0 - AccMagnitude), 0.0, 1.0);
+
+ VCross(&RollPitchError[0], &Acc[0], &DCM[2][0]); //adjust the reference ground
+ VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch * AccWeight);
+
+ VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch * AccWeight);
+ VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
+
+#ifdef USE_DCM_YAW
+ // Yaw - drift correction based on compass/magnetometer heading
+ HeadingCos = cos( Heading );
+ HeadingSin = sin( Heading );
+ ErrorCourse = ( U[0][0] * HeadingSin ) - ( U[1][0] * HeadingCos );
+ VScale(YawError, &U[2][0], ErrorCourse );
+
+ VScale(&ScaledOmegaP[0], &YawError[0], Kp_Yaw );
+ VAdd(&OmegaP[0], &OmegaP[0], &ScaledOmegaP[0]);
+
+ VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw );
+ VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
+#endif // USE_DCM_YAW
+} // DCMDriftCorrection
+
+void DCMUpdate(void) {
+
+ static uint8 i, j, k;
+ static real32 op[3];
+
+ VAdd(&Omega[0], &Gyro[0], &OmegaI[0]);
+ VAdd(&OmegaV[0], &Omega[0], &OmegaP[0]);
+
+// MotionCompensation();
+
+ U[0][0] = 0.0;
+ U[0][1] = -dT * OmegaV[2]; //-z
+ U[0][2] = dT * OmegaV[1]; // y
+ U[1][0] = dT * OmegaV[2]; // z
+ U[1][1] = 0.0;
+ U[1][2] = -dT * OmegaV[0]; //-x
+ U[2][0] = -dT * OmegaV[1]; //-y
+ U[2][1] = dT * OmegaV[0]; // x
+ U[2][2] = 0.0;
+
+ for ( i = 0; i < (uint8)3; i++ )
+ for ( j = 0; j < (uint8)3; j++ ) {
+ for ( k = 0; k < (uint8)3; k++ )
+ op[k] = DCM[i][k] * U[k][j];
+
+ TempM[i][j] = op[0] + op[1] + op[2];
+ }
+
+ for ( i = 0; i < (uint8)3; i++ )
+ for (j = 0; j < (uint8)3; j++ )
+ DCM[i][j] += TempM[i][j];
+
+} // DCMUpdate
+
+void DCMEulerAngles(void) {
+
+ static uint8 g;
+
+ for ( g = 0; g < (uint8)3; g++ )
+ Rate[g] = Gyro[g];
+
+ Angle[Pitch] = asin(DCM[2][0]);
+ Angle[Roll] = -atan2(DCM[2][1], DCM[2][2]);
+
+#ifdef USE_DCM_YAW
+ Angle[Yaw] = atan2(DCM[1][0], DCM[0][0]);
+#endif // USE_DCM_YAW
+
+} // DCMEulerAngles
+
+//___________________________________________________________________________________
+
+// IMU.c
+// S.O.H. Madgwick
+// 25th September 2010
+
+// Description:
+
+// Quaternion implementation of the 'DCM filter' [Mahony et al.].
+
+// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
+
+// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
+// orientation. See my report for an overview of the use of quaternions in this application.
+
+// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
+// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer
+// units are irrelevant as the vector is normalised.
+
+#include <math.h>
+
+void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az);
+
+#define MKp 2.0 // proportional gain governs rate of convergence to accelerometer/magnetometer
+#define MKi 0.005f // integral gain governs rate of convergence of gyroscope biases
+
+real32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; // quaternion elements representing the estimated orientation
+real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0; // scaled integral error
+
+void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) {
+
+ static real32 rnorm;
+ static real32 vx, vy, vz;
+ static real32 ex, ey, ez;
+ static real32 aMag;
+
+//swap z and y?
+
+ // normalise the measurements
+ aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero Acc values into AHRS is FATAL
+ if ( aMag < 0.9 ) {
+ ax = ay = 0.0;
+ az = 1.0;
+ } else {
+ rnorm = 1.0/aMag;
+ ax *= rnorm;
+ ay *= rnorm;
+ az *= rnorm;
+ }
+
+ // estimated direction of gravity
+ vx = 2.0*(q1*q3 - q0*q2);
+ vy = 2.0*(q0*q1 + q2*q3);
+ vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3);
+
+ // error is sum of cross product between reference direction of field and direction measured by sensor
+ ex = (ay*vz - az*vy);
+ ey = (az*vx - ax*vz);
+ ez = (ax*vy - ay*vx);
+
+ // integral error scaled integral gain
+ exInt += ex*MKi;
+ eyInt += ey*MKi;
+ ezInt += ez*MKi;
+
+ // adjusted gyroscope measurements
+ gx += MKp*ex + exInt;
+ gy += MKp*ey + eyInt;
+ gz += MKp*ez + ezInt;
+
+ // integrate quaternion rate and normalise
+ q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
+ q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
+ q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
+ q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;
+
+ // normalise quaternion
+ rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
+ q0 *= rnorm;
+ q1 *= rnorm;
+ q2 *= rnorm;
+ q3 *= rnorm;
+
+} // IMUupdate
+
+//_________________________________________________________________________________
+
+// AHRS.c
+// S.O.H. Madgwick
+// 25th August 2010
+
+// Description:
+
+// Quaternion implementation of the 'DCM filter' [Mahoney et al]. Incorporates the magnetic distortion
+// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
+// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
+// axis only.
+
+// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
+
+// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
+// orientation. See my report for an overview of the use of quaternions in this application.
+
+// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
+// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are
+// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
+
+void AHRSupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) {
+
+ static real32 rnorm;
+ static real32 hx, hy, hz, bx2, bz2, mx2, my2, mz2;
+ static real32 vx, vy, vz, wx, wy, wz;
+ static real32 ex, ey, ez;
+ static real32 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+ static real32 aMag;
+
+ // auxiliary variables to reduce number of repeated operations
+ q0q0 = q0*q0;
+ q0q1 = q0*q1;
+ q0q2 = q0*q2;
+ q0q3 = q0*q3;
+ q1q1 = q1*q1;
+ q1q2 = q1*q2;
+ q1q3 = q1*q3;
+ q2q2 = q2*q2;
+ q2q3 = q2*q3;
+ q3q3 = q3*q3;
+
+ aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero values into AHRS is FATAL
+ if ( aMag < 0.9 ) {
+ ax = ay = 0.0;
+ az = 1.0;
+ } else {
+ // normalise the measurements
+ rnorm = 1.0/aMag;
+ ax *= rnorm;
+ ay *= rnorm;
+ az *= rnorm;
+ }
+
+ rnorm = 1.0/sqrt(Sqr(mx) + Sqr(my) + Sqr(mz));
+ mx *= rnorm;
+ my *= rnorm;
+ mz *= rnorm;
+ mx2 = 2.0 * mx;
+ my2 = 2.0 * my;
+ mz2 = 2.0 * mz;
+
+ // compute reference direction of flux
+ hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2);
+ hy = mx2*(q1q2 + q0q3) + my2*(0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1);
+ hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*(0.5 - q1q1 - q2q2);
+ bx2 = 2.0*sqrt(Sqr(hx) + Sqr(hy));
+ bz2 = 2.0*hz;
+
+ // estimated direction of gravity and flux (v and w)
+ vx = 2.0*(q1q3 - q0q2);
+ vy = 2.0*(q0q1 + q2q3);
+ vz = q0q0 - q1q1 - q2q2 + q3q3;
+
+ wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2);
+ wy = bx2*(q1q2 - q0q3) + bz2*(q0q1 + q2q3);
+ wz = bx2*(q0q2 + q1q3) + bz2*(0.5 - q1q1 - q2q2);
+
+ // error is sum of cross product between reference direction of fields and direction measured by sensors
+ ex = (ay*vz - az*vy) + (my*wz - mz*wy);
+ ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
+ ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
+
+ // integral error scaled integral gain
+ exInt += ex*MKi;
+ eyInt += ey*MKi;
+ ezInt += ez*MKi;
+
+ // adjusted gyroscope measurements
+ gx += MKp*ex + exInt;
+ gy += MKp*ey + eyInt;
+ gz += MKp*ez + ezInt;
+
+ // integrate quaternion rate and normalise
+ q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
+ q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
+ q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
+ q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;
+
+ // normalise quaternion
+ rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
+ q0 *= rnorm;
+ q1 *= rnorm;
+ q2 *= rnorm;
+ q3 *= rnorm;
+
+} // AHRSupdate
+
+void EulerAngles(void) {
+
+ static uint8 g;
+
+ Angle[Roll] = atan2(2.0*q2*q3 - 2.0*q0*q1 , 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0);
+ Angle[Pitch] = asin(2.0*q1*q2 - 2.0*q0*q2);
+ Angle[Yaw] = -atan2(2.0*q1*q2 - 2.0*q0*q3 , 2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);
+
+ for ( g = 0; g < (uint8)3; g++ ) {
+#ifdef USE_GYRO_RATE
+ Rate[g] = Gyro[g];
+#else
+ Rate[g] = ( Angle[g] - Anglep[g] ) * dTR;
+ Anglep[g] = Angle[g];
+#endif // USE_GYRO_RATE
+ }
+
+} // EulerAngles
+
+/*
+heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
+attitude = asin(2*qx*qy + 2*qz*qw)
+bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)
+
+except when qx*qy + qz*qw = 0.5 (north pole)
+which gives:
+heading = 2 * atan2(x,w)
+bank = 0
+and when qx*qy + qz*qw = -0.5 (south pole)
+which gives:
+heading = -2 * atan2(x,w)
+bank = 0
+*/
+
+
+