Mahony.h

00001 
00002 //=============================================================================================
00003 // Mahony.h
00004 //=============================================================================================
00005 //
00006 // Madgwick's implementation of Mayhony's AHRS algorithm.
00007 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
00008 //
00009 // Date         Author          Notes
00010 // 29/09/2011   SOH Madgwick    Initial release
00011 // 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
00012 //
00013 //=============================================================================================
00014 #ifndef __Mahony_h__
00015 #define __Mahony_h__
00016 #include <math.h>
00017 
00018 //--------------------------------------------------------------------------------------------
00019 // Variable declaration
00020 
00021 class Mahony {
00022 private:
00023     float twoKp;        // 2 * proportional gain (Kp)
00024     float twoKi;        // 2 * integral gain (Ki)
00025     float q0, q1, q2, q3;   // quaternion of sensor frame relative to auxiliary frame
00026     float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
00027     float invSampleFreq;
00028     float roll, pitch, yaw;
00029     static float invSqrt(float x);
00030 
00031 
00032 //-------------------------------------------------------------------------------------------
00033 // Function declarations
00034 
00035 public:
00036     Mahony(float);
00037     void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
00038     void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
00039     void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
00040     char anglesComputed;
00041     void computeAngles();
00042     float getRoll() {
00043         if (!anglesComputed) computeAngles();
00044         return roll * 57.29578f;
00045     }
00046     float getPitch() {
00047         if (!anglesComputed) computeAngles();
00048         return pitch * 57.29578f;
00049     }
00050     float getYaw() {
00051         if (!anglesComputed) computeAngles();
00052         return yaw * 57.29578f + 180.0f;
00053     }
00054     float getRollRadians() {
00055         if (!anglesComputed) computeAngles();
00056         return roll;
00057     }
00058     float getPitchRadians() {
00059         if (!anglesComputed) computeAngles();
00060         return pitch;
00061     }
00062     float getYawRadians() {
00063         if (!anglesComputed) computeAngles();
00064         return yaw;
00065     }
00066     void getQuaternion(float *w, float *x, float *y, float *z) {
00067        *w = q0;
00068        *x = q1;
00069        *y = q2;
00070        *z = q3;
00071    }
00072 };
00073 
00074 #endif