零知开源模块使用-MPU9250九轴传感器
本次将演示九轴传感器模块-MPU9250获取加速度、陀螺仪、地磁数据。
一、接线
本次使用I2C接口，因此直接把模块的I2C与零知板的I2C引脚对应相连接即可，供电选择3.3v，其他默认。
二、代码
/* MPU9250 DEMO
POWERED BY lingzhilab.com
*/
#include "quaternionFilters.h"
#include "MPU9250.h"
#define AHRS true
#define SerialDebug true
int intPin = 12;
int myLed = LED_BUILTIN;
MPU9250 myIMU;
void setup()
{
Wire.begin();
Serial.begin(9600);
pinMode(intPin, INPUT);
digitalWrite(intPin, LOW);
pinMode(myLed, OUTPUT);
digitalWrite(myLed, HIGH);
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);
if (c == 0x71)
{
Serial.println("MPU9250 is online...");
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");
myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
myIMU.initMPU9250();
Serial.println("MPU9250 initialized for active data mode....");
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);
myIMU.initAK8963(myIMU.magCalibration);
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);
}
} // if (c == 0x71)
else
{
Serial.print("Could not connect to MPU9250: 0x");
Serial.println(c, HEX);
while(1) ;
}
}
void loop()
{
if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
{
myIMU.readAccelData(myIMU.accelCount);
myIMU.getAres();
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);
myIMU.getGres();
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);
myIMU.getMres();
myIMU.magbias[0] = +470.;
myIMU.magbias[1] = +120.;
myIMU.magbias[2] = +125.;
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)
myIMU.updateTime();
// 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)
{
Serial.print("X-acceleration: "); Serial.print(1000*myIMU.ax);
Serial.print(" mg ");
Serial.print("Y-acceleration: "); Serial.print(1000*myIMU.ay);
Serial.print(" mg ");
Serial.print("Z-acceleration: "); Serial.print(1000*myIMU.az);
Serial.println(" mg ");
Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3);
Serial.print(" degrees/sec ");
Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3);
Serial.print(" degrees/sec ");
Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3);
Serial.println(" degrees/sec");
Serial.print("X-mag field: "); Serial.print(myIMU.mx);
Serial.print(" mG ");
Serial.print("Y-mag field: "); Serial.print(myIMU.my);
Serial.print(" mG ");
Serial.print("Z-mag field: "); Serial.print(myIMU.mz);
Serial.println(" mG");
myIMU.tempCount = myIMU.readTempData();
myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0;
Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1);
Serial.println(" degrees C");
}
myIMU.count = millis();
digitalWrite(myLed, !digitalRead(myLed));
} // if (myIMU.delt_t > 500)
} // if (!AHRS)
else
{
myIMU.delt_t = millis() - myIMU.count;
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));
}
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;
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");
}
myIMU.count = millis();
myIMU.sumCount = 0;
myIMU.sum = 0;
} // if (myIMU.delt_t > 500)
} // if (AHRS)
}
代码上传到零知板，即可看到九轴的数据如下所示：
三、完整工程参考
MPU9250-DEMO.zip(点击下载)