Diff: MahonyAHRS.h

Revision:

29:6075f35f472f

Parent:

28:76e2ba7a1ecd

--- a/MahonyAHRS.h	Wed Jul 06 10:02:49 2016 +0000
+++ b/MahonyAHRS.h	Wed Jul 06 11:44:22 2016 +0000
@@ -1,36 +1,51 @@
-//---------------------------------------------------------------------------------------------------
-// Definitions
-
-#define sampleFreq	500.0f			// sample frequency in Hz
-#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain
-#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain
-
-class MahonyAHRS{
+//=====================================================================================================
+// MahonyAHRS.h
+//=====================================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date			Author			Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+//=====================================================================================================
 
-//---------------------------------------------------------------------------------------------------
-// Variable definitions
-private:
-	//volatile float twoKp;											// 2 * proportional gain (Kp)
-	//volatile float twoKi;											// 2 * integral gain (Ki)
-	//volatile float q0, q1, q2, q3;					// quaternion of sensor frame relative to auxiliary frame
-	//volatile float integralFBx,  integralFBy, integralFBz;	// integral error terms scaled by Ki
 //----------------------------------------------------------------------------------------------------
-public:
-	// Variable declaration
-	volatile float twoKp;			// 2 * proportional gain (Kp)
-	volatile float twoKi;			// 2 * integral gain (Ki)
-	volatile float q0, q1, q2, q3;	// quaternion of sensor frame relative to auxiliary frame
-	volatile float integralFBx,  integralFBy, integralFBz;	// integral error terms scaled by Ki
-	MahonyAHRS();
-	void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
-	void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-	float invSqrt(float x);
+// Variable declaration
+
+extern volatile float twoKp;			// 2 * proportional gain (Kp)
+extern volatile float twoKi;			// 2 * integral gain (Ki)
+extern volatile float q0, q1, q2, q3;	// quaternion of sensor frame relative to auxiliary frame
 
 //---------------------------------------------------------------------------------------------------
 // Function declarations
 
-};
+void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+
+
+#include <math.h>
+
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreq	512.0f			// sample frequency in Hz
+#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain
+#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain
 
+//---------------------------------------------------------------------------------------------------
+// Variable definitions
+
+volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
+volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
+volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+float invSqrt(float x);
 
 //====================================================================================================
 // Functions
@@ -38,20 +53,7 @@
 //---------------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-MahonyAHRS::MahonyAHRS()
-{
-    twoKp = twoKpDef;
-    twoKi = twoKiDef;
-    integralFBx = 0.0f;
-    integralFBy = 0.0f;
-    integralFBz = 0.0f;
-    q0 = 1.0f;
-    q1 = 0.0f;
-    q2 = 0.0f;
-    q3 = 0.0f;          // quaternion of sensor frame relative to auxiliary frame
-}
-
-void MahonyAHRS::MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
 	float recipNorm;
     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
 	float hx, hy, bx, bz;
@@ -155,7 +157,7 @@
 //---------------------------------------------------------------------------------------------------
 // IMU algorithm update
 
-void MahonyAHRS::MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
 	float recipNorm;
 	float halfvx, halfvy, halfvz;
 	float halfex, halfey, halfez;
@@ -225,7 +227,7 @@
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
 
-float MahonyAHRS::invSqrt(float x) {
+float invSqrt(float x) {
 	float halfx = 0.5f * x;
 	float y = x;
 	long i = *(long*)&y;