Ability to set the sample frequency

Author: PaulStoffregen

src/MadgwickAHRS.cpp

@@ -1,6 +1,6 @@
-//=====================================================================================================
+//=============================================================================================
 // MadgwickAHRS.c
-//=====================================================================================================
+//=============================================================================================
 //
 // Implementation of Madgwick's IMU and AHRS algorithms.
 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
@@ -14,23 +14,25 @@
 // 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 sampleFreqDef   512.0f          // sample frequency in Hz
+#define betaDef         0.1f            // 2 * proportional gain
 
 
-//====================================================================================================
+//============================================================================================
 // Functions
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
 Madgwick::Madgwick() {
@@ -39,6 +41,7 @@ Madgwick::Madgwick() {
 	q1 = 0.0f;
 	q2 = 0.0f;
 	q3 = 0.0f;
+	invSampleFreq = 1.0f / sampleFreqDef;
 }
 
 void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
@@ -67,7 +70,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
 		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
 		ax *= recipNorm;
 		ay *= recipNorm;
-		az *= recipNorm;   
+		az *= recipNorm;
 
 		// Normalise magnetometer measurement
 		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
@@ -124,10 +127,10 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
 	}
 
 	// Integrate rate of change of quaternion to yield quaternion
-	q0 += qDot1 * (1.0f / sampleFreq);
-	q1 += qDot2 * (1.0f / sampleFreq);
-	q2 += qDot3 * (1.0f / sampleFreq);
-	q3 += qDot4 * (1.0f / sampleFreq);
+	q0 += qDot1 * invSampleFreq;
+	q1 += qDot2 * invSampleFreq;
+	q2 += qDot3 * invSampleFreq;
+	q3 += qDot4 * invSampleFreq;
 
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -137,7 +140,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
 	q3 *= recipNorm;
 }
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // IMU algorithm update
 
 void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
@@ -195,10 +198,10 @@ void Madgwick::updateIMU(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);
-	q1 += qDot2 * (1.0f / sampleFreq);
-	q2 += qDot3 * (1.0f / sampleFreq);
-	q3 += qDot4 * (1.0f / sampleFreq);
+	q0 += qDot1 * invSampleFreq;
+	q1 += qDot2 * invSampleFreq;
+	q2 += qDot3 * invSampleFreq;
+	q3 += qDot4 * invSampleFreq;
 
 	// Normalise quaternion
 	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -208,7 +211,7 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float
 	q3 *= recipNorm;
 }
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
 
@@ -222,6 +225,6 @@ float Madgwick::invSqrt(float x) {
 	return y;
 }
 
-//====================================================================================================
+//============================================================================================
 // END OF CODE
-//====================================================================================================
+//============================================================================================

src/MadgwickAHRS.h

@@ -1,6 +1,6 @@
-//=====================================================================================================
+//=============================================================================================
 // MadgwickAHRS.h
-//=====================================================================================================
+//=============================================================================================
 //
 // Implementation of Madgwick's IMU and AHRS algorithms.
 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
@@ -13,14 +13,12 @@
 // 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>
-#define sampleFreq	512.0f		// sample frequency in Hz
-#define betaDef		0.1f		// 2 * proportional gain
 
-//----------------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------
 // Variable declaration
 class Madgwick{
 private:
@@ -30,11 +28,13 @@ class Madgwick{
     float q1;
     float q2;
     float q3;	// quaternion of sensor frame relative to auxiliary frame
+    float invSampleFreq;
 
-//---------------------------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------
 // 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);
     float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
@@ -43,6 +43,3 @@ class Madgwick{
 };
 #endif
 
-//=====================================================================================================
-// End of file
-//=====================================================================================================