New Madgwick filter library

src/control/MadgwickAHRS.cpp

@@ -1,47 +1,51 @@
-//=====================================================================================================
+//=============================================================================================
 // MadgwickAHRS.c
-//=====================================================================================================
+//=============================================================================================
 //
 // Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_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
 // 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
 //
-//=====================================================================================================
+//=============================================================================================
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // Header files
 
 #include "MadgwickAHRS.h"
 #include <math.h>
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // Definitions
 
-#define sampleFreq	512.0f		// sample frequency in Hz
+#define sampleFreqDef   512.0f          // sample frequency in Hz
 #define betaDef         0.1f            // 2 * proportional gain
 
-//---------------------------------------------------------------------------------------------------
-// Variable definitions
 
-volatile float beta = betaDef;								// 2 * proportional gain (Kp)
+//============================================================================================
-volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame
-
-//---------------------------------------------------------------------------------------------------
-// Function declarations
-
-float invSqrt(float x);
-
-//====================================================================================================
 // Functions
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+Madgwick::Madgwick() {
+	beta = betaDef;
+	q0 = 1.0f;
+	q1 = 0.0f;
+	q2 = 0.0f;
+	q3 = 0.0f;
+	invSampleFreq = 1.0f / sampleFreqDef;
+	anglesComputed = 0;
+}
+
+void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
 	float recipNorm;
 	float s0, s1, s2, s3;
 	float qDot1, qDot2, qDot3, qDot4;
@@ -50,10 +54,15 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float
 
 	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
 	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-		MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+		updateIMU(gx, gy, gz, ax, ay, az);
 		return;
 	}
 
+	// Convert gyroscope degrees/sec to radians/sec
+	gx *= 0.0174533f;
+	gy *= 0.0174533f;
+	gz *= 0.0174533f;
+
 	// Rate of change of quaternion from gyroscope
 	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
 	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
@@ -100,7 +109,7 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float
 		// Reference direction of Earth's magnetic field
 		hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
 		hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
-		_2bx = sqrt(hx * hx + hy * hy);
+		_2bx = sqrtf(hx * hx + hy * hy);
 		_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
 		_4bx = 2.0f * _2bx;
 		_4bz = 2.0f * _2bz;
@@ -124,10 +133,10 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float
 	}
 
 	// Integrate rate of change of quaternion to yield quaternion
-	q0 += qDot1 * (1.0f / sampleFreq);
+	q0 += qDot1 * invSampleFreq;
-	q1 += qDot2 * (1.0f / sampleFreq);
+	q1 += qDot2 * invSampleFreq;
-	q2 += qDot3 * (1.0f / sampleFreq);
+	q2 += qDot3 * invSampleFreq;
-	q3 += qDot4 * (1.0f / sampleFreq);
+	q3 += qDot4 * invSampleFreq;
 
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -135,17 +144,23 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float
 	q1 *= recipNorm;
 	q2 *= recipNorm;
 	q3 *= recipNorm;
+	anglesComputed = 0;
 }
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // IMU algorithm update
 
-void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
 	float recipNorm;
 	float s0, s1, s2, s3;
 	float qDot1, qDot2, qDot3, qDot4;
 	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
 
+	// Convert gyroscope degrees/sec to radians/sec
+	gx *= 0.0174533f;
+	gy *= 0.0174533f;
+	gz *= 0.0174533f;
+
 	// Rate of change of quaternion from gyroscope
 	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
 	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
@@ -195,10 +210,10 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo
 	}
 
 	// Integrate rate of change of quaternion to yield quaternion
-	q0 += qDot1 * (1.0f / sampleFreq);
+	q0 += qDot1 * invSampleFreq;
-	q1 += qDot2 * (1.0f / sampleFreq);
+	q1 += qDot2 * invSampleFreq;
-	q2 += qDot3 * (1.0f / sampleFreq);
+	q2 += qDot3 * invSampleFreq;
-	q3 += qDot4 * (1.0f / sampleFreq);
+	q3 += qDot4 * invSampleFreq;
 
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -206,27 +221,30 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo
 	q1 *= recipNorm;
 	q2 *= recipNorm;
 	q3 *= recipNorm;
+	anglesComputed = 0;
 }
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
 
-float invSqrt(float x) {
+float Madgwick::invSqrt(float x) {
 	float halfx = 0.5f * x;
 	float y = x;
 	long i = *(long*)&y;
 	i = 0x5f3759df - (i>>1);
 	y = *(float*)&i;
 	y = y * (1.5f - (halfx * y * y));
+	y = y * (1.5f - (halfx * y * y));
 	return y;
 }
 
-float getPitch()
+//-------------------------------------------------------------------------------------------
-{
-	return asinf(-2.0f * (q1*q3 - q0*q2));
-}
 
-//====================================================================================================
+void Madgwick::computeAngles()
-// END OF CODE
+{
-//====================================================================================================
+	roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
+	pitch = asinf(-2.0f * (q1*q3 - q0*q2));
+	yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+	anglesComputed = 1;
+}

src/control/MadgwickAHRS.h

@@ -1,32 +1,71 @@
-//=====================================================================================================
+//=============================================================================================
 // MadgwickAHRS.h
-//=====================================================================================================
+//=============================================================================================
 //
 // Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_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 q0;
+    float q1;
+    float q2;
+    float q3;	// quaternion of sensor frame relative to auxiliary frame
+    float invSampleFreq;
+    float roll;
+    float pitch;
+    float yaw;
+    char anglesComputed;
+    void computeAngles();
 
-extern volatile float beta;				// algorithm gain
+//-------------------------------------------------------------------------------------------
-extern volatile float q0, q1, q2, q3;	// quaternion of sensor frame relative to auxiliary frame
-
-//---------------------------------------------------------------------------------------------------
 // Function declarations
+public:
+    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);
     
-void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+    float getRoll() {
-void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+        if (!anglesComputed) computeAngles();
-
+        return roll * 57.29578f;
-float getPitch();
+    }
+    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
-//=====================================================================================================
-// End of file
-//=====================================================================================================