MahonyAHRS.h

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