我正在尝试使用此i2cdevlib库通过I2C从Raspberry Pi 3上的MPU-9150 9DOF IMU上读取数字运动处理器。我正在使用以下C ++代码：
using namespace std;
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include <signal.h>
#include "I2Cdev.h"
#include "MPU9150_9Axis_MotionApps41.h"
MPU9150 mpu;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
int sample_rate;
string frame_id;
// mpu offsets
int ax, ay, az, gx, gy, gz;
bool debug = false;
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
bool ado = false;
double angular_velocity_covariance, pitch_roll_covariance, yaw_covariance, linear_acceleration_covariance;
double linear_acceleration_stdev_, angular_velocity_stdev_, yaw_stdev_, pitch_roll_stdev_;
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// http://www.i2cdevlib.com/forums/topic/4-understanding-raw-values-of-accelerometer-and-gyrometer/
//The output scale for any setting is [-32768, +32767] for each of the six axes.
//The default setting in the I2Cdevlib class is +/- 2g for the accel and +/- 250 deg/sec
//for the gyro. If the device is perfectly level and not moving, then:
//
// X/Y accel axes should read 0
// Z accel axis should read 1g, which is +16384 at a sensitivity of 2g
// X/Y/Z gyro axes should read 0
//
//In reality, the accel axes won't read exactly 0 since it is difficult to be perfectly level
//and there is some noise/error, and the gyros will also not read exactly 0 for the same
//reason (noise/error).
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (fifoCount >= 42) {
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
printf("quat %7.2f %7.2f %7.2f %7.2f ", q.w,q.x,q.y,q.z);
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
printf("ypr (degrees) %7.2f %7.2f %7.2f ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
// display real acceleration, adjusted to remove gravity
// https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU9150/MPU9150_6Axis_MotionApps20.h
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
// By default, accel is in arbitrary units with a scale of 16384/1g.
// Per http://www.ros.org/reps/rep-0103.html
// and http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
// should be in m/s^2.
printf("areal (raw) %6d %6d %6d ", aaReal.x, aaReal.y, aaReal.z);
printf("areal (m/s^2) %6d %6d %6d ", aaReal.x * 1/16384. * 9.80665, imu_msg.linear_acceleration.y, aaReal.z * 1/16384. * 9.80665);
printf("\n");
}
}
int main(int argc, char **argv){
angular_velocity_covariance = angular_velocity_stdev_ * angular_velocity_stdev_;
linear_acceleration_covariance = linear_acceleration_stdev_ * linear_acceleration_stdev_;
pitch_roll_covariance = pitch_roll_stdev_ * pitch_roll_stdev_;
yaw_covariance = yaw_stdev_ * yaw_stdev_;
// ================================================================
// === INITIAL SETUP ===
// ================================================================
printf("Initializing I2C...\n");
I2Cdev::initialize();
// verify connection
printf("Testing device connections...\n");
mpu = MPU9150(MPU9150_ADDRESS_AD0_HIGH);
cout << "device id:" << HEX(mpu.getDeviceID()) << endl;
if(mpu.testConnection()){
cout << "MPU9150 connection successful" << endl << flush;
}else{
cout << "MPU9150 connection failed" << endl << flush;
return 1;
}
// initialize device
printf("Initializing I2C devices...\n");
mpu.initialize();
// load and configure the DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
//attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
printf("DMP ready!\n");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
usleep(100000);
while(1){
loop();
}
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
printf("DMP Initialization failed (code %d)\n", devStatus);
}
cout << "Shutdown." << endl << flush;
return 0;
}
它编译得很好，似乎用输出正确初始化：
Initializing I2C...
Testing device connections...
device id:34
MPU9150 connection successful
Initializing I2C devices...
Initializing DMP...
Resetting MPU9150...
Disabling sleep mode...
Selecting user bank 16...
Selecting memory byte 6...
Checking hardware revision...
Revision @ user[16][6] = a5
Resetting memory bank selection to 0...
Reading OTP bank valid flag...
OTP bank is invalid!
Reading gyro offset values...
X gyro offset =
Y gyro offset =
Z gyro offset =
Enabling interrupt latch, clear on any read, AUX bypass enabled
Setting magnetometer mode to power-down...
Setting magnetometer mode to fuse access...
Reading mag magnetometer factory calibration...
Adjustment X/Y/Z = / /
Setting magnetometer mode to power-down...
Writing DMP code to MPU memory banks (7aa bytes)
Success! DMP code written and verified.
Configuring DMP and related settings...
Writing DMP configuration to MPU memory banks (e8 bytes in config def)
Success! DMP configuration written and verified.
Setting DMP and FIFO_OFLOW interrupts enabled...
Setting sample rate to 200Hz...
Setting clock source to Z Gyro...
Setting DLPF bandwidth to 42Hz...
Setting external frame sync to TEMP_OUT_L[0]...
Setting gyro sensitivity to +/- 2000 deg/sec...
Setting DMP configuration bytes (function unknown)...
Clearing OTP Bank flag...
Setting X/Y/Z gyro offsets to previous values...
Setting X/Y/Z gyro user offsets to zero...
Writing final memory update 1/19 (function unknown)...
Writing final memory update 2/19 (function unknown)...
Resetting FIFO...
Reading FIFO count...
Current FIFO count=
Writing final memory update 3/19 (function unknown)...
Writing final memory update 4/19 (function unknown)...
Disabling all standby flags...
Setting accelerometer sensitivity to +/- 2g...
Setting motion detection threshold to 2...
Setting zero-motion detection threshold to 156...
Setting motion detection duration to 80...
Setting zero-motion detection duration to 0...
Setting AK8975 to single measurement mode...
Setting up AK8975 read slave 0...
Setting up AK8975 write slave 2...
Setting up slave access delay...
Enabling default interrupt behavior/no bypass...
Enabling I2C master mode...
Resetting FIFO...
Rewriting I2C master mode enabled because...I don't know
Enabling and resetting DMP/FIFO...
Writing final memory update 5/19 (function unknown)...
Writing final memory update 6/19 (function unknown)...
Writing final memory update 7/19 (function unknown)...
Writing final memory update 8/19 (function unknown)...
Writing final memory update 9/19 (function unknown)...
Writing final memory update 10/19 (function unknown)...
Writing final memory update 11/19 (function unknown)...
Reading final memory update 12/19 (function unknown)...
Read bytes: 00
00
00
00
Writing final memory update 13/19 (function unknown)...
Writing final memory update 14/19 (function unknown)...
Writing final memory update 15/19 (function unknown)...
Writing final memory update 16/19 (function unknown)...
Writing final memory update 17/19 (function unknown)...
Waiting for FIRO count >= 46...
Reading FIFO...
Reading interrupt status...
Writing final memory update 18/19 (function unknown)...
Waiting for FIRO count >= 48...
Reading FIFO...
Reading interrupt status...
Waiting for FIRO count >= 48...
Reading FIFO...
Reading interrupt status...
Writing final memory update 19/19 (function unknown)...
Disabling DMP (you turn it on later)...
Setting up internal 48-byte (default) DMP packet buffer...
Resetting FIFO and clearing INT status one last time...
Setting X accel offset: 0
Setting Y accel offset: 0
Setting Z accel offset: 0
Setting X gyro offset: 0
Setting Y gyro offset: 0
Setting Z gyro offset: 0
Enabling DMP...
DMP ready!
然后它继续输出全零/默认读数，如：
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
FIFO overflow!
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
FIFO overflow!
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
quat 0.00 0.00 0.00 0.00 ypr (degrees) 180.00 nan nan areal (raw) 0 0 0 areal (m/s^2) 0 0 0
FIFO overflow!
即使我在物理上旋转并移动IMU，读数仍为0.我有两个这样的板，每个都采用相同的方式，所以它不太可能有缺陷。我做错了什么？