Erik van de Coevering / Madgwick

Library implementing Madgwick's IMU and AHRS algorithms

Dependents:   Hexi_GPSIMU_Hotshoe

MadgwickAHRS.h@0:9b434b5e28d4, 2016-12-18 (annotated)

Committer:
Anaesthetix
Date:
Sun Dec 18 21:50:15 2016 +0000
Revision:
0:9b434b5e28d4
Library implementing Madgwick's IMU and AHRS algorithms

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:9b434b5e28d4 1 //=============================================================================================
Anaesthetix 0:9b434b5e28d4 2 // MadgwickAHRS.h
Anaesthetix 0:9b434b5e28d4 3 //=============================================================================================
Anaesthetix 0:9b434b5e28d4 4 //
Anaesthetix 0:9b434b5e28d4 5 // Implementation of Madgwick's IMU and AHRS algorithms.
Anaesthetix 0:9b434b5e28d4 6 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Anaesthetix 0:9b434b5e28d4 7 //
Anaesthetix 0:9b434b5e28d4 8 // From the x-io website "Open-source resources available on this website are
Anaesthetix 0:9b434b5e28d4 9 // provided under the GNU General Public Licence unless an alternative licence
Anaesthetix 0:9b434b5e28d4 10 // is provided in source."
Anaesthetix 0:9b434b5e28d4 11 //
Anaesthetix 0:9b434b5e28d4 12 // Date Author Notes
Anaesthetix 0:9b434b5e28d4 13 // 29/09/2011 SOH Madgwick Initial release
Anaesthetix 0:9b434b5e28d4 14 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
Anaesthetix 0:9b434b5e28d4 15 //
Anaesthetix 0:9b434b5e28d4 16 //=============================================================================================
Anaesthetix 0:9b434b5e28d4 17 #ifndef MadgwickAHRS_h
Anaesthetix 0:9b434b5e28d4 18 #define MadgwickAHRS_h
Anaesthetix 0:9b434b5e28d4 19 #include <math.h>
Anaesthetix 0:9b434b5e28d4 20
Anaesthetix 0:9b434b5e28d4 21 //--------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 22 // Variable declaration
Anaesthetix 0:9b434b5e28d4 23 class Madgwick{
Anaesthetix 0:9b434b5e28d4 24 private:
Anaesthetix 0:9b434b5e28d4 25 static float invSqrt(float x);
Anaesthetix 0:9b434b5e28d4 26 float beta; // algorithm gain
Anaesthetix 0:9b434b5e28d4 27 float q0;
Anaesthetix 0:9b434b5e28d4 28 float q1;
Anaesthetix 0:9b434b5e28d4 29 float q2;
Anaesthetix 0:9b434b5e28d4 30 float q3; // quaternion of sensor frame relative to auxiliary frame
Anaesthetix 0:9b434b5e28d4 31 //float invSampleFreq;
Anaesthetix 0:9b434b5e28d4 32 float roll;
Anaesthetix 0:9b434b5e28d4 33 float pitch;
Anaesthetix 0:9b434b5e28d4 34 float yaw;
Anaesthetix 0:9b434b5e28d4 35 char anglesComputed;
Anaesthetix 0:9b434b5e28d4 36 void computeAngles();
Anaesthetix 0:9b434b5e28d4 37
Anaesthetix 0:9b434b5e28d4 38 //-------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 39 // Function declarations
Anaesthetix 0:9b434b5e28d4 40 public:
Anaesthetix 0:9b434b5e28d4 41 float invSampleFreq;
Anaesthetix 0:9b434b5e28d4 42 Madgwick(void);
Anaesthetix 0:9b434b5e28d4 43 void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
Anaesthetix 0:9b434b5e28d4 44 void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
Anaesthetix 0:9b434b5e28d4 45 void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
Anaesthetix 0:9b434b5e28d4 46 //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
Anaesthetix 0:9b434b5e28d4 47 //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
Anaesthetix 0:9b434b5e28d4 48 //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
Anaesthetix 0:9b434b5e28d4 49 float getRoll() {
Anaesthetix 0:9b434b5e28d4 50 if (!anglesComputed) computeAngles();
Anaesthetix 0:9b434b5e28d4 51 return roll * 57.29578f;
Anaesthetix 0:9b434b5e28d4 52 }
Anaesthetix 0:9b434b5e28d4 53 float getPitch() {
Anaesthetix 0:9b434b5e28d4 54 if (!anglesComputed) computeAngles();
Anaesthetix 0:9b434b5e28d4 55 return pitch * 57.29578f;
Anaesthetix 0:9b434b5e28d4 56 }
Anaesthetix 0:9b434b5e28d4 57 float getYaw() {
Anaesthetix 0:9b434b5e28d4 58 if (!anglesComputed) computeAngles();
Anaesthetix 0:9b434b5e28d4 59 return yaw * 57.29578f + 180.0f;
Anaesthetix 0:9b434b5e28d4 60 }
Anaesthetix 0:9b434b5e28d4 61 float getRollRadians() {
Anaesthetix 0:9b434b5e28d4 62 if (!anglesComputed) computeAngles();
Anaesthetix 0:9b434b5e28d4 63 return roll;
Anaesthetix 0:9b434b5e28d4 64 }
Anaesthetix 0:9b434b5e28d4 65 float getPitchRadians() {
Anaesthetix 0:9b434b5e28d4 66 if (!anglesComputed) computeAngles();
Anaesthetix 0:9b434b5e28d4 67 return pitch;
Anaesthetix 0:9b434b5e28d4 68 }
Anaesthetix 0:9b434b5e28d4 69 float getYawRadians() {
Anaesthetix 0:9b434b5e28d4 70 if (!anglesComputed) computeAngles();
Anaesthetix 0:9b434b5e28d4 71 return yaw;
Anaesthetix 0:9b434b5e28d4 72 }
Anaesthetix 0:9b434b5e28d4 73 };
Anaesthetix 0:9b434b5e28d4 74 #endif