A powerful 9 axis sensor, MPU9250, is placed on the BPI:bit board.
- The 9 axis here is different from the 9 axis of space understanding, and is actually the combination of 3 separate 3 axis sensors (accelerometers - Accelerator, gyroscope - Gyroscope, magnetometer - Magnetometer). For the detailed introduction of this chip, you can visit GitHub to see the document.github url
Code:
#include "quaternionFilters.h"
#include "MPU9250.h"
#ifdef LCD
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>
// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 9 - Serial clock out (SCLK)
// pin 8 - Serial data out (DIN)
// pin 7 - Data/Command select (D/C)
// pin 5 - LCD chip select (CS)
// pin 6 - LCD reset (RST)
Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
#endif // LCD
#define AHRS true // Set to false for basic data read
#define SerialDebug true // Set to true to get Serial output for debugging
// Pin definitions
int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins
int myLed = 13; // Set up pin 13 led for toggling
MPU9250 myIMU;
void setup()
{
// TWBR = 12; // 400 kbit/sec I2C speed
Serial.begin(115200);
// Set up the interrupt pin, its set as active high, push-pull
pinMode(intPin, INPUT);
digitalWrite(intPin, LOW);
pinMode(myLed, OUTPUT);
digitalWrite(myLed, HIGH);
#ifdef LCD
display.begin(); // Ini8ialize the display
display.setContrast(58); // Set the contrast
// Start device display with ID of sensor
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0, 0);
display.print("MPU9250");
display.setTextSize(1);
display.setCursor(0, 20);
display.print("9-DOF 16-bit");
display.setCursor(0, 30);
display.print("motion sensor");
display.setCursor(20, 40);
display.print("60 ug LSB");
display.display();
delay(1000);
// Set up for data display
display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
display.clearDisplay(); // clears the screen and buffer
#endif // LCD
Wire.begin(21, 22, 100000), delay(500);
// Read the WHO_AM_I register, this is a good test of communication
byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
Serial.print("MPU9250 ");
Serial.print("I AM ");
Serial.print(c, HEX);
Serial.print(" I should be ");
Serial.println(0x71, HEX);
#ifdef LCD
display.setCursor(20, 0);
display.print("MPU9250");
display.setCursor(0, 10);
display.print("I AM");
display.setCursor(0, 20);
display.print(c, HEX);
display.setCursor(0, 30);
display.print("I Should Be");
display.setCursor(0, 40);
display.print(0x71, HEX);
display.display();
delay(1000);
#endif // LCD
if (c == 0x71) // WHO_AM_I should always be 0x68
{
Serial.println("MPU9250 is online...");
// Start by performing self test and reporting values
myIMU.MPU9250SelfTest(myIMU.SelfTest);
Serial.print("x-axis self test: acceleration trim within : ");
Serial.print(myIMU.SelfTest[0], 1);
Serial.println("% of factory value");
Serial.print("y-axis self test: acceleration trim within : ");
Serial.print(myIMU.SelfTest[1], 1);
Serial.println("% of factory value");
Serial.print("z-axis self test: acceleration trim within : ");
Serial.print(myIMU.SelfTest[2], 1);
Serial.println("% of factory value");
Serial.print("x-axis self test: gyration trim within : ");
Serial.print(myIMU.SelfTest[3], 1);
Serial.println("% of factory value");
Serial.print("y-axis self test: gyration trim within : ");
Serial.print(myIMU.SelfTest[4], 1);
Serial.println("% of factory value");
Serial.print("z-axis self test: gyration trim within : ");
Serial.print(myIMU.SelfTest[5], 1);
Serial.println("% of factory value");
// Calibrate gyro and accelerometers, load biases in bias registers
myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
#ifdef LCD
display.clearDisplay();
display.setCursor(0, 0);
display.print("MPU9250 bias");
display.setCursor(0, 8);
display.print(" x y z ");
display.setCursor(0, 16);
display.print((int)(1000 * accelBias[0]));
display.setCursor(24, 16);
display.print((int)(1000 * accelBias[1]));
display.setCursor(48, 16);
display.print((int)(1000 * accelBias[2]));
display.setCursor(72, 16);
display.print("mg");
display.setCursor(0, 24);
display.print(myIMU.gyroBias[0], 1);
display.setCursor(24, 24);
display.print(myIMU.gyroBias[1], 1);
display.setCursor(48, 24);
display.print(myIMU.gyroBias[2], 1);
display.setCursor(66, 24);
display.print("o/s");
display.display();
delay(1000);
#endif // LCD
myIMU.initMPU9250();
// Initialize device for active mode read of acclerometer, gyroscope, and
// temperature
Serial.println("MPU9250 initialized for active data mode....");
// Read the WHO_AM_I register of the magnetometer, this is a good test of
// communication
byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
Serial.print("AK8963 ");
Serial.print("I AM ");
Serial.print(d, HEX);
Serial.print(" I should be ");
Serial.println(0x48, HEX);
#ifdef LCD
display.clearDisplay();
display.setCursor(20, 0);
display.print("AK8963");
display.setCursor(0, 10);
display.print("I AM");
display.setCursor(0, 20);
display.print(d, HEX);
display.setCursor(0, 30);
display.print("I Should Be");
display.setCursor(0, 40);
display.print(0x48, HEX);
display.display();
delay(1000);
#endif // LCD
// Get magnetometer calibration from AK8963 ROM
myIMU.initAK8963(myIMU.magCalibration);
// Initialize device for active mode read of magnetometer
Serial.println("AK8963 initialized for active data mode....");
if (SerialDebug)
{
// Serial.println("Calibration values: ");
Serial.print("X-Axis sensitivity adjustment value ");
Serial.println(myIMU.magCalibration[0], 2);
Serial.print("Y-Axis sensitivity adjustment value ");
Serial.println(myIMU.magCalibration[1], 2);
Serial.print("Z-Axis sensitivity adjustment value ");
Serial.println(myIMU.magCalibration[2], 2);
}
#ifdef LCD
display.clearDisplay();
display.setCursor(20, 0);
display.print("AK8963");
display.setCursor(0, 10);
display.print("ASAX ");
display.setCursor(50, 10);
display.print(myIMU.magCalibration[0], 2);
display.setCursor(0, 20);
display.print("ASAY ");
display.setCursor(50, 20);
display.print(myIMU.magCalibration[1], 2);
display.setCursor(0, 30);
display.print("ASAZ ");
display.setCursor(50, 30);
display.print(myIMU.magCalibration[2], 2);
display.display();
delay(1000);
#endif // LCD
} // if (c == 0x71)
else
{
Serial.print("Could not connect to MPU9250: 0x");
Serial.println(c, HEX);
while (1)
; // Loop forever if communication doesn't happen
}
}
void loop()
{
// If intPin goes high, all data registers have new data
// On interrupt, check if data ready interrupt
if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
{
myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values
myIMU.getAres();
// Now we'll calculate the accleration value into actual g's
// This depends on scale being set
myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - accelBias[0];
myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - accelBias[1];
myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - accelBias[2];
myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values
myIMU.getGres();
// Calculate the gyro value into actual degrees per second
// This depends on scale being set
myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values
myIMU.getMres();
// User environmental x-axis correction in milliGauss, should be
// automatically calculated
myIMU.magbias[0] = +470.;
// User environmental x-axis correction in milliGauss TODO axis??
myIMU.magbias[1] = +120.;
// User environmental x-axis correction in milliGauss
myIMU.magbias[2] = +125.;
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental
// corrections
// Get actual magnetometer value, this depends on scale being set
myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes * myIMU.magCalibration[0] -
myIMU.magbias[0];
myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes * myIMU.magCalibration[1] -
myIMU.magbias[1];
myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes * myIMU.magCalibration[2] -
myIMU.magbias[2];
} // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
// Must be called before updating quaternions!
myIMU.updateTime();
// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of
// the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis
// (+ up) of accelerometer and gyro! We have to make some allowance for this
// orientationmismatch in feeding the output to the quaternion filter. For the
// MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward
// along the x-axis just like in the LSM9DS0 sensor. This rotation can be
// modified to allow any convenient orientation convention. This is ok by
// aircraft orientation standards! Pass gyro rate as rad/s
// MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
myIMU.mx, myIMU.mz, myIMU.deltat);
if (!AHRS)
{
myIMU.delt_t = millis() - myIMU.count;
if (myIMU.delt_t > 500)
{
if (SerialDebug)
{
// Print acceleration values in milligs!
Serial.print("X-acceleration: ");
Serial.print(1000 * myIMU.ax);
Serial.println(" mg ");
Serial.print("Y-acceleration: ");
Serial.print(1000 * myIMU.ay);
Serial.println(" mg ");
Serial.print("Z-acceleration: ");
Serial.print(1000 * myIMU.az);
Serial.println(" mg ");
// Print gyro values in degree/sec
Serial.print("X-gyro rate: ");
Serial.print(myIMU.gx, 3);
Serial.println(" degrees/sec ");
Serial.print("Y-gyro rate: ");
Serial.print(myIMU.gy, 3);
Serial.println(" degrees/sec ");
Serial.print("Z-gyro rate: ");
Serial.print(myIMU.gz, 3);
Serial.println(" degrees/sec");
// Print mag values in degree/sec
Serial.print("X-mag field: ");
Serial.print(myIMU.mx);
Serial.println(" mG ");
Serial.print("Y-mag field: ");
Serial.print(myIMU.my);
Serial.println(" mG ");
Serial.print("Z-mag field: ");
Serial.print(myIMU.mz);
Serial.println(" mG");
myIMU.tempCount = myIMU.readTempData(); // Read the adc values
// Temperature in degrees Centigrade
myIMU.temperature = ((float)myIMU.tempCount) / 333.87 + 21.0;
// Print temperature in degrees Centigrade
Serial.print("Temperature is ");
Serial.print(myIMU.temperature, 1);
Serial.println(" degrees C");
}
#ifdef LCD
display.clearDisplay();
display.setCursor(0, 0);
display.print("MPU9250/AK8963");
display.setCursor(0, 8);
display.print(" x y z ");
display.setCursor(0, 16);
display.print((int)(1000 * myIMU.ax));
display.setCursor(24, 16);
display.print((int)(1000 * myIMU.ay));
display.setCursor(48, 16);
display.print((int)(1000 * myIMU.az));
display.setCursor(72, 16);
display.print("mg");
display.setCursor(0, 24);
display.print((int)(myIMU.gx));
display.setCursor(24, 24);
display.print((int)(myIMU.gy));
display.setCursor(48, 24);
display.print((int)(myIMU.gz));
display.setCursor(66, 24);
display.print("o/s");
display.setCursor(0, 32);
display.print((int)(myIMU.mx));
display.setCursor(24, 32);
display.print((int)(myIMU.my));
display.setCursor(48, 32);
display.print((int)(myIMU.mz));
display.setCursor(72, 32);
display.print("mG");
display.setCursor(0, 40);
display.print("Gyro T ");
display.setCursor(50, 40);
display.print(myIMU.temperature, 1);
display.print(" C");
display.display();
#endif // LCD
myIMU.count = millis();
digitalWrite(myLed, !digitalRead(myLed)); // toggle led
} // if (myIMU.delt_t > 500)
} // if (!AHRS)
else
{
// Serial print and/or display at 0.5 s rate independent of data rates
myIMU.delt_t = millis() - myIMU.count;
// update LCD once per half-second independent of read rate
if (myIMU.delt_t > 500)
{
if (SerialDebug)
{
Serial.print("ax = ");
Serial.print((int)1000 * myIMU.ax);
Serial.print(" ay = ");
Serial.print((int)1000 * myIMU.ay);
Serial.print(" az = ");
Serial.print((int)1000 * myIMU.az);
Serial.println(" mg");
Serial.print("gx = ");
Serial.print(myIMU.gx, 2);
Serial.print(" gy = ");
Serial.print(myIMU.gy, 2);
Serial.print(" gz = ");
Serial.print(myIMU.gz, 2);
Serial.println(" deg/s");
Serial.print("mx = ");
Serial.print((int)myIMU.mx);
Serial.print(" my = ");
Serial.print((int)myIMU.my);
Serial.print(" mz = ");
Serial.print((int)myIMU.mz);
Serial.println(" mG");
Serial.print("q0 = ");
Serial.print(*getQ());
Serial.print(" qx = ");
Serial.print(*(getQ() + 1));
Serial.print(" qy = ");
Serial.print(*(getQ() + 2));
Serial.print(" qz = ");
Serial.println(*(getQ() + 3));
}
// Define output variables from updated quaternion---these are Tait-Bryan
// angles, commonly used in aircraft orientation. In this coordinate system,
// the positive z-axis is down toward Earth. Yaw is the angle between Sensor
// x-axis and Earth magnetic North (or true North if corrected for local
// declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the
// Earth is positive, up toward the sky is negative. Roll is angle between
// sensor y-axis and Earth ground plane, y-axis up is positive roll. These
// arise from the definition of the homogeneous rotation matrix constructed
// from quaternions. Tait-Bryan angles as well as Euler angles are
// non-commutative; that is, the get the correct orientation the rotations
// must be applied in the correct order which for this configuration is yaw,
// pitch, and then roll.
// For more see
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// which has additional links.
myIMU.yaw = atan2(2.0f * (*(getQ() + 1) * *(getQ() + 2) + *getQ() *
*(getQ() + 3)),
*getQ() * *getQ() + *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) - *(getQ() + 3) * *(getQ() + 3));
myIMU.pitch = -asin(2.0f * (*(getQ() + 1) * *(getQ() + 3) - *getQ() *
*(getQ() + 2)));
myIMU.roll = atan2(2.0f * (*getQ() * *(getQ() + 1) + *(getQ() + 2) *
*(getQ() + 3)),
*getQ() * *getQ() - *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) + *(getQ() + 3) * *(getQ() + 3));
myIMU.pitch *= RAD_TO_DEG;
myIMU.yaw *= RAD_TO_DEG;
// Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
// 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19
// - http://www.ngdc.noaa.gov/geomag-web/#declination
myIMU.yaw -= 8.5;
myIMU.roll *= RAD_TO_DEG;
if (SerialDebug)
{
Serial.print("Yaw, Pitch, Roll: ");
Serial.print(myIMU.yaw, 2);
Serial.print(", ");
Serial.print(myIMU.pitch, 2);
Serial.print(", ");
Serial.println(myIMU.roll, 2);
Serial.print("rate = ");
Serial.print((float)myIMU.sumCount / myIMU.sum, 2);
Serial.println(" Hz");
}
#ifdef LCD
display.clearDisplay();
display.setCursor(0, 0);
display.print(" x y z ");
display.setCursor(0, 8);
display.print((int)(1000 * myIMU.ax));
display.setCursor(24, 8);
display.print((int)(1000 * myIMU.ay));
display.setCursor(48, 8);
display.print((int)(1000 * myIMU.az));
display.setCursor(72, 8);
display.print("mg");
display.setCursor(0, 16);
display.print((int)(myIMU.gx));
display.setCursor(24, 16);
display.print((int)(myIMU.gy));
display.setCursor(48, 16);
display.print((int)(myIMU.gz));
display.setCursor(66, 16);
display.print("o/s");
display.setCursor(0, 24);
display.print((int)(myIMU.mx));
display.setCursor(24, 24);
display.print((int)(myIMU.my));
display.setCursor(48, 24);
display.print((int)(myIMU.mz));
display.setCursor(72, 24);
display.print("mG");
display.setCursor(0, 32);
display.print((int)(myIMU.yaw));
display.setCursor(24, 32);
display.print((int)(myIMU.pitch));
display.setCursor(48, 32);
display.print((int)(myIMU.roll));
display.setCursor(66, 32);
display.print("ypr");
// With these settings the filter is updating at a ~145 Hz rate using the
// Madgwick scheme and >200 Hz using the Mahony scheme even though the
// display refreshes at only 2 Hz. The filter update rate is determined
// mostly by the mathematical steps in the respective algorithms, the
// processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
// an ODR of 10 Hz for the magnetometer produce the above rates, maximum
// magnetometer ODR of 100 Hz produces filter update rates of 36 - 145 and
// ~38 Hz for the Madgwick and Mahony schemes, respectively. This is
// presumably because the magnetometer read takes longer than the gyro or
// accelerometer reads. This filter update rate should be fast enough to
// maintain accurate platform orientation for stabilization control of a
// fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
// produced by the on-board Digital Motion Processor of Invensense's MPU6050
// 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty
// well!
display.setCursor(0, 40);
display.print("rt: ");
display.print((float)myIMU.sumCount / myIMU.sum, 2);
display.print(" Hz");
display.display();
#endif // LCD
myIMU.count = millis();
myIMU.sumCount = 0;
myIMU.sum = 0;
} // if (myIMU.delt_t > 500)
} // if (AHRS)
}
This code has been uploaded to GitHub and can be downloaded.Github url
The following is the result of the return of the serial port
MPU9250 datasheet: