Revision 29:6075f35f472f, committed 2016-07-06

Comitter:

uribotail

Date:

Wed Jul 06 11:44:22 2016 +0000

Parent:

28:76e2ba7a1ecd

Commit message:

20:44wada debag now

Changed in this revision

--- a/MPU9250_SPI.lib	Wed Jul 06 10:02:49 2016 +0000
+++ b/MPU9250_SPI.lib	Wed Jul 06 11:44:22 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/maedalab/code/MPU9250_SPI/#c3e3c8243945
+https://developer.mbed.org/teams/maedalab/code/MPU9250_SPI/#ca8c671ba87c

--- a/MadgwickAHRS.h	Wed Jul 06 10:02:49 2016 +0000
+++ b/MadgwickAHRS.h	Wed Jul 06 11:44:22 2016 +0000
@@ -43,11 +43,11 @@
     float qDot1, qDot2, qDot3, qDot4;
     float hx, hy;
     float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
-
-    printf("%f %f %f ",gx,gy,gz);
-    printf("%f %f %f ",ax,ay,az);
-    printf("%f %f %f ",mx,my,mz);
-    printf("\n");
+/*
+    printf("%+4.3f %+4.3f %+4.3f  ",gx,gy,gz);
+    printf("%+4.3f %+4.3f %+4.3f  ",ax,ay,az);
+    printf("%.3f %.3f %.3f ",mx,my,mz);
+    printf("\n");*/
     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
         updateIMU(gx, gy, gz, ax, ay, az);

--- 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;

--- a/main.cpp	Wed Jul 06 10:02:49 2016 +0000
+++ b/main.cpp	Wed Jul 06 11:44:22 2016 +0000
@@ -11,7 +11,7 @@
 #include "MPU9250.h"
 #include "MadgwickAHRS.h"
 #include "MahonyAHRS.h"
-#define DEBUG_putc  //Wada
+//#define DEBUG_putc  //Wada
 
 /* MPU9250 Library
  *
@@ -34,7 +34,7 @@
 mpu9250_spi     *imu[2];
 
 // define AHRS filters
-MahonyAHRS    *ahrs[2];
+MadgwickAHRS    *ahrs[2];
 
 // define serial objects
 Serial          pc(USBTX, USBRX);
@@ -55,8 +55,8 @@
     imu[0] = new mpu9250_spi(spi, p8);
     imu[1] = new mpu9250_spi(spi, p9);
 
-    ahrs[0] = new MahonyAHRS();
-    ahrs[1] = new MahonyAHRS();
+    ahrs[0] = new MadgwickAHRS();
+    ahrs[1] = new MadgwickAHRS();
 
     for(int i=0; i<2; i++) {
 
@@ -108,7 +108,8 @@
     
     // update filters
     for(int i=0; i<2; i++) 
-       ahrs[i]->MahonyAHRSupdate(
+    {
+       ahrs[i]->update(
         imu[i]->gyroscope_data[0]*DEGREE2RAD,
         imu[i]->gyroscope_data[1]*DEGREE2RAD,
         imu[i]->gyroscope_data[2]*DEGREE2RAD,
@@ -119,6 +120,14 @@
         imu[i]->Magnetometer[1],
         imu[i]->Magnetometer[2]
         );
+    }   
+     /*   printf("%+4.3f %+4.3f %+4.3f  ",
+        imu[1]->Magnetometer[0],
+        imu[1]->Magnetometer[1],
+        imu[1]->Magnetometer[2]
+        );
+    printf("\n");
+    */
     #ifdef DEBUG_putc
     pc.putc(0x34);  //STX
     #endif