johnsonzzd 发表于 2012-11-13 16:00:13

MPU 6050 读角度与单位换算

本帖最后由 johnsonzzd 于 2013-5-28 18:36 编辑

参考数据手册:PS-MPU-6000A
使用带有DMP的最新库函数(https://github.com/jrowberg/i2cdevlib),程序模板采用MPU6050_DMP6例程。

角度:
    DMP库函数的dmpGetYawPitchRoll,可以得到pitch(俯仰),yaw(偏航),roll(滚转)角度。

角速度:
       void getRotation(int16_t* x, int16_t* y, int16_t* z);
      int16_t getRotationX();
      int16_t getRotationY();
      int16_t getRotationZ();
16位,配置时四个量程可选:±250,±500,±1000,±2000 度/s。
在dmp的例程中初始化:setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
说明是选用最高量程±2000º/s,则换算系数=2^16/4000=16.4 LSB/(度/s)



加速度库函数:
      void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
      int16_t getAccelerationX();
      int16_t getAccelerationY();
      int16_t getAccelerationZ();
16位,配置时四个量程可选:±2,±4,±8,±16 g。
在dmp的例程中没有找到初始化部分,估计是采用了缺省配置。
通过观察输出值,采用的是最小量程±2g,则换算系数=2^16/4=16384 LSB/g



下面是程序,中断方式,需要把MPU的中断接到arduino数字2脚上。

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu(0x68);

// 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; // FIFO storage buffer

// orientation/motion vars
Quaternion q;         //          quaternion container
VectorFloat gravity;    //             gravity vector
float ypr;         //    yaw/pitch/roll container and gravity vector


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;   // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                     ===
// ================================================================

void setup() {
Serial.begin(115200);        // opens serial port, sets data rate to 9600 bps

// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();

// initialize device
Serial.println("Initializing I2C devices...");
mpu.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

delay(2);

// load and configure the DMP
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if so)
if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println("Enabling DMP...");
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println("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
    Serial.println("DMP ready! Waiting for first interrupt...");
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print("DMP Initialization failed (code ");
    Serial.print(devStatus);
    Serial.println(")");
}
}

void loop()
{
float alpha,omiga;

// if programming failed, don't try to do anything
if (!dmpReady)
    return;

// wait for MPU interrupt or extra packet(s) available
if (!mpuInterrupt && fifoCount < packetSize)
    return;

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    Serial.println("FIFO overflow!");

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);//从DMP中取出Yaw、Pitch、Roll三个轴的角度,放入数组ypr。单位:弧度
    alpha=-ypr * 180/M_PI;

    omiga=mpu.getRotationX()/16.4;        //配置是16位表示正负2000°/s, 65536/4000

    Serial.print("Alpha ");
    Serial.print(alpha);
    Serial.print("\tOmiga ");
    Serial.println(omiga);
   
}
}

****MPU6050库的安装方法*************************
1. 下载arduino编译器,目前版本为1.0.5。
2. 下载MPU6050库。进入“https://github.com/jrowberg/i2cdevlib”,点击“zip”即可。
3. 安装。将上一步下载的“I2Cdev”、“MPU6050”两个文件夹拷贝到“arduino-1.0.5\libraries”目录下。
运行arduino可以在例子中看到MPU6050。OK!

zhangzhe0617 发表于 2012-11-13 21:33:54

楼主,是int接数字二么?能不能把你的结果贴出来谢谢啦

johnsonzzd 发表于 2012-11-14 20:52:07

本帖最后由 johnsonzzd 于 2012-11-14 21:04 编辑

zhangzhe0617 发表于 2012-11-13 21:33 static/image/common/back.gif
楼主,是int接数字二么?能不能把你的结果贴出来谢谢啦

对,就是这样。

接线图

晃动时的运行结果

zhangzhe0617 发表于 2012-11-15 09:25:40

楼主,我放着不动怎么数据跳得这么厉害啊
Alpha -0.11        Omiga 3.17
Alpha 71.51        Omiga 3.17
Alpha -21.91        Omiga 3.23
Alpha -20.44        Omiga 3.23
Alpha -85.02        Omiga 3.23
Alpha -41.15        Omiga 3.23
FIFO overflow!
FIFO overflow!
Alpha -0.95        Omiga 3.17
Alpha 29.76        Omiga 3.23
Alpha -29.50        Omiga 3.11
Alpha -3.14        Omiga 3.17
Alpha 27.24        Omiga 3.23
Alpha 43.88        Omiga 3.11
Alpha 0.05        Omiga 3.23
Alpha 2.24        Omiga 3.23
Alpha 56.07        Omiga 3.23
Alpha 46.69        Omiga 3.23
Alpha 53.02        Omiga 3.17
FIFO overflow!
Alpha -1.66        Omiga 3.23
Alpha 60.40        Omiga 3.23
Alpha -12.41        Omiga 3.23
Alpha -1.61        Omiga 3.17
Alpha -71.60        Omiga 3.23
Alpha 12.12        Omiga 3.23
Alpha 0.03        Omiga 3.17
Alpha -23.43        Omiga 3.23
Alpha 50.35        Omiga 3.23
Alpha 60.28        Omiga 3.23
FIFO overflow!
Alpha -2.18        Omiga 3.23
Alpha 20.16        Omiga 3.23
Alpha 5.45        Omiga 3.23
Alpha -1.11        Omiga 3.17
Alpha 58.67        Omiga 3.17
Alpha 51.14        Omiga 3.17
Alpha 0.10        Omiga 3.29
Alpha -48.13        Omiga 3.17
Alpha 30.72        Omiga 3.23
Alpha -8.20        Omiga 3.17
Alpha -2.00        Omiga 3.17
FIFO overflow!
我还不太明白这个dmp的主要作用是什么?

palysh 发表于 2012-11-15 15:10:50

johnsonzzd 发表于 2012-11-14 20:52 static/image/common/back.gif
对,就是这样。

接线图


楼主这个MPU6050 I2C地址是多少?
通过看手册应该是AD0接地为:0x68AD0接VCC为:0x69
但通过WHO_AM_I寄存器进行身份验证时出错
例程中给的是0xD0不知道为什么

johnsonzzd 发表于 2012-11-15 19:39:32

MPU6050已经不需要咱们自己写程序把角速度和角速度融合得到角度了。它里面同时集成了角速度和陀螺传感器,内部还有处理器(Digital Motion Processor)在进行数据处理,可以直接从它的内部寄存器把角度读出来,这个东西就是所谓的DMP。你的角度跳动是不应该的,我也不清楚原因。可以直接读加速度出来看看,静止应该是不变的。
我的地址从程序里可以看出来,MPU6050 mpu(0x68);。

http://www.i2cdevlib.com/devices/mpu6050#help
What is the DMP?
The DMP, or Digital Motion Processor, is an internal processing unit contained within the MPU-6050 and its successors (MPU-6150, MPU-9150, possibly more in the future). The processor has been described by InvenSense employees as a sort of limited CPU, or alternatively as an enhanced ALU (arithmetic logic unit), which is built with an instruction set designed for very specific 3D math operations necessary for orientation calculation. There is currently no known resource for understanding what this instruction set is or how it works.

zhangzhe0617 发表于 2012-11-16 13:04:49

johnsonzzd 发表于 2012-11-15 19:39 static/image/common/back.gif
MPU6050已经不需要咱们自己写程序把角速度和角速度融合得到角度了。它里面同时集成了角速度和陀螺传感器,内 ...

单独读加速度和陀螺仪都没有问题,这个就不行,俯仰角Alpha静止了也一直在跳,Omiga应该是该旋转轴的角速度吧?静止了不是应该为0么?

smokesea 发表于 2012-11-16 13:11:24

那到底哪个值可以用在PID上呢?alpha和omiga

utuu 发表于 2012-11-20 20:09:11

zhangzhe0617 发表于 2012-11-15 09:25 static/image/common/back.gif
楼主,我放着不动怎么数据跳得这么厉害啊
Alpha -0.11        Omiga 3.17
Alpha 71.51        Omiga 3.17


遇上了同样的问题。真是纠结啊。

utuu 发表于 2012-11-20 20:33:58

zhangzhe0617 发表于 2012-11-16 13:04 static/image/common/back.gif
单独读加速度和陀螺仪都没有问题,这个就不行,俯仰角Alpha静止了也一直在跳,Omiga应该是该旋转轴的角速 ...

知道问题出在哪没呢

zhangzhe0617 发表于 2012-11-20 20:53:09

utuu 发表于 2012-11-20 20:33 static/image/common/back.gif
知道问题出在哪没呢

我放弃dmp了,还是用我的四元素好使

utuu 发表于 2012-11-20 22:05:24

zhangzhe0617 发表于 2012-11-20 20:53 static/image/common/back.gif
我放弃dmp了,还是用我的四元素好使

分享下代码呗

johnsonzzd 发表于 2012-11-20 22:56:31

加速度有零点漂移是正常的。角度乱跳就不知道了。电压接3.3v试试。

zhangzhe0617 发表于 2012-11-21 10:25:15

johnsonzzd 发表于 2012-11-20 22:56 static/image/common/back.gif
加速度有零点漂移是正常的。角度乱跳就不知道了。电压接3.3v试试。

就是接的3.3v。也许是漂移没处理好,所以导致dmp结果出现问题

zhangzhe0617 发表于 2012-11-21 10:26:05

utuu 发表于 2012-11-20 22:05 static/image/common/back.gif
分享下代码呗

正在调试,z轴乱跳还没解决,貌似是哪个式子有问题
页: [1] 2 3 4 5 6 7 8 9 10
查看完整版本: MPU 6050 读角度与单位换算