MPU6050的DMP,如何根據FIFO的42位數據得到四元數,解出歐拉角？

ID:297166 · 發表于 2018-9-19 23:22

新手上路，緊急求助……

最近要用MPU6050測物體的翻轉角度，用的是51單片機，stc12c5a60s2。官方的庫看不懂，在網上找到一個用于51的DMP初始化，但是得到的是一個來自于FIFO的42位數據。請問這個數據要怎么處理才能得到四元數，解出歐拉角？

而且在不做任何改動的情況下串口接收到的都是亂碼，如何處理呢?
自己實在搞不清楚了，來求助萬能的壇友們，希望個位大佬能給點寶貴的意見……
多謝了

下面貼下源碼，來自于blog.csdn點net/ksws0263785/article/details/46462991

/*****************************************************
功能: 采集MPU6050 DMP 數據
CPU: STC89C54RD+
晶震: 11.0592
環境: Keli2.0
語言: c
作者：XW1005
來源：移植 Jeff Jrowberg 公開的程序。
目的：
sda 或則scl 為高時是:釋放總線
*****************************************************/
#include <reg52.h>
#include <intrins.h>
bit ack; //應答標志位 0:ack 1：nak
sbit scl = P2^0;
sbit sda = P2^1;
unsigned char dmpdatas[42]; //DMP數據
#define SlaveAddress 0xD0 //IIC寫入時的地址字節數據，+1為讀取
//#define SlaveAddress 0xae //i2c測試地址
void SendByte(unsigned char dat);
unsigned char i2cread(unsigned char addr,unsigned char *Data);
unsigned char i2creads(unsigned char addr,unsigned char length,unsigned char *Data);
//以下的 firmware 及 config update 數據來自于 Jeff Jrowberg 公開的程序
code unsigned char dmpmemorydata[1929]={
// bank 0, 256 bytes
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
// bank 1, 256 bytes
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
// bank 2, 256 bytes
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
// bank 3, 256 bytes
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C,
0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
// bank 4, 256 bytes
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
// bank 5, 256 bytes
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
// bank 6, 256 bytes
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
// bank 7, 138 bytes (remainder)
0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3,
0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC,
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
};
code unsigned char dmpcfgupddata[192] = {
// dmp config
// BANK OFFSET LENGTH [DATA]
0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C,
0x03, 0xAB, 0x03, 0x36, 0x56, 0x76,
0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2,
0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1,
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00,
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
0x03, 0x89, 0x03, 0x26, 0x46, 0x66,
0x00, 0x6C, 0x02, 0x20, 0x00,
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00,
0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00,
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00,
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D,
0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D,
0x00, 0xA3, 0x01, 0x00,
0x00, 0x00, 0x00, 0x01, //這里是開啟DMP的特殊中斷的
//原程序中此行代碼為(這里不一定錯)
//0x00, 0x00, 0x00, 0x01, 即LENGTH=0x00，有錯
0x07, 0x86, 0x01, 0xFE,
0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38,
0x07, 0x7E, 0x01, 0x30,
0x07, 0x46, 0x01, 0x9A,
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38,
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38,
0x02, 0x16, 0x02, 0x00, 0x01,
/* 上行最后一個數據調整FIFO rate :0x01=100HZ,0x02=66HZ,0x03=50HZ ,0x04=40HZ,0x05=33.33HZ,
// 可從 datasheet 公式推算
//dmp updates
0x01, 0xB2, 0x02, 0xFF, 0xFF,
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35,
0x01, 0x6A, 0x02, 0x06, 0x00,
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
0x01, 0x62, 0x02, 0x00, 0x00,
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00*/
};
code unsigned char dmpUpdates[47]={
0x01, 0xB2, 0x02, 0xFF, 0xFF,
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35,
0x01, 0x6A, 0x02, 0x06, 0x00,
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
0x01, 0x62, 0x02, 0x00, 0x00,
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00
};
void DelayUs2x(unsigned char t) //延時1
{
while(--t);
}
void DelayMs(unsigned char t) //延時2
{
while(t--)
{
//大致延時1mS
DelayUs2x(245);
DelayUs2x(245);
}
}
/*i2c基本協議*/
/*i2c基本協議*/
void i2_qs(void) //啟始信號
{
sda = 1;
scl = 1;
_nop_();
sda = 0;
_nop_();_nop_();
_nop_();_nop_();
scl = 0;
}
void i2_tz() //停止信號
{
sda = 0;
scl = 1;
_nop_();_nop_();
_nop_();_nop_();
sda = 1;
_nop_();
}
void i2_fs(unsigned char Data) //發送8位數據
{
unsigned char i=0;
for(;i<8;i++)
{
Data <<= 1;
sda = CY;
scl = 1;
_nop_();_nop_();
_nop_();_nop_();
scl = 0;
}
//下面是接收從設備發回的應答信號
sda = 1;
scl = 1;
_nop_();_nop_();
_nop_();_nop_();
ack = sda; //接收設備的ack _n
scl = 0;
sda = 1;
}
unsigned char i2_js(bit _ack) //接收8位數據
{
unsigned char i = 0,Data = 0;
for(;i<8;i++)
{
Data <<= 1;
scl = 1;
_nop_();_nop_();
_nop_();_nop_();
Data |= sda; //接收數據
scl = 0;
_nop_();
}
//下面時主控器發送ACK或則NAK
sda = _ack; //ack或者nak
scl = 1; //拉高時鐘產生第9個時鐘
_nop_();_nop_();
_nop_();_nop_();
scl = 0; //拉底時鐘
sda = 1; //釋放總線
_nop_();
return Data;
}
/*i2c協議基本*/
/*寫入8位寄存器一個字節
參數 addr 寄存器地址
參數 Data 寫入數據
返回值 (1=成功,0=失敗)
*/
unsigned char i2cwrite(unsigned char addr,unsigned char Data) //寫入8位有效數據
{
i2_qs(); //起始信號
i2_fs(SlaveAddress); //設備地址+寫信號
if(ack) return 0; //判斷是否是ACK如果不是返回0
i2_fs(addr); //設備內部地址
if(ack) return 0;
i2_fs(Data); //寫入的數據
if(ack) return 0;
i2_tz(); //停止信號
return 1;
}
/*讀取8位寄存器一個字節
參數 addr 寄存器地址
參數 *Data 數據存儲地址
返回值 (1=成功,0=失敗)
*/
unsigned char i2cread(unsigned char addr,unsigned char *Data)
{
/*unsigned char Data1;
i2_qs(); //起始信號
i2_fs(SlaveAddress); //設備地址+寫信號
SendByte(ack);
i2_fs(addr); //設備內部地址
i2_qs(); //起始信號
i2_fs(SlaveAddress+1); //設備地址+讀信號
*Data = i2_js(1); //讀取數據
i2_tz(); //停止信號*/
return i2creads(addr,1,Data);
}
/*讀取8位寄存器多個字節
* @參數 addr I2C從器件內部地址
* @參數 length 寫入數據長度
* @參數 Data 保存數據的地址
* @返回值返回狀態 (1=成功)
*/
unsigned char i2creads(unsigned char addr,unsigned char length,unsigned char *Data)
{
unsigned char i;
length --;
i2_qs(); //起始信號
i2_fs(SlaveAddress); //設備地址+寫信號
if(ack) return 0;
i2_fs(addr); //設備內部地址
if(ack) return 0;
i2_qs(); //起始信號
i2_fs(SlaveAddress+1); //設備地址+讀信號
if(ack) return 0;
for(i=0;i<length;i++)
{
Data[i]= i2_js(0); //讀取數據,發送ACK
}
Data[length] = i2_js(1);//讀取數據,發送NAK
i2_tz(); //停止信號
return 1;
}
/**寫入8位寄存器的一個位。
* @參數 addr I2C從器件內部地址
* @參數 bitNum 寫入的比特位(0-7)
* @參數 data 寫入數據
* @返回值返回狀態 (1=成功)
*/
unsigned char writeBit(unsigned char addr,unsigned char bitNum,unsigned char Data)
{
unsigned char b;
if (i2cread(addr,&b))
{
b = (Data != 0) ? (b | (1 << bitNum)):(b & ~(1 << bitNum));
return i2cwrite(addr,b); //寫入數據
}
else
return 0;
}
/**寫入8位寄存器的多個位。
* @參數 addr I2C從器件內部地址
* @參數 bitStart 第一位的寫入位置（0-7）
* @參數 length 寫的比特數(不超過8)
* @參數 Data 寫入數據
* @返回值返回狀態 (1=成功)
*/
unsigned char writeBits(unsigned char addr,unsigned char bitStart,unsigned char length,unsigned char Data)
{
// 010 要寫入的值
// 76543210 比特位
// xxx args: bitStart=4, length=3
// 00011100 掩碼字節
// 10101111 原始值（樣本）
// 10100011 原始值 & ~掩碼
// 10101011 掩碼 | 原始值
unsigned char b,mask=0;
if (i2cread(addr,&b))
{
mask = (((1<<length) - 1) << (bitStart-length + 1)); //掩碼
Data <<=(bitStart - length + 1); //把寫入的數據移動到位
Data &= mask;
b &= ~(mask);
b |= Data;
i2cwrite(addr,b); //寫入數據
return 1;
}
else
return 0;
}
/**讀取一個位從8位器件的寄存器。
* @參數 addr I2C從器件內部地址
* @參數 bitNum 位的位置來讀取（0-7）
* @參數 *data 數據存儲地址
* @返回值（1=成功）
*/
unsigned char readBit(unsigned char addr,unsigned char bitNum,unsigned char *Data)
{
unsigned char b;
if(i2cread(addr,&b))
{
*Data = b & (1 << bitNum);
return 1;
}
else
{
return 0;
}
}
/**讀取8位寄存器的多個位。
* @參數 addr I2C從器件內部地址
* @參數 bitStart第一位的位置讀取（0-7）
* @參數 length 位讀取@參數長度數（不超過8）
* @參數 *data 數據存儲地址（即'101'任何bitStart位置讀取將等于0X05）
* @返回值（1=成功）
*/
unsigned char readBits(unsigned char addr,unsigned char bitStart,unsigned char length,unsigned char *Data)
{
// 01101001 讀取字節
// 76543210 比特位
// xxx args: bitStart=4, length=3
// 010 masked
// -> 010 shifted
unsigned char b,mask=0;
if(i2cread(addr,&b))
{
mask = ((1 << length) - 1) << (bitStart - length + 1);
b &= mask;
b >>= (bitStart - length + 1);
*Data = b;
return 1;
}
else
return 0;
}
/*void init6050() //初始化 6050
{
i2cwrite(0x6b,0x80); //電源管理（0x00 正常啟用）
i2cwrite(0x6b ,0x01); //No sleep , 3 PLL with X axis gyroscope reference電源管理
DelayMs(200);
i2cwrite(0x23 ,0x00); //FIFO 11111000 ,temp,gx,gy,gz,accel？
i2cwrite(0x19 ,0x05); //陀螺儀采樣率
i2cwrite(0x1a ,0x03); //低通濾波
i2cwrite(0x1b ,0x18); //陀螺儀自檢，量程范圍 2000 /s
i2cwrite(0x1c ,0x00); //加速計自檢，量程范圍及高通濾波頻率 +-2g/s
}
*/
/*
加載 DMP代碼到
返回值 (1=成功,0=失敗)
*/
unsigned char loadfirmware(void)
{
unsigned int datanum=0; //DMP固件寫入標志位
unsigned char ye,i,j;
unsigned char bank=0; //段（256個數據一段）
unsigned char addr=0;
for(;bank<8;bank++)
{
if(bank == 7) //這里的作用就是區分最后一段數據
i = 8;
else
i = 16;
for(ye=0;ye<i;ye++)
{
i2cwrite(0x6d,bank);
i2cwrite(0x6e,addr);
i2_qs(); //起始信號
i2_fs(SlaveAddress); //設備地址+寫信號
i2_fs(0x6f); //設備內部地址
for(j=0;j<16;j++)
{
i2_fs(dmpmemorydata[datanum++]); //寫入DMP的數據
if(ack) return 0;
}
addr += 16;
i2_tz(); //停止信號
}
}
i2cwrite(0x6d,7);
i2cwrite(0x6e,addr);
i2_qs(); //起始信號
i2_fs(SlaveAddress); //設備地址+寫信號
i2_fs(0x6f); //設備內部地址
for(i=0;i<9;i++)
{
i2_fs(dmpmemorydata[datanum++]); //寫入DMP的數據
if(ack) return 0;
}
i2_tz(); //停止信號
return 1;
}
unsigned char loadcfgupd(void) //DMP設置
{
unsigned char line; //一共需要寫入30條設置數據
unsigned char bank; //頁
unsigned char datacounts=0; //DMP設置數據標志位
unsigned char bytes2write; //數據長度。
unsigned char offset; //偏移地址
unsigned char writingcounts; //數據寫入標志與bytes2write一同使用
unsigned char special;
for (line=0;line<30;line++)
{
bank=dmpcfgupddata[datacounts++];
offset=dmpcfgupddata[datacounts++];
bytes2write=dmpcfgupddata[datacounts++];
i2cwrite(0x6d,bank);
i2cwrite(0x6e,offset);
i2_qs(); //起始信號
i2_fs(SlaveAddress); //設備地址+寫信號
i2_fs(0x6f); //設備內部地址
for(writingcounts=0;writingcounts<bytes2write;writingcounts++)
{
i2_fs(dmpcfgupddata[datacounts++]); //寫入DMP配置數據
if(ack) return 0;
}
if(0 == bytes2write)
{
special=dmpcfgupddata[datacounts++];
if(0x01 == special)
{
//設置零運動中斷啟用（真）;
//設置FIFO緩沖區溢出啟用（真）;
//設置DMP啟用（真）;
i2cwrite(0x38,0x32);
}
else
return 0;
}
}
i2_tz(); //停止信號
return 1;
}
/*最后更新DMP*/
unsigned char xdmpUpdates(unsigned char datacounts)
{
unsigned char writingcounts,bank,offset,bytes2write;
bank=dmpUpdates[datacounts++];
offset=dmpUpdates[datacounts++];
bytes2write=dmpUpdates[datacounts++];
i2cwrite(0x6d,bank);
i2cwrite(0x6e,offset);
i2_qs(); //起始信號
i2_fs(SlaveAddress); //設備地址+寫信號
i2_fs(0x6f); //設備內部地址
for(writingcounts=0;writingcounts<bytes2write;writingcounts++)
{
i2_fs(dmpUpdates[datacounts++]); //寫入DMP配置數據
if(ack) return 0;
}
i2_tz(); //停止信號
return 0;
}
/*讀取 FIFO 計數*/
unsigned int getFIFOCount()
{
unsigned char i[2];
i2creads(0x72,2,i);
return ((i[0]<<8)+i[1]);
}
/*FIFO數據讀取
參數 *Data 存儲數據的地址
返回值 (1=讀取成功,0讀取失敗)
*/
unsigned char readdmp(unsigned char *Data)
{
return i2creads(0x74,42,Data);
}
//加載并配置 DMP 數字運動處理引擎
unsigned char dmpInitialize(void)
{
unsigned char hwRevision,otpValid,mpuIntStatus/*fifoBuffer[128]*/;
unsigned char xgOffsetTC,ygOffsetTC,zgOffsetTC;
unsigned int fifoCount;
writeBit(0x6B,7,1); //復位 MPU6050
DelayMs(30);
writeBit(0x6B,6,0); //禁止睡眠模式
i2cwrite(0x6D,0x70); //寫入一個字節數據到0x6d寄存器(選擇用戶 bank)
i2cwrite(0x6E,0x06); //寫入一個字節數據到0x6e寄存器(選擇存儲字節)
i2cread(0x6F,&hwRevision); //讀取
i2cwrite(0x6D,0); //重置內存 bank 選擇
readBit(0x00,0,&otpValid); //讀取 OTP bank 有效標志
readBits(0x00,6,6,&xgOffsetTC); //讀陀螺偏置TC值 X
readBits(0x01,6,6,&ygOffsetTC); //讀陀螺偏置TC值 Y)
readBits(0x02,6,6,&zgOffsetTC); //讀陀螺偏置TC值 Z
i2cwrite(0x25,0x7f); //設置從0地址 0x7
writeBit(0x6A,5,0); //禁用I2C主模式
i2cwrite(0x25,0x68); //這里可能要改。還沒有弄明白這里
writeBit(0x6A,1,1); //I2C總線主控復位
DelayMs(20);
if((loadfirmware()) == 0 ) return 0; //加載 DMP代碼到內存
if((loadcfgupd()) == 0 ) return 0; //配制DMP
writeBits(0x6B,2,3,0x03); //設置時鐘脈沖源Z陀螺
i2cwrite(0x38,0x12); //設置DMP和FIFO_OFLOW啟用中斷
i2cwrite(0x19,4); //設置采樣率為200 hz (1khz / (1 + 4) = 200 Hz)
writeBits(0x1A,5,3,0x1); //設置外部幀同步TEMP_OUT_L[0]
writeBits(0x1A,2,3,0x03); //設置DLPF帶寬42赫茲
writeBits(0x1B,4,2,0x03); //陀螺靈敏度設置為+ / - 2000 deg/sec
i2cwrite(0x70,0x03); //設置DMP配置字節（功能未知）
i2cwrite(0x71,0x00); //設置DMP配置字節（功能未知）
writeBit(0x00,0,0); //清除OTP Bank 標志
writeBits(0x00,6,6,xgOffsetTC); //設置X 陀螺抵消TCs之前的值
writeBits(0x01,6,6,ygOffsetTC); //設置Y 陀螺抵消TCs之前的值
writeBits(0x02,6,6,zgOffsetTC); //設置Z 陀螺抵消TCs之前的值
xdmpUpdates(0); //最后更新1/7(函數未知)dmpUpdates數組第一行
xdmpUpdates(5); //最后更新2/7(函數未知)dmpUpdates數組第二行
writeBit(0x6A,2,1); //復位 FIFO
fifoCount = getFIFOCount(); //讀取 FIFO 計數
//readdmp(fifoCount,fifoBuffer); //讀取FIFO里的數據
writeBit(0x6A,2,1); //復位 FIFO
i2cwrite(0x1F,2); //運動檢測閾值設置為2
i2cwrite(0x21,156); //零運動檢測閾值為156
i2cwrite(0x20,80); //設置運動檢測持續時間至80
i2cwrite(0x22,0); //設置零運動檢測時間0
writeBit(0x6A,2,1); //復位 FIFO
writeBit(0x6A,6,1); //使能 FIFO
writeBit(0x6A,7,1); //使能 DMP
writeBit(0x6A,3,1); //復位 DMP
xdmpUpdates(12); //最后更新3/7(函數未知)dmpUpdates數組第三行
xdmpUpdates(17); //最后更新4/7(函數未知)dmpUpdates數組第四行
xdmpUpdates(28); //最后更新5/7(函數未知)dmpUpdates數組第五行
while((fifoCount = getFIFOCount()) < 3); //等待 FIFO 計數 > 2
writeBit(0x6A,2,1); //復位 FIFO
//readdmp(fifoCount,fifoBuffer); //讀取FIFO里的數據
i2cread(0x3A,&mpuIntStatus); //讀取中斷狀態
xdmpUpdates(35); //最后更新6/7(函數未知)dmpUpdates數組第六行
while((fifoCount = getFIFOCount()) < 3); //等待 FIFO 計數 > 2
writeBit(0x6A,2,1); //復位 FIFO
//readdmp(fifoCount,fifoBuffer); //讀取FIFO里的數據
i2cread(0x3A,&mpuIntStatus); //讀取中斷狀態
xdmpUpdates(40); //最后更新7/7(函數未知)dmpUpdates數組第七行
writeBit(0x6A,7,0); //禁用DMP(稍后您打開它)
writeBit(0x6A,2,1); //復位 FIFO
i2cread(0x3A,&mpuIntStatus);
//星期六 (2014/06/28)
return 1;
}
/*初始化MPU6050*/
void initMPU6050(void)
{
writeBits (0x6B,2,3,0x01); //電源管理
writeBits (0x1B,4,2,0x00); //設置陀螺儀量程 250/s
writeBits (0x1C,4,2,0x00); //設置加速度量程 2G
writeBit (0x6B,6,1); //電源管理MUP進入睡眠模式
}
/*驗證MPU6050連接*/
unsigned char getDeviceID(void)
{
unsigned char b=0; //臨時變量
readBits(0x75,6,6,&b); //讀取i2c固定地址，去掉最高位和最低位這兩位數據
return b == 0x34; //判斷B是否等于0x34，如果等于返回1，不等于返回0(庫的是0x38)
}
/**************************************串口**********************************/
void CSH (void) //初始化串口
{
SCON = 0x50; // SCON: 模式 1, 8-bit UART, 使能接收
TMOD |= 0x20; // TMOD: timer 1, mode 2, 8-bit 重裝
TH1 = 0xFD; // TH1: 重裝值 9600 波特率晶振 11.0592MHz
TR1 = 1; // TR1: timer 1 打開
EA = 1; //打開總中斷
//ES = 1; //打開串口中斷
}
void SendByte(unsigned char dat) //發送一個字符
{
SBUF = dat; //SBUF 串行數據緩沖器
while(!TI); //TI發送中斷標志位 (當數據發送完畢后由硬件置 1 否則等待硬件置 1)
TI = 0;
}
/************************************************************************************/
void main(void)
{
unsigned char zd,i;
CSH(); //初始化串口
initMPU6050(); //初始化
if (getDeviceID()) //驗證連接是否正常(讀取MPU6050的I2C地址)
{
if(!(dmpInitialize())) //加載并配置運動庫
while(1);
}
writeBit(0x6A,2,1); //復位 FIFO
writeBit(0x6A,7,1); //使能DMP
while(1)
{
i=getFIFOCount();//讀取FIFO計數
i2cread(0x3A,&zd); //讀取中斷狀態
if((zd & 0x10)||i==1024) //判斷FIFO是否溢出
{
writeBit(0x6A,2,1); //復位 FIFO
}
else if (zd & 0x02)
{
while(i<42) i=getFIFOCount();
readdmp(dmpdatas); //讀取FIFO數據(四元數+其他的數據)
SendByte(dmpdatas[0]);
SendByte(dmpdatas[1]);
SendByte(dmpdatas[4]);
SendByte(dmpdatas[5]);
SendByte(dmpdatas[8]);
SendByte(dmpdatas[9]);
SendByte(dmpdatas[12]);
SendByte(dmpdatas[13]);
}
}
}

復制代碼

ID:310519 · 發表于 2018-9-20 11:54

用官方的吧，直接輸出四元數。

ID:574341 · 發表于 2019-7-9 13:16

#include<reg52.h> #include<math.h> #include<stdio.h> #include<intrins.h> typedef unsigned char uchar; typedef unsigned short ushort; typedef unsigned int uint; sbit SCL = P1^5; sbit SDA = P1^4; #define Kp 100.0f #define Ki 0.002f #define halfT 0.001f uint q0 = 1, q1 = 0, q2 = 0, q3 = 0; uint exInt = 0, eyInt = 0, ezInt = 0; uint Yaw; #define SMPRT_DIV 0x19 #define CONFIG 0x1A #define GYRO_CONFIG 0x1B #define ACCEL_CONFIG 0x1C #define ACCEL_XOUT_H 0x3B #define ACCEL_XOUT_L 0x3C #define ACCEL_YOUT_H 0x3D #define ACCEL_YOUT_L 0x3E #define ACCEL_ZOUT_H 0x3F #define ACCEL_ZOUT_L 0x40 #define TEMP_OUT_H 0x41 #define TEMP_OUT_L 0x42 #define GYRO_XOUT_H 0x43 #define GYRo_XOUT_L 0x44 #define GYRO_YOUT_H 0x45 #define GYRo_YOUT_L 0x46 #define GYRO_ZOUT_H 0x47 #define GYRo_ZOUT_L 0x48 #define PWR_MGMT_1 0x6B #define WHO_AM_I 0x75 #define SlaveAddress 0xD0 uchar dis[6]; int dis_data; void delay(uint k) { uint i,j; for(i=0;i<k;i++) { for(j=0;j<121;j++); } } void Delay5us() { _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); } void I2C_Start() { SCL = 1; SDA = 1; Delay5us(); SDA = 0; Delay5us(); SCL = 0; } void I2C_Stop() { SDA = 0; SCL = 1; Delay5us(); SDA = 1; Delay5us(); SDA = 0; } void I2C_SendACK(bit ack)//發送應答信號 { SDA = ack; SCL = 1; Delay5us(); SCL = 0; Delay5us(); } bit I2C_RecvACk() { SCL = 1; Delay5us(); CY = SDA; SCL = 0; Delay5us(); return CY; } void I2C_SendByte(uchar dat) { uchar i; for(i=0;i<8;i++) { dat <<= 1; SDA = CY; SCL = 1; Delay5us(); SCL = 0; Delay5us(); } I2C_RecvACk(); } bit I2C_RecvByte() { uchar i; uchar dat = 0; SDA = 1; for(i=0;i<8;i++) { dat <<=1; SCL = 1; Delay5us(); dat |= SDA; SCL = 0; Delay5us(); } return dat; } void Single_WriteI2C(uchar REG_address,uchar REG_data) { I2C_Start(); I2C_SendByte(SlaveAddress); I2C_SendByte(REG_address); I2C_SendByte(REG_data); I2C_Stop(); } uchar Single_ReadI2C(uchar REG_Address) { uchar REG_data; I2C_Start(); I2C_SendByte(SlaveAddress); I2C_SendByte(REG_Address); I2C_Start(); I2C_SendByte(SlaveAddress+1); REG_data = I2C_RecvByte(); I2C_SendACK(1); I2C_Stop(); return REG_data; } void init_uart() { TMOD = 0x21; TH1 = 0xfd; //f3 TL1 = 0xfd; SCON = 0x50; PS = 1; TR0 = 1; TR1 = 1; ET0 = 1; ES = 1; EA = 1; } void InitMPU6050() { Single_WriteI2C(PWR_MGMT_1, 0x00); Single_WriteI2C(SMPRT_DIV, 0x07); Single_WriteI2C(CONFIG, 0x06); Single_WriteI2C(GYRO_CONFIG,0x18); Single_WriteI2C(ACCEL_CONFIG, 0x01); } void SeriPushSend(uchar send_data) { SBUF = send_data; while(!TI);TI = 0; } int GetData(uchar REG_Address) { uchar H,L; H = Single_ReadI2C(REG_Address); L = Single_ReadI2C(REG_Address+1); return ((H<<8)+L); } void lcd_printf(uchar *s, int temp_data) { if(temp_data<0) { temp_data = -temp_data; *s = '-'; } else *s = ' '; *++s = temp_data/1000+0x30; temp_data = temp_data%10000; *++s = temp_data/1000+0x30; temp_data = temp_data%10000; *++s = temp_data/100+0x30; temp_data = temp_data%100; *++s = temp_data/10+0x30; temp_data = temp_data%10; *++s = temp_data+0x30; } void Display10BitData(int value) { uchar i; lcd_printf(dis,value); for(i=0;i<6;i++) { SeriPushSend(dis[i]); } } //********************* void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 - q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)* 57.3; } //**************************** void main() { delay(500); init_uart(); InitMPU6050(); delay(150); while(1) { SeriPushSend('A'); SeriPushSend(0x20);SeriPushSend('X'); SeriPushSend(':'); Display10BitData(GetData(ACCEL_XOUT_H)); //??X???? SeriPushSend(0x20);SeriPushSend('Y'); SeriPushSend(':'); Display10BitData(GetData(ACCEL_YOUT_H)); //??Y???? SeriPushSend(0x20);SeriPushSend('Z'); SeriPushSend(':'); Display10BitData(GetData(ACCEL_ZOUT_H)); //??Z???? SeriPushSend(0x20); SeriPushSend('G'); SeriPushSend(0x20);SeriPushSend('X'); SeriPushSend(':'); Display10BitData(GetData(GYRO_XOUT_H)); //??X???? SeriPushSend(0x20);SeriPushSend('Y'); SeriPushSend(':'); Display10BitData(GetData(GYRO_YOUT_H)); //??Y???? SeriPushSend(0x20);SeriPushSend('Z'); SeriPushSend(':'); Display10BitData(GetData(GYRO_ZOUT_H)); //??Z???? SeriPushSend(0x0d); IMUupdate(GetData(GYRO_XOUT_H), GetData(GYRO_YOUT_H), GetData(GYRO_ZOUT_H), GetData(ACCEL_XOUT_H), GetData(ACCEL_YOUT_H), GetData(ACCEL_ZOUT_H)); SeriPushSend(0x20);SeriPushSend(Yaw); SeriPushSend(0x0a);//??,?? delay(2000); } }

ID:574341 · 發表于 2019-7-9 13:17

這個是我的代碼，可以實現。
#include<reg52.h>
#include<math.h>
#include<stdio.h>
#include<intrins.h>

typedef unsigned char  uchar;
typedef unsigned short ushort;
typedef unsigned int uint;

sbit SCL = P1^5;
sbit SDA = P1^4;
#define Kp 100.0f
#define Ki 0.002f
#define halfT 0.001f

uint q0 = 1, q1 = 0, q2 = 0, q3 = 0;
uint exInt = 0, eyInt = 0, ezInt = 0;
uint Yaw;

#define SMPRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRo_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRo_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRo_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B
#define WHO_AM_I 0x75
#define SlaveAddress 0xD0

uchar dis[6];
int dis_data;

void delay(uint k)
{
uint i,j;
for(i=0;i<k;i++)
{
for(j=0;j<121;j++);
}
}

void Delay5us()
{
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
}

void I2C_Start()
{
SCL = 1;
SDA = 1;
Delay5us();
SDA = 0;
Delay5us();
SCL = 0;
}

void I2C_Stop()
{
SDA = 0;
SCL = 1;
Delay5us();
SDA = 1;
Delay5us();
SDA = 0;
}

void I2C_SendACK(bit ack)//發送應答信號
{
SDA = ack;
SCL = 1;
Delay5us();
SCL = 0;
Delay5us();
}

bit I2C_RecvACk()
{
SCL = 1;
Delay5us();
CY = SDA;
SCL = 0;
Delay5us();
return CY;
}

void I2C_SendByte(uchar dat)
{
uchar i;
for(i=0;i<8;i++)
{
dat <<= 1;
SDA = CY;
SCL = 1;
Delay5us();
SCL = 0;
Delay5us();
}
I2C_RecvACk();
}

bit I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1;
for(i=0;i<8;i++)
{
dat <<=1;
SCL = 1;
Delay5us();
dat |= SDA;
SCL = 0;
Delay5us();
}
return dat;
}

void Single_WriteI2C(uchar REG_address,uchar REG_data)
{
I2C_Start();
I2C_SendByte(SlaveAddress);
I2C_SendByte(REG_address);
I2C_SendByte(REG_data);
I2C_Stop();
}

uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start();
I2C_SendByte(SlaveAddress);
I2C_SendByte(REG_Address);
I2C_Start();
I2C_SendByte(SlaveAddress+1);
REG_data = I2C_RecvByte();
I2C_SendACK(1);
I2C_Stop();
return REG_data;
}

void init_uart()
{
TMOD = 0x21;
TH1 = 0xfd;    //f3
TL1 = 0xfd;
SCON = 0x50;
PS = 1;
TR0 = 1;
TR1 = 1;
ET0 = 1;
ES = 1;
EA = 1;
}

void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00);
Single_WriteI2C(SMPRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG,0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}

void SeriPushSend(uchar send_data)
{
SBUF = send_data;
while(!TI);TI = 0;
}

int GetData(uchar REG_Address)
{
uchar H,L;
H  = Single_ReadI2C(REG_Address);
L  = Single_ReadI2C(REG_Address+1);
return ((H<<8)+L);
}

void lcd_printf(uchar *s, int temp_data)
{
if(temp_data<0)
{
temp_data = -temp_data;
*s = '-';
}
else *s = ' ';

*++s = temp_data/1000+0x30;
temp_data = temp_data%10000;

*++s = temp_data/1000+0x30;
temp_data = temp_data%10000;

*++s = temp_data/100+0x30;
temp_data = temp_data%100;

*++s = temp_data/10+0x30;
temp_data = temp_data%10;

*++s = temp_data+0x30;

}

void Display10BitData(int value)
{
uchar i;
lcd_printf(dis,value);
for(i=0;i<6;i++)
{
SeriPushSend(dis[i]);
}
}
//*********************

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;

norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;

vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 - q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);

exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;

gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)* 57.3;
}
//****************************
void main()
{
delay(500);
init_uart();
InitMPU6050();
delay(150);
while(1)
{
SeriPushSend('A');
SeriPushSend(0x20);SeriPushSend('X'); SeriPushSend(':');
Display10BitData(GetData(ACCEL_XOUT_H)); //??X????
SeriPushSend(0x20);SeriPushSend('Y'); SeriPushSend(':');
Display10BitData(GetData(ACCEL_YOUT_H)); //??Y????
SeriPushSend(0x20);SeriPushSend('Z'); SeriPushSend(':');
Display10BitData(GetData(ACCEL_ZOUT_H)); //??Z????
SeriPushSend(0x20);
SeriPushSend('G');
SeriPushSend(0x20);SeriPushSend('X'); SeriPushSend(':');
Display10BitData(GetData(GYRO_XOUT_H)); //??X????
SeriPushSend(0x20);SeriPushSend('Y'); SeriPushSend(':');
Display10BitData(GetData(GYRO_YOUT_H)); //??Y????
SeriPushSend(0x20);SeriPushSend('Z'); SeriPushSend(':');
Display10BitData(GetData(GYRO_ZOUT_H)); //??Z????
SeriPushSend(0x0d);
IMUupdate(GetData(GYRO_XOUT_H), GetData(GYRO_YOUT_H), GetData(GYRO_ZOUT_H), GetData(ACCEL_XOUT_H), GetData(ACCEL_YOUT_H), GetData(ACCEL_ZOUT_H));
SeriPushSend(0x20);SeriPushSend(Yaw);
SeriPushSend(0x0a);//??,??
delay(2000);
}
}

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

MPU6050的DMP,如何根據FIFO的42位數據得到四元數,解出歐拉角？