极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 83868|回复: 134

MPU 6050 读角度与单位换算

  [复制链接]
发表于 2012-11-13 16:00:13 | 显示全部楼层 |阅读模式
本帖最后由 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)
g.png


加速度库函数:
        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
a.png


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

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

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

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

  9. // MPU control/status vars
  10. bool dmpReady = false;  // set true if DMP init was successful
  11. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  12. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  13. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  14. uint16_t fifoCount;     // count of all bytes currently in FIFO
  15. uint8_t fifoBuffer[64]; // FIFO storage buffer

  16. // orientation/motion vars
  17. Quaternion q;           // [w, x, y, z]         quaternion container
  18. VectorFloat gravity;    // [x, y, z]            gravity vector
  19. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector


  20. // ================================================================
  21. // ===               INTERRUPT DETECTION ROUTINE                ===
  22. // ================================================================

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



  27. // ================================================================
  28. // ===                      INITIAL SETUP                       ===
  29. // ================================================================

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

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

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

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

  40.   delay(2);

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

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

  49.     // enable Arduino interrupt detection
  50.     Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
  51.     attachInterrupt(0, dmpDataReady, RISING);
  52.     mpuIntStatus = mpu.getIntStatus();

  53.     // set our DMP Ready flag so the main loop() function knows it's okay to use it
  54.     Serial.println("DMP ready! Waiting for first interrupt...");
  55.     dmpReady = true;

  56.     // get expected DMP packet size for later comparison
  57.     packetSize = mpu.dmpGetFIFOPacketSize();
  58.   }
  59.   else {
  60.     // ERROR!
  61.     // 1 = initial memory load failed
  62.     // 2 = DMP configuration updates failed
  63.     // (if it's going to break, usually the code will be 1)
  64.     Serial.print("DMP Initialization failed (code ");
  65.     Serial.print(devStatus);
  66.     Serial.println(")");
  67.   }
  68. }

  69. void loop()
  70. {
  71.   float alpha,omiga;

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

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

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

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

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

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

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

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

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

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

  103.     Serial.print("Alpha ");
  104.     Serial.print(alpha);
  105.     Serial.print("\tOmiga ");
  106.     Serial.println(omiga);
  107.    
  108.   }
  109. }
复制代码


****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!
回复

使用道具 举报

发表于 2012-11-13 21:33:54 | 显示全部楼层
楼主,是int接数字二么?能不能把你的结果贴出来谢谢啦
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-11-14 20:52:07 | 显示全部楼层
本帖最后由 johnsonzzd 于 2012-11-14 21:04 编辑
zhangzhe0617 发表于 2012-11-13 21:33
楼主,是int接数字二么?能不能把你的结果贴出来谢谢啦


对,就是这样。
无标题.jpg
接线图

无标题.jpg
晃动时的运行结果


回复 支持 反对

使用道具 举报

发表于 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的主要作用是什么?
回复 支持 反对

使用道具 举报

发表于 2012-11-15 15:10:50 | 显示全部楼层
johnsonzzd 发表于 2012-11-14 20:52
对,就是这样。

接线图

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

使用道具 举报

 楼主| 发表于 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.

点评

有用的信息呀  发表于 2013-12-25 12:57
回复 支持 反对

使用道具 举报

发表于 2012-11-16 13:04:49 | 显示全部楼层
johnsonzzd 发表于 2012-11-15 19:39
MPU6050已经不需要咱们自己写程序把角速度和角速度融合得到角度了。它里面同时集成了角速度和陀螺传感器,内 ...

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

使用道具 举报

发表于 2012-11-16 13:11:24 | 显示全部楼层
那到底哪个值可以用在PID上呢?alpha和omiga
回复 支持 反对

使用道具 举报

发表于 2012-11-20 20:09:11 | 显示全部楼层
zhangzhe0617 发表于 2012-11-15 09:25
楼主,我放着不动怎么数据跳得这么厉害啊
Alpha -0.11        Omiga 3.17
Alpha 71.51        Omiga 3.17

遇上了同样的问题。真是纠结啊。
回复 支持 反对

使用道具 举报

发表于 2012-11-20 20:33:58 | 显示全部楼层
zhangzhe0617 发表于 2012-11-16 13:04
单独读加速度和陀螺仪都没有问题,这个就不行,俯仰角Alpha静止了也一直在跳,Omiga应该是该旋转轴的角速 ...

知道问题出在哪没呢
回复 支持 反对

使用道具 举报

发表于 2012-11-20 20:53:09 | 显示全部楼层
utuu 发表于 2012-11-20 20:33
知道问题出在哪没呢

我放弃dmp了,还是用我的四元素好使
回复 支持 反对

使用道具 举报

发表于 2012-11-20 22:05:24 | 显示全部楼层
zhangzhe0617 发表于 2012-11-20 20:53
我放弃dmp了,还是用我的四元素好使

分享下代码呗
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-11-20 22:56:31 | 显示全部楼层
加速度有零点漂移是正常的。角度乱跳就不知道了。电压接3.3v试试。
回复 支持 反对

使用道具 举报

发表于 2012-11-21 10:25:15 | 显示全部楼层
johnsonzzd 发表于 2012-11-20 22:56
加速度有零点漂移是正常的。角度乱跳就不知道了。电压接3.3v试试。

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

使用道具 举报

发表于 2012-11-21 10:26:05 | 显示全部楼层
utuu 发表于 2012-11-20 22:05
分享下代码呗

正在调试,z轴乱跳还没解决,貌似是哪个式子有问题
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2018-12-16 16:22 , Processed in 0.078343 second(s), 29 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表