Revision 31:f30e4effec54, committed 2016-07-07

Comitter:

uribotail

Date:

Thu Jul 07 06:04:05 2016 +0000

Parent:

30:a1bbb934b053

Commit message:

drift corrected

Changed in this revision

--- a/MadgwickAHRS.h	Thu Jul 07 04:35:04 2016 +0000
+++ b/MadgwickAHRS.h	Thu Jul 07 06:04:05 2016 +0000
@@ -1,7 +1,7 @@
 //---------------------------------------------------------------------------------------------------
 // Definitions
 
-#define sampleFreq  500.0f      // sample frequency in Hz
+#define sampleFreq  150.0f      // sample frequency in Hz
 #define betaDef     0.02f        // 2 * proportional gain
 #define PI          3.14159265358979f
 #define DEGREE2RAD  PI/180.0f

--- a/MahonyAHRS.h	Thu Jul 07 04:35:04 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,247 +0,0 @@
-//=====================================================================================================
-// 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 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
-
-//---------------------------------------------------------------------------------------------------
-// AHRS algorithm update
-
-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;
-	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
-	float halfex, halfey, halfez;
-	float qa, qb, qc;
-
-	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
-	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-		MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
-		return;
-	}
-
-	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-		// Normalise accelerometer measurement
-		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-		ax *= recipNorm;
-		ay *= recipNorm;
-		az *= recipNorm;     
-
-		// Normalise magnetometer measurement
-		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-		mx *= recipNorm;
-		my *= recipNorm;
-		mz *= recipNorm;   
-
-        // Auxiliary variables to avoid repeated arithmetic
-        q0q0 = q0 * q0;
-        q0q1 = q0 * q1;
-        q0q2 = q0 * q2;
-        q0q3 = q0 * q3;
-        q1q1 = q1 * q1;
-        q1q2 = q1 * q2;
-        q1q3 = q1 * q3;
-        q2q2 = q2 * q2;
-        q2q3 = q2 * q3;
-        q3q3 = q3 * q3;   
-
-        // Reference direction of Earth's magnetic field
-        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
-        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
-        bx = sqrt(hx * hx + hy * hy);
-        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
-
-		// Estimated direction of gravity and magnetic field
-		halfvx = q1q3 - q0q2;
-		halfvy = q0q1 + q2q3;
-		halfvz = q0q0 - 0.5f + q3q3;
-        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
-        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
-        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
-	
-		// Error is sum of cross product between estimated direction and measured direction of field vectors
-		halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
-		halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
-		halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
-
-		// Compute and apply integral feedback if enabled
-		if(twoKi > 0.0f) {
-			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
-			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
-			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
-			gx += integralFBx;	// apply integral feedback
-			gy += integralFBy;
-			gz += integralFBz;
-		}
-		else {
-			integralFBx = 0.0f;	// prevent integral windup
-			integralFBy = 0.0f;
-			integralFBz = 0.0f;
-		}
-
-		// Apply proportional feedback
-		gx += twoKp * halfex;
-		gy += twoKp * halfey;
-		gz += twoKp * halfez;
-	}
-	
-	// Integrate rate of change of quaternion
-	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
-	gy *= (0.5f * (1.0f / sampleFreq));
-	gz *= (0.5f * (1.0f / sampleFreq));
-	qa = q0;
-	qb = q1;
-	qc = q2;
-	q0 += (-qb * gx - qc * gy - q3 * gz);
-	q1 += (qa * gx + qc * gz - q3 * gy);
-	q2 += (qa * gy - qb * gz + q3 * gx);
-	q3 += (qa * gz + qb * gy - qc * gx); 
-	
-	// Normalise quaternion
-	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-	q0 *= recipNorm;
-	q1 *= recipNorm;
-	q2 *= recipNorm;
-	q3 *= recipNorm;
-}
-
-//---------------------------------------------------------------------------------------------------
-// IMU algorithm update
-
-void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
-	float recipNorm;
-	float halfvx, halfvy, halfvz;
-	float halfex, halfey, halfez;
-	float qa, qb, qc;
-
-	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-		// Normalise accelerometer measurement
-		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-		ax *= recipNorm;
-		ay *= recipNorm;
-		az *= recipNorm;        
-
-		// Estimated direction of gravity and vector perpendicular to magnetic flux
-		halfvx = q1 * q3 - q0 * q2;
-		halfvy = q0 * q1 + q2 * q3;
-		halfvz = q0 * q0 - 0.5f + q3 * q3;
-	
-		// Error is sum of cross product between estimated and measured direction of gravity
-		halfex = (ay * halfvz - az * halfvy);
-		halfey = (az * halfvx - ax * halfvz);
-		halfez = (ax * halfvy - ay * halfvx);
-
-		// Compute and apply integral feedback if enabled
-		if(twoKi > 0.0f) {
-			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
-			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
-			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
-			gx += integralFBx;	// apply integral feedback
-			gy += integralFBy;
-			gz += integralFBz;
-		}
-		else {
-			integralFBx = 0.0f;	// prevent integral windup
-			integralFBy = 0.0f;
-			integralFBz = 0.0f;
-		}
-
-		// Apply proportional feedback
-		gx += twoKp * halfex;
-		gy += twoKp * halfey;
-		gz += twoKp * halfez;
-	}
-	
-	// Integrate rate of change of quaternion
-	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
-	gy *= (0.5f * (1.0f / sampleFreq));
-	gz *= (0.5f * (1.0f / sampleFreq));
-	qa = q0;
-	qb = q1;
-	qc = q2;
-	q0 += (-qb * gx - qc * gy - q3 * gz);
-	q1 += (qa * gx + qc * gz - q3 * gy);
-	q2 += (qa * gy - qb * gz + q3 * gx);
-	q3 += (qa * gz + qb * gy - qc * gx); 
-	
-	// Normalise quaternion
-	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-	q0 *= recipNorm;
-	q1 *= recipNorm;
-	q2 *= recipNorm;
-	q3 *= recipNorm;
-}
-
-//---------------------------------------------------------------------------------------------------
-// Fast inverse square-root
-// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-
-float invSqrt(float x) {
-	float halfx = 0.5f * x;
-	float y = x;
-	long i = *(long*)&y;
-	i = 0x5f3759df - (i>>1);
-	y = *(float*)&i;
-	y = y * (1.5f - (halfx * y * y));
-	return y;
-}
-
-//====================================================================================================
-// END OF CODE
-//====================================================================================================
-
-//=====================================================================================================
-// End of file
-//=====================================================================================================
-

--- a/main.cpp	Thu Jul 07 04:35:04 2016 +0000
+++ b/main.cpp	Thu Jul 07 06:04:05 2016 +0000
@@ -10,7 +10,7 @@
 #include "mbed.h"
 #include "MPU9250.h"
 #include "MadgwickAHRS.h"
-#include "MahonyAHRS.h"
+//#include "MahonyAHRS.h"
 //#define DEBUG_putc  //Wada
 
 /* MPU9250 Library
@@ -47,6 +47,10 @@
 float putg[2][3] = {0};
 float putq[4] = {0};
 int waittime = 80;
+int count = 0;
+float calibg[2][3] = {0};
+float caliba[2][3] = {0};
+
 
 void init(void)
 {
@@ -96,6 +100,24 @@
 }//float2byte
 
 
+void calibFunc(void)
+{
+     for(int i=0; i<2; i++) {
+        imu[0]->deselect();
+        imu[1]->deselect();
+        imu[i]->select();
+        imu[i]->read_all();
+        calibg[i][0] = (calibg[i][0] + imu[i]->gyroscope_data[0])/2;
+        calibg[i][1] = (calibg[i][1] + imu[i]->gyroscope_data[1])/2;
+        calibg[i][2] = (calibg[i][2] + imu[i]->gyroscope_data[2])/2;
+        //caliba[i][0] = (caliba[i][0] + imu[i]->accelerometer_data[0])/2;
+        //caliba[i][1] = (caliba[i][1] + imu[i]->accelerometer_data[1])/2;
+        //caliba[i][2] = (caliba[i][2] + imu[i]->accelerometer_data[2])/2;
+    }
+    printf("calib = %f \n",calibg[1][0]);
+}
+    
+
 void eventFunc(void)
 {    
     for(int i=0; i<2; i++) {
@@ -113,17 +135,17 @@
        imu[i]->Magnetometer[1] = 0.0f;  //use updateIMU
        imu[i]->Magnetometer[2] = 0.0f;  //use updateIMU
        
-       putg[i][0] = imu[i]->gyroscope_data[0];
-       putg[i][1] = imu[i]->gyroscope_data[1];
-       putg[i][2] = imu[i]->gyroscope_data[2];
+       putg[i][0] = imu[i]->gyroscope_data[0] - calibg[i][0];
+       putg[i][1] = imu[i]->gyroscope_data[1] - calibg[i][1];
+       putg[i][2] = imu[i]->gyroscope_data[2] - calibg[i][2];
        
        ahrs[i]->update(
         putg[i][0]*DEGREE2RAD,
         putg[i][1]*DEGREE2RAD,
         putg[i][2]*DEGREE2RAD,
-        imu[i]->accelerometer_data[0],
-        imu[i]->accelerometer_data[1],
-        imu[i]->accelerometer_data[2],
+        imu[i]->accelerometer_data[0] - caliba[i][0],
+        imu[i]->accelerometer_data[1] - caliba[i][1],
+        imu[i]->accelerometer_data[2] - caliba[i][2],
         imu[i]->Magnetometer[0],
         imu[i]->Magnetometer[1],
         imu[i]->Magnetometer[2]
@@ -134,62 +156,79 @@
     pc.putc(0x34);  //STX
     #endif
     wait_us(waittime);
-    for(int i=0; i<2; i++) {
-            putq[0] = ahrs[i]->q0;
-            putq[1] = ahrs[i]->q1;
-            putq[2] = ahrs[i]->q2;
-            putq[3] = ahrs[i]->q3;         
-            
-            if(i==0){            
-            printf("%+5.0f %+5.0f %+5.0f\t",
-                putg[i][0],
-                putg[i][1],
-                putg[i][2]);
-            printf("%+0.3f %+0.3f %+0.3f\t", 
-                imu[i]->accelerometer_data[0],
-                imu[i]->accelerometer_data[1],
-                imu[i]->accelerometer_data[2]);
-            printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f\t",
-                putq[0],
-                putq[1],
-                putq[2],
-                putq[3]);
-            printf("%0.3f\n",(putq[0]*putq[0])+(putq[1]*putq[1])+(putq[2]*putq[2])+(putq[3]*putq[3]));
-            }            
-            
-            float2byte(putg[i][0],16.4);
-            float2byte(putg[i][1],16.4);
-            float2byte(putg[i][2],16.4);
-            float2byte(putq[0],32800);
-            float2byte(putq[1],32800);
-            float2byte(putq[2],32800);
-            float2byte(putq[3],32800);      
-            
-            /*
-            //test signal 29,June wada
-            float2byte(-200,32.8);
-            float2byte(   0,32.8);
-            float2byte( 200,32.8);
-            float2byte(-0.2,32800);
-            float2byte(   0,32800);
-            float2byte( 0.4,32800);
-            float2byte( 0.8,32800);
-            */
-                           
-    }
+    if(count==4){   //sample500Hzに対してprintf100Hz
+        for(int i=0; i<2; i++) {
+                putq[0] = ahrs[i]->q0;
+                putq[1] = ahrs[i]->q1;
+                putq[2] = ahrs[i]->q2;
+                putq[3] = ahrs[i]->q3;         
+                
+                if(i==1){            
+                    printf("%+5.0f %+5.0f %+5.0f\t",
+                        putg[i][0],
+                        putg[i][1],
+                        putg[i][2]);
+                    printf("%+0.3f %+0.3f %+0.3f\t", 
+                        imu[i]->accelerometer_data[0] - caliba[i][0],
+                        imu[i]->accelerometer_data[1] - caliba[i][1],
+                        imu[i]->accelerometer_data[2] - caliba[i][2]);
+                    printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f\t",
+                        putq[0],
+                        putq[1],
+                        putq[2],
+                        putq[3]);
+                    printf("%0.3f\n",(putq[0]*putq[0])+(putq[1]*putq[1])+(putq[2]*putq[2])+(putq[3]*putq[3]));
+                }            
+                  
+                float2byte(putg[i][0],16.4);
+                float2byte(putg[i][1],16.4);
+                float2byte(putg[i][2],16.4);
+                float2byte(putq[0],32800);
+                float2byte(putq[1],32800);
+                float2byte(putq[2],32800);
+                float2byte(putq[3],32800);      
+                
+                /*
+                //test signal 29,June wada
+                float2byte(-200,32.8);
+                float2byte(   0,32.8);
+                float2byte( 200,32.8);
+                float2byte(-0.2,32800);
+                float2byte(   0,32800);
+                float2byte( 0.4,32800);
+                float2byte( 0.8,32800);
+                */       
+        }
     #ifdef DEBUG_putc
     pc.putc(0x12);  //ETX
-    #endif    
+    #endif   
+    
+    count = 0;
+    }//count  
+    count++;
 }
 
 int main()
 {
     // make instances and check sensors
     init();
+    
+    ticker.attach_us(calibFunc, 1000000.0f/sampleFreq);
 
+    for(int i=0; i<1000000; i++){
+        if(pc.readable())
+            if(pc.getc() == 'r') {
+                ticker.detach();
+                // write something event here
+                ticker.attach_us(calibFunc, 1000000.0f/sampleFreq);
+            }
+    }
+
+
+    ticker.detach();
     // define callback event
     ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
-
+        
     while(1) {
         if(pc.readable())
             if(pc.getc() == 'r') {