MadgwickAHRS.h

00001 //=============================================================================================
00002 // MadgwickAHRS.h
00003 //=============================================================================================
00004 //
00005 // Implementation of Madgwick's IMU and AHRS algorithms.
00006 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
00007 //
00008 // From the x-io website "Open-source resources available on this website are
00009 // provided under the GNU General Public Licence unless an alternative licence
00010 // is provided in source."
00011 //
00012 // Date         Author          Notes
00013 // 29/09/2011   SOH Madgwick    Initial release
00014 // 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
00015 //
00016 //=============================================================================================
00017 #ifndef MadgwickAHRS_h
00018 #define MadgwickAHRS_h
00019 #include <math.h>
00020 
00021 //--------------------------------------------------------------------------------------------
00022 // Variable declaration
00023 class Madgwick{
00024 private:
00025     static float invSqrt(float x);
00026     float beta;             // algorithm gain
00027     float q0;
00028     float q1;
00029     float q2;
00030     float q3;   // quaternion of sensor frame relative to auxiliary frame
00031     //float invSampleFreq;
00032     float roll;
00033     float pitch;
00034     float yaw;
00035     char anglesComputed;
00036     void computeAngles();
00037 
00038 //-------------------------------------------------------------------------------------------
00039 // Function declarations
00040 public:
00041     float invSampleFreq;
00042     Madgwick(void);
00043     void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
00044     void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
00045     void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
00046     //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
00047     //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
00048     //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
00049     float getRoll() {
00050         if (!anglesComputed) computeAngles();
00051         return roll * 57.29578f;
00052     }
00053     float getPitch() {
00054         if (!anglesComputed) computeAngles();
00055         return pitch * 57.29578f;
00056     }
00057     float getYaw() {
00058         if (!anglesComputed) computeAngles();
00059         return yaw * 57.29578f + 180.0f;
00060     }
00061     float getRollRadians() {
00062         if (!anglesComputed) computeAngles();
00063         return roll;
00064     }
00065     float getPitchRadians() {
00066         if (!anglesComputed) computeAngles();
00067         return pitch;
00068     }
00069     float getYawRadians() {
00070         if (!anglesComputed) computeAngles();
00071         return yaw;
00072     }
00073 };
00074 #endif
00075