P R
A quick and dirty demo of the Xadow M0 acceleromoeter values displayed on the Xadow OLED 0.96" (using the SSD1308 128x64 OLED Driver with I2C interface library).
Dependencies: mbed SSD1308_128x64_I2C_opt XadowGPS BMP180 ADXL345_I2C MPU9250 USBDevice
Revision 9:310663c014d8, committed 2019-03-01
- Comitter:
- ruevs
- Date:
- Fri Mar 01 09:48:20 2019 +0000
- Parent:
- 8:4e8991196bb8
- Commit message:
- Implement online magnetometer calibration - offset and scale.
Changed in this revision
| MPU9250.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/MPU9250.lib Sat Feb 23 18:07:47 2019 +0000 +++ b/MPU9250.lib Fri Mar 01 09:48:20 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/ruevs/code/MPU9250/#2315cd1878a1 +https://os.mbed.com/users/ruevs/code/MPU9250/#fc94c7336b7c
--- 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