Diff: main.cpp

Revision:

9:310663c014d8

Parent:

8:4e8991196bb8

--- a/main.cpp	Sat Feb 23 18:07:47 2019 +0000
+++ b/main.cpp	Fri Mar 01 09:48:20 2019 +0000
@@ -138,9 +138,11 @@
     LOG("Accelerometer sensitivity is %f LSB/g \r\n", 1.0f/aRes);
     LOG("Gyroscope sensitivity is %f LSB/deg/s \r\n", 1.0f/gRes);
     LOG("Magnetometer sensitivity is %f LSB/G \r\n", 1.0f/mRes);
+/*
     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+ */
     
     while(1) {
       
@@ -176,9 +178,29 @@
                 LOG("I2C error ocurred while reading magnetometer data. I2Cstate = %d \r\n", I2Cstate);
             else{ // I2C read or write ok
                 I2Cstate = 1;
-                mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
-                my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
-                mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+                /* Online magnetometer calibration */
+                float magAverageXYZRange = 0;
+                for(int i=0;i<3;++i) {
+                    if (magCount[i]<minMagCount[i]) {
+                        minMagCount[i] = magCount[i];
+                        magbias[i] = (minMagCount[i]+maxMagCount[i])*mRes*magCalibration[1]/2.;
+                    }
+                    if (magCount[i]>maxMagCount[i]) {
+                        maxMagCount[i] = magCount[i];
+                        magbias[i] = (minMagCount[i]+maxMagCount[i])*mRes*magCalibration[1]/2.;
+                    }
+                    magAverageXYZRange += maxMagCount[i]-minMagCount[i];
+                }
+
+                magAverageXYZRange /= 3.;
+
+                for(int i=0;i<3;++i) {
+                    magScale[i] = magAverageXYZRange / (maxMagCount[i]-minMagCount[i]);
+                }
+
+                mx = (float)magCount[0]*mRes*magCalibration[0]*magScale[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+                my = (float)magCount[1]*mRes*magCalibration[1]*magScale[1] - magbias[1];  
+                mz = (float)magCount[2]*mRes*magCalibration[2]*magScale[2] - magbias[2];
             }
                        
             mpu9250.getCompassOrientation(orientation);
@@ -203,6 +225,7 @@
         // mpu9250.MadgwickQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f,  mx,  my, mz);
         mpu9250.MahonyQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f, mx, my, mz);
 
+        white_led= !white_led;
 
         // Serial print and/or display at 1.5 s rate independent of data rates
         delt_t = t.read_ms() - count;
@@ -217,6 +240,18 @@
             LOG("mx = %f", mx); 
             LOG(" my = %f", my); 
             LOG(" mz = %f  mG\r\n", mz);
+            LOG("minmx = %i", minMagCount[0]); 
+            LOG(" minmy = %i", minMagCount[1]); 
+            LOG(" minmz = %i\r\n", minMagCount[2]);
+            LOG("maxmx = %i", maxMagCount[0]); 
+            LOG(" maxmy = %i", maxMagCount[1]); 
+            LOG(" maxmz = %i\r\n", maxMagCount[2]);
+            LOG("magbiasx = %f", magbias[0]); 
+            LOG(" magbiasy = %f", magbias[1]); 
+            LOG(" magbiasz = %f mG\r\n", magbias[2]);
+            LOG("magscalex = %f", magScale[0]); 
+            LOG(" magscaley = %f", magScale[1]); 
+            LOG(" magscalez = %f mG\r\n", magScale[2]);
             
 
             tempCount = mpu9250.readTempData();  // Read the adc values