Arduino如何获取MPU6050的姿态数据

Arduino如何获取MPU6050的姿态数据

其实代码也是在Github开源的项目，但是自己确实也是理解了许久，先给出自己翻译以及稍微修改的代码，这代码是可以直接使用的（无需修改就行可以用，串口打印数据），关于接线我就不做过多的说明了，注意arduino nano的SCL是A5，SDA是A4就行，我用的是Arduino MEGA 2560，上面有SCL，SDA专门标注有。
准备：I2Cdev、MPU6050库（这里附送的是我发在网上的资源，大家也可找开源的地址了）
注意：MPU6050用最新的，不然下面的 CalibrateAccel、、 那个三个函数用不了（会报错找不到），当然我发的这个不会有错。。。有点黄婆卖瓜——自卖自夸了，，，哈哈哈

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <EEPROM.h>

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define OUTPUT_READABLE_QUATERNION  //定义后串口才可打印四元数
#define OUTPUT_READABLE_YAWPITCHROLL //定义后串口才可打印 偏航/俯仰/横摇容器
#define OUTPUT_READABLE_GRAVITY

#define INTERRUPT_PIN 2  // 中断引脚D2
#define LED_PIN 13 // led灯
#define MPUCALIB 80           // 16字节数组
//缩写
#define PT(s) Serial.print(s)  //makes life easier
#define PTL(s) Serial.println(s)
#define PTF(s) Serial.print(F(s))//trade flash memory for dynamic memory with F() function
#define PTLF(s) Serial.println(F(s))

bool blinkState = false;

//MPU控制/状态变量
bool dmpReady = false;  // set true if DMP init was successful            如果DMP init成功，则设置为true
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU    保留来自MPU的实际中断状态字节
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)每次设备操作后返回状态（0 =成功，！0 =错误）
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes) 预期的DMP包大小(默认为42字节)
uint16_t fifoCount;     // count of all bytes currently in FIFO           当前FIFO中所有字节的计数
uint8_t fifoBuffer[64]; // FIFO storage buffer                            FIFO存储缓冲区

Quaternion q;           // [w, x, y, z]         quaternion container四元数容器
VectorFloat gravity;    // [x, y, z]            gravity vector      重力矢量
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container 偏航/俯仰/横摇容器

//中断检测程序
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high 指示主控板中断引脚是否过高
void dmpDataReady() {
    mpuInterrupt = true;
}

int EEPROMReadInt(int p_address)
{
  byte lowByte = EEPROM.read(p_address);
  byte highByte = EEPROM.read(p_address + 1);
  return ((lowByte << 0) & 0xFF) + ((highByte << 8) & 0xFF00);
}

//初始化设置
void setup()
{
    //加入I2C总线（I2Cdev库不会自动执行此操作）
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        //Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties //400千赫I2C时钟。如果有编译困难，注释这行
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
    
    //初始化串行通讯
    Serial.begin(57600);
    while (!Serial); 

    //初始化设备
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();             //初始化MPU6050
    pinMode(INTERRUPT_PIN, INPUT);//设置中断引脚D2的模式为输入

    // 验证连接
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // 等待准备
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again

     // 加载并配置DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    //在此处提供您自己的陀螺仪偏移量，以最小灵敏度进行缩放
     for (byte i = 0; i < 4; i++)
     {
      PT(EEPROMReadInt(MPUCALIB + 4 + i * 2));
      PT(" ");
     }
    PTL();
    mpu.setZAccelOffset(EEPROMReadInt(MPUCALIB + 4));
    mpu.setXGyroOffset(EEPROMReadInt(MPUCALIB + 6));
    mpu.setYGyroOffset(EEPROMReadInt(MPUCALIB + 8));
    mpu.setZGyroOffset(EEPROMReadInt(MPUCALIB + 10));

    //确保它正常工作（如果是，则返回0）
    if (devStatus == 0) 
    {
        //校准时间：生成偏移并校准我们的MPU6050
       mpu.CalibrateAccel(6);
       mpu.CalibrateGyro(6);
       mpu.PrintActiveOffsets();
        
        //打开DMP，现在已经准备好
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        //启用Arduino中断检测
        Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        //设置DMP Ready标志，以便主loop（）函数知道可以使用它
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;
        
        //获得预期的DMP数据包大小，以便以后进行比较
        packetSize = mpu.dmpGetFIFOPacketSize();
    }
    else 
    {
        // ERROR!
        // 1 = 初始内存加载失败
        // 2 = (DMP配置更新失败)
        //（如果要中断，通常代码将为1）
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    // 配置LED输出
    pinMode(LED_PIN, OUTPUT);
    
}
//主程序循环
void loop()
{  
  //如果编程失败，请不要尝试做任何事情
  if (!dmpReady) return;
   if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
   {
    #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            //以简单矩阵形式显示四元数值：w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);                                                                                                                                                                          
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

         #ifdef OUTPUT_READABLE_GRAVITY
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);                                                                                                                                                                         
            Serial.print("gravity\t");
            Serial.print(gravity.x);
            Serial.print("\t");
            Serial.print(gravity.y);
            Serial.print("\t");
            Serial.println(gravity.z);
        #endif

        // led闪烁指示活动
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
   }
  
}

Arduino写ROS的发布者发布MPU数据

在基础上，我再添加了的ros的发布者，将MPU的四元数数据发布给了上位机。
（提一嘴，如果想把Arduino写成ROS的一个节点而且交互数据量在280字节以上的，不要用nano板子，不然MPU6050这样的数据都传不到上位机，接不通的（毕设遇到的坑，rosserial_python节点运行会一直报错），所以最好MEGA，输出数据可达1024）。

这部分代码后续更新博客