Erik van de Coevering / Madgwick

Library implementing Madgwick's IMU and AHRS algorithms

Dependents:   Hexi_GPSIMU_Hotshoe

MadgwickAHRS.h@1:d7c70d593694, 2018-07-12 (annotated)

Committer:
Anaesthetix
Date:
Thu Jul 12 14:00:30 2018 +0000
Revision:
1:d7c70d593694
Parent:
0:9b434b5e28d4 
Corrected an error in the algorithm and added a regular square root instead of the fast square root as it does not increase processing time by that much.

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