Merge pull request #7 from PaulStoffregen/master

Improve performance

Author: mbanzi

keywords.txt

@@ -1,9 +1,9 @@
 name=Madgwick
-version=1.0
+version=1.1
 author=Arduino
 maintainer=Arduino <[email protected]>
 sentence=Helpers for MadgwickAHRS algorithm
 paragraph=This library wraps the official implementation of MadgwickAHRS algorithm to get orientation of an object based on accelerometer and gyroscope readings
 category=Data Processing
-url=http://arduino.cc/en/Reference/XXXXXX
+url=https://github.com/arduino-libraries/MadgwickAHRS
 architectures=*

library.properties

@@ -3,7 +3,11 @@
 //=====================================================================================================
 //
 // 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
@@ -29,6 +33,14 @@
 //---------------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
+Madgwick::Madgwick() {
+	beta = betaDef;
+	q0 = 1.0f;
+	q1 = 0.0f;
+	q2 = 0.0f;
+	q3 = 0.0f;
+}
+
 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;

src/MadgwickAHRS.cpp

@@ -3,7 +3,11 @@
 //=====================================================================================================
 //
 // 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
@@ -20,22 +24,22 @@
 // Variable declaration
 class Madgwick{
 private:
-    float invSqrt(float x);
-    volatile float beta = betaDef;				// algorithm gain
-    volatile float q0 = 1.0f;
-    volatile float q1 = 0.0f;
-    volatile float q2 = 0.0f;
-    volatile float q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame
+    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
 
 //---------------------------------------------------------------------------------------------------
 // Function declarations
 public:
-    Madgwick(void){};
+    Madgwick(void);
     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 atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1);};
-    float getRoll(){return -1 * asin(2 * q1 * q3 + 2 * q0 * q2);};
-    float getYaw(){return atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0 * q0 + 2 * q1 * q1 - 1);};
+    float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
+    float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
+    float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
 };
 #endif