Diff: MahonyAHRS.h

Revision:

28:76e2ba7a1ecd

Parent:

27:7dd32c696d17

Child:

29:6075f35f472f

diff -r 7dd32c696d17 -r 76e2ba7a1ecd MahonyAHRS.h
--- a/MahonyAHRS.h	Wed Jul 06 09:30:37 2016 +0000
+++ b/MahonyAHRS.h	Wed Jul 06 10:02:49 2016 +0000
@@ -1,51 +1,36 @@
-//=====================================================================================================
-// 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
-//
-//=====================================================================================================
+//---------------------------------------------------------------------------------------------------
+// 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{
 
+//---------------------------------------------------------------------------------------------------
+// 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
 //----------------------------------------------------------------------------------------------------
-// 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
+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);
 
 //---------------------------------------------------------------------------------------------------
 // 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
@@ -53,7 +38,20 @@
 //---------------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+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) {
 	float recipNorm;
     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
 	float hx, hy, bx, bz;
@@ -157,7 +155,7 @@
 //---------------------------------------------------------------------------------------------------
 // IMU algorithm update
 
-void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+void MahonyAHRS::MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
 	float recipNorm;
 	float halfvx, halfvy, halfvz;
 	float halfex, halfey, halfez;
@@ -227,7 +225,7 @@
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
 
-float invSqrt(float x) {
+float MahonyAHRS::invSqrt(float x) {
 	float halfx = 0.5f * x;
 	float y = x;
 	long i = *(long*)&y;