KPC / MadgwickAHRS

MadgwickAHRS

Dependents:   Pmod_NAV

MadgwickAHRS.h@0:0f88b99a97ce, 2019-04-25 (annotated)

Committer:
771_8bit
Date:
Thu Apr 25 04:03:54 2019 +0000
Revision:
0:0f88b99a97ce
hello;

Who changed what in which revision?

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