Skip to content

Commit 77f8916

Browse files
Brent WilkinsBrent Wilkins
Brent Wilkins
authored and
Brent Wilkins
committed
Added magnetometer calibration routine and cleaned up some things
1 parent bc4416f commit 77f8916

File tree

3 files changed

+589
-347
lines changed

3 files changed

+589
-347
lines changed

‎examples/MPU9250BasicAHRS/MPU9250BasicAHRS.ino‎

Lines changed: 82 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -97,7 +97,7 @@ void setup()
9797
delay(1000);
9898
#endif // LCD
9999

100-
if (c == 0x71) // WHO_AM_I should always be 0x68
100+
if (c == 0x71) // WHO_AM_I should always be 0x71
101101
{
102102
Serial.println("MPU9250 is online...");
103103

@@ -147,8 +147,11 @@ void setup()
147147
// Read the WHO_AM_I register of the magnetometer, this is a good test of
148148
// communication
149149
byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
150-
Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX);
151-
Serial.print(" I should be "); Serial.println(0x48, HEX);
150+
Serial.print("AK8963 ");
151+
Serial.print("I AM ");
152+
Serial.print(d, HEX);
153+
Serial.print(" I should be ");
154+
Serial.println(0x48, HEX);
152155

153156
#ifdef LCD
154157
display.clearDisplay();
@@ -165,43 +168,54 @@ void setup()
165168
myIMU.initAK8963(myIMU.factoryMagCalibration);
166169
// Initialize device for active mode read of magnetometer
167170
Serial.println("AK8963 initialized for active data mode....");
171+
168172
if (SerialDebug)
169173
{
170174
// Serial.println("Calibration values: ");
171-
Serial.print("X-Axis sensitivity adjustment value ");
175+
Serial.print("X-Axis factory sensitivity adjustment value ");
172176
Serial.println(myIMU.factoryMagCalibration[0], 2);
173-
Serial.print("Y-Axis sensitivity adjustment value ");
177+
Serial.print("Y-Axis factory sensitivity adjustment value ");
174178
Serial.println(myIMU.factoryMagCalibration[1], 2);
175-
Serial.print("Z-Axis sensitivity adjustment value ");
179+
Serial.print("Z-Axis factory sensitivity adjustment value ");
176180
Serial.println(myIMU.factoryMagCalibration[2], 2);
177181
}
178182

179183
#ifdef LCD
180184
display.clearDisplay();
181-
display.setCursor(20,0); display.print("AK8963");
182-
display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10);
183-
display.print(myIMU.factoryMagCalibration[0], 2);
184-
display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20);
185-
display.print(myIMU.factoryMagCalibration[1], 2);
186-
display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30);
187-
display.print(myIMU.factoryMagCalibration[2], 2);
185+
display.setCursor(20,0); display.print("AK8963");
186+
display.setCursor(0,10); display.print("ASAX ");
187+
display.setCursor(50,10); display.print(myIMU.factoryMagCalibration[0], 2);
188+
display.setCursor(0,20); display.print("ASAY ");
189+
display.setCursor(50,20); display.print(myIMU.factoryMagCalibration[1], 2);
190+
display.setCursor(0,30); display.print("ASAZ ");
191+
display.setCursor(50,30); display.print(myIMU.factoryMagCalibration[2], 2);
188192
display.display();
189193
delay(1000);
190194
#endif // LCD
191195

196+
// Get sensor resolutions, only need to do this once
197+
myIMU.getAres();
198+
myIMU.getGres();
199+
myIMU.getMres();
200+
201+
// The next call delays for 4 seconds, and then records about 15 seconds of
202+
// data to calculate bias and scale.
192203
myIMU.magCalMPU9250(myIMU.magBias, myIMU.magScale);
193-
Serial.println("AK8963 mag biases (mG)"); Serial.println(myIMU.magBias[0]);
194-
Serial.println(myIMU.magBias[1]); Serial.println(myIMU.magBias[2]);
195-
Serial.println("AK8963 mag scale (mG)"); Serial.println(myIMU.magScale[0]);
196-
Serial.println(myIMU.magScale[1]); Serial.println(myIMU.magScale[2]);
204+
Serial.println("AK8963 mag biases (mG)");
205+
Serial.println(myIMU.magBias[0]);
206+
Serial.println(myIMU.magBias[1]);
207+
Serial.println(myIMU.magBias[2]);
208+
209+
Serial.println("AK8963 mag scale (mG)");
210+
Serial.println(myIMU.magScale[0]);
211+
Serial.println(myIMU.magScale[1]);
212+
Serial.println(myIMU.magScale[2]);
197213
delay(2000); // Add delay to see results before serial spew of data
198-
214+
199215
if(SerialDebug)
200216
{
201-
// Serial.println("Calibration values: ");
202217
Serial.println("Magnetometer:");
203218
Serial.print("X-Axis sensitivity adjustment value ");
204-
// TODO: change this from factory to calculated values
205219
Serial.println(myIMU.factoryMagCalibration[0], 2);
206220
Serial.print("Y-Axis sensitivity adjustment value ");
207221
Serial.println(myIMU.factoryMagCalibration[1], 2);
@@ -235,49 +249,35 @@ void loop()
235249
// If intPin goes high, all data registers have new data
236250
// On interrupt, check if data ready interrupt
237251
if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
238-
{
252+
{
239253
myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values
240-
myIMU.getAres();
241254

242255
// Now we'll calculate the accleration value into actual g's
243256
// This depends on scale being set
244-
myIMU.ax = (float)myIMU.accelCount[0]*myIMU.aRes; // - myIMU.accelBias[0];
245-
myIMU.ay = (float)myIMU.accelCount[1]*myIMU.aRes; // - myIMU.accelBias[1];
246-
myIMU.az = (float)myIMU.accelCount[2]*myIMU.aRes; // - myIMU.accelBias[2];
257+
myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - myIMU.accelBias[0];
258+
myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1];
259+
myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2];
247260

248261
myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values
249-
myIMU.getGres();
250262

251263
// Calculate the gyro value into actual degrees per second
252264
// This depends on scale being set
253-
myIMU.gx = (float)myIMU.gyroCount[0]*myIMU.gRes;
254-
myIMU.gy = (float)myIMU.gyroCount[1]*myIMU.gRes;
255-
myIMU.gz = (float)myIMU.gyroCount[2]*myIMU.gRes;
265+
myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
266+
myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
267+
myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
256268

257269
myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values
258-
myIMU.getMres();
259-
// TODO: This needs to be fixed. No need to hard code it anymore.
260-
// User environmental x-axis correction in milliGauss, should be
261-
// automatically calculated
262-
myIMU.magBias[0] = +470.;
263-
// User environmental x-axis correction in milliGauss TODO axis??
264-
myIMU.magBias[1] = +120.;
265-
// User environmental x-axis correction in milliGauss
266-
myIMU.magBias[2] = +125.;
267270

268271
// Calculate the magnetometer values in milliGauss
269272
// Include factory calibration per data sheet and user environmental
270273
// corrections
271274
// Get actual magnetometer value, this depends on scale being set
272-
myIMU.mx =
273-
(float)myIMU.magCount[0] * myIMU.mRes*myIMU.factoryMagCalibration[0] -
274-
myIMU.magBias[0];
275-
myIMU.my =
276-
(float)myIMU.magCount[1] * myIMU.mRes*myIMU.factoryMagCalibration[1] -
277-
myIMU.magBias[1];
278-
myIMU.mz =
279-
(float)myIMU.magCount[2] * myIMU.mRes*myIMU.factoryMagCalibration[2] -
280-
myIMU.magBias[2];
275+
myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes
276+
* myIMU.factoryMagCalibration[0] - myIMU.magBias[0];
277+
myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes
278+
* myIMU.factoryMagCalibration[1] - myIMU.magBias[1];
279+
myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes
280+
* myIMU.factoryMagCalibration[2] - myIMU.magBias[2];
281281
} // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
282282

283283
// Must be called before updating quaternions!
@@ -291,9 +291,8 @@ void loop()
291291
// along the x-axis just like in the LSM9DS0 sensor. This rotation can be
292292
// modified to allow any convenient orientation convention. This is ok by
293293
// aircraft orientation standards! Pass gyro rate as rad/s
294-
// MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
295-
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx*DEG_TO_RAD,
296-
myIMU.gy*DEG_TO_RAD, myIMU.gz*DEG_TO_RAD, myIMU.my,
294+
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
295+
myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
297296
myIMU.mx, myIMU.mz, myIMU.deltat);
298297

299298
if (!AHRS)
@@ -304,11 +303,11 @@ void loop()
304303
if(SerialDebug)
305304
{
306305
// Print acceleration values in milligs!
307-
Serial.print("X-acceleration: "); Serial.print(1000*myIMU.ax);
306+
Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax);
308307
Serial.print(" mg ");
309-
Serial.print("Y-acceleration: "); Serial.print(1000*myIMU.ay);
308+
Serial.print("Y-acceleration: "); Serial.print(1000 * myIMU.ay);
310309
Serial.print(" mg ");
311-
Serial.print("Z-acceleration: "); Serial.print(1000*myIMU.az);
310+
Serial.print("Z-acceleration: "); Serial.print(1000 * myIMU.az);
312311
Serial.println(" mg ");
313312

314313
// Print gyro values in degree/sec
@@ -340,9 +339,9 @@ void loop()
340339
display.setCursor(0, 0); display.print("MPU9250/AK8963");
341340
display.setCursor(0, 8); display.print(" x y z ");
342341

343-
display.setCursor(0, 16); display.print((int)(1000*myIMU.ax));
344-
display.setCursor(24, 16); display.print((int)(1000*myIMU.ay));
345-
display.setCursor(48, 16); display.print((int)(1000*myIMU.az));
342+
display.setCursor(0, 16); display.print((int)(1000 * myIMU.ax));
343+
display.setCursor(24, 16); display.print((int)(1000 * myIMU.ay));
344+
display.setCursor(48, 16); display.print((int)(1000 * myIMU.az));
346345
display.setCursor(72, 16); display.print("mg");
347346

348347
display.setCursor(0, 24); display.print((int)(myIMU.gx));
@@ -375,22 +374,22 @@ void loop()
375374
{
376375
if(SerialDebug)
377376
{
378-
Serial.print("ax = "); Serial.print((int)1000*myIMU.ax);
379-
Serial.print(" ay = "); Serial.print((int)1000*myIMU.ay);
380-
Serial.print(" az = "); Serial.print((int)1000*myIMU.az);
377+
Serial.print("ax = "); Serial.print((int)1000 * myIMU.ax);
378+
Serial.print(" ay = "); Serial.print((int)1000 * myIMU.ay);
379+
Serial.print(" az = "); Serial.print((int)1000 * myIMU.az);
381380
Serial.println(" mg");
382381

383-
Serial.print("gx = "); Serial.print( myIMU.gx, 2);
384-
Serial.print(" gy = "); Serial.print( myIMU.gy, 2);
385-
Serial.print(" gz = "); Serial.print( myIMU.gz, 2);
382+
Serial.print("gx = "); Serial.print(myIMU.gx, 2);
383+
Serial.print(" gy = "); Serial.print(myIMU.gy, 2);
384+
Serial.print(" gz = "); Serial.print(myIMU.gz, 2);
386385
Serial.println(" deg/s");
387386

388-
Serial.print("mx = "); Serial.print( (int)myIMU.mx );
389-
Serial.print(" my = "); Serial.print( (int)myIMU.my );
390-
Serial.print(" mz = "); Serial.print( (int)myIMU.mz );
387+
Serial.print("mx = "); Serial.print((int)myIMU.mx);
388+
Serial.print(" my = "); Serial.print((int)myIMU.my);
389+
Serial.print(" mz = "); Serial.print((int)myIMU.mz);
391390
Serial.println(" mG");
392391

393-
Serial.print("q0 = "); Serial.print(*getQ());
392+
Serial.print("q0 = "); Serial.print(*getQ());
394393
Serial.print(" qx = "); Serial.print(*(getQ() + 1));
395394
Serial.print(" qy = "); Serial.print(*(getQ() + 2));
396395
Serial.print(" qz = "); Serial.println(*(getQ() + 3));
@@ -412,21 +411,24 @@ void loop()
412411
// For more see
413412
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
414413
// which has additional links.
415-
myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() *
416-
*(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1)
417-
- *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3));
418-
myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() *
419-
*(getQ()+2)));
420-
myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) *
421-
*(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1)
422-
- *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3));
414+
myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ()
415+
* *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1)
416+
* *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3)
417+
* *(getQ()+3));
418+
myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ()
419+
* *(getQ()+2)));
420+
myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2)
421+
* *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1)
422+
* *(getQ()+1) - *(getQ()+2) * *(getQ()+2) + *(getQ()+3)
423+
* *(getQ()+3));
423424
myIMU.pitch *= RAD_TO_DEG;
424425
myIMU.yaw *= RAD_TO_DEG;
426+
425427
// Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
426428
// 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19
427429
// - http://www.ngdc.noaa.gov/geomag-web/#declination
428-
myIMU.yaw -= 8.5;
429-
myIMU.roll *= RAD_TO_DEG;
430+
myIMU.yaw -= 8.5;
431+
myIMU.roll *= RAD_TO_DEG;
430432

431433
if(SerialDebug)
432434
{
@@ -438,7 +440,7 @@ void loop()
438440
Serial.println(myIMU.roll, 2);
439441

440442
Serial.print("rate = ");
441-
Serial.print((float)myIMU.sumCount/myIMU.sum, 2);
443+
Serial.print((float)myIMU.sumCount / myIMU.sum, 2);
442444
Serial.println(" Hz");
443445
}
444446

@@ -447,9 +449,9 @@ void loop()
447449

448450
display.setCursor(0, 0); display.print(" x y z ");
449451

450-
display.setCursor(0, 8); display.print((int)(1000*myIMU.ax));
451-
display.setCursor(24, 8); display.print((int)(1000*myIMU.ay));
452-
display.setCursor(48, 8); display.print((int)(1000*myIMU.az));
452+
display.setCursor(0, 8); display.print((int)(1000 * myIMU.ax));
453+
display.setCursor(24, 8); display.print((int)(1000 * myIMU.ay));
454+
display.setCursor(48, 8); display.print((int)(1000 * myIMU.az));
453455
display.setCursor(72, 8); display.print("mg");
454456

455457
display.setCursor(0, 16); display.print((int)(myIMU.gx));

0 commit comments

Comments
 (0)