Diff: MPU9250/MadgwickAHRS.h

Revision:

3:a4677501ae87

Parent:

0:4ff8aeb3e4d1

diff -r 0de4854743f7 -r a4677501ae87 MPU9250/MadgwickAHRS.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250/MadgwickAHRS.h	Tue Nov 30 08:13:05 2021 +0000
@@ -0,0 +1,77 @@
+//=============================================================================================
+// MadgwickAHRS.h
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+//=============================================================================================
+#ifndef MadgwickAHRS_h
+#define MadgwickAHRS_h
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+class Madgwick{
+private:
+    static float invSqrt(float x);
+    float beta;				// algorithm gain
+    float roll;
+    float pitch;
+    float yaw;
+    char anglesComputed;
+    void computeAngles();
+
+//-------------------------------------------------------------------------------------------
+// Function declarations
+public:
+    float invSampleFreq;
+
+    float q0;
+    float q1;
+    float q2;
+    float q3;	// quaternion of sensor frame relative to auxiliary frame
+
+    Madgwick(void);
+    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
+    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+    //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
+    //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
+    //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
+    float getRoll() {
+        if (!anglesComputed) computeAngles();
+        return roll * 57.29578f;
+    }
+    float getPitch() {
+        if (!anglesComputed) computeAngles();
+        return pitch * 57.29578f;
+    }
+    float getYaw() {
+        if (!anglesComputed) computeAngles();
+        return yaw * 57.29578f + 180.0f;
+    }
+    float getRollRadians() {
+        if (!anglesComputed) computeAngles();
+        return roll;
+    }
+    float getPitchRadians() {
+        if (!anglesComputed) computeAngles();
+        return pitch;
+    }
+    float getYawRadians() {
+        if (!anglesComputed) computeAngles();
+        return yaw;
+    }
+};
+#endif
+
+