Diff: Mahony.h

Revision:

1:36bbe04e1f6f

Child:

3:6811c0ce95f6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mahony.h	Fri Oct 26 05:56:25 2018 +0000
@@ -0,0 +1,73 @@
+
+//=============================================================================================
+// Mahony.h
+//=============================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+//
+//=============================================================================================
+#ifndef __Mahony_h__
+#define __Mahony_h__
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+
+class Mahony {
+private:
+    float twoKp;        // 2 * proportional gain (Kp)
+    float twoKi;        // 2 * integral gain (Ki)
+    float q0, q1, q2, q3;   // quaternion of sensor frame relative to auxiliary frame
+    float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
+    float invSampleFreq;
+    float roll, pitch, yaw;
+    char anglesComputed;
+    static float invSqrt(float x);
+    void computeAngles();
+
+//-------------------------------------------------------------------------------------------
+// Function declarations
+
+public:
+    Mahony();
+    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 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;
+    }
+    void getQuaternion(float *w, float *x, float *y, float *z) {
+       *w = q0;
+       *x = q1;
+       *y = q2;
+       *z = q3;
+   }
+};
+
+#endif
\ No newline at end of file