6050(DMP)C代码,来自网友，试着移植了，不成功.....

blackblue · 发表于 2014-1-20 10:05:49

本帖最后由 blackblue 于 2014-6-8 15:56 编辑

上回说到准备用NXT S4的高速端口,扩展出一个高速率数字陀螺,估计这里有兴趣的人也是有限的,我这里一步步讲一些我的做法和思路;

加顾上一贴的,将NXT S4扩展成TTL,有了TTL,我们可以方便地与其他MCU相连了;

这一贴是讲,使用ATMEL的MCU,打开MPU6050的DMP功能，MPU6050能以400K的速率输出姿态数据（FIFO，一共42个字节）；DMP有个好处，
不需要经过复杂的滤波过程，出来的数据，特别是四位元和YRP数据，是直接可用，而且效果比简化KALMAN滤波的要好和稳定，响应速度很快很快；

下面是代码：有兴趣的可以直接用........

---------------------
/*****************************************************
Date : 2013-7-2
Author :注意，原代码是本地一位朋友给的，这里本来空着，经过核实，原贴来自阿莫论坛，作者见贴子下面的跟贴，感谢他......
Chip type : ATmega16
Program type : Application
AVR Core Clock frequency: 16.000000 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 256
*****************************************************/
#include <mega16.h>
#include <alcd.h>
#include <delay.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <twi.h> /* 从WINAVR目录C:\WinAVR\avr\include\util 中拷贝来，删除了#include <avr/io.h>一句 */
unsigned char dmpdatas[42];
unsigned char i2cw=0xd0;
unsigned char i2cr=0xd1;
float quartf[4]; /* 四元数*/
float gyrof[3]; /* 陀螺仪读数*/
float accelf[3]; /* 加速度值*/
float yprf[3]; /* Euler 角度*/
//以下的 firmware 及 config update 数据来自于 Jeff Jrowberg 公开的程序
flash 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
};
flash unsigned char dmpcfgupddata[239] = {
// 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, 0x01, 0x01,
//原程序中此行代码为
//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
};
void i2cinit(void)
{
// 16 M晶振，400K TWI 速率
TWBR=0x0a;
TWAR=0x00;
TWCR=0x94;
TWSR=0x00;
}
void showfloat(unsigned char x,unsigned char y,float f_2show)
{
unsigned char str2show[10];
lcd_gotoxy(x,y);
ftoa( f_2show,10,str2show);
lcd_puts(str2show);
}
void showlong(unsigned char x,unsigned char y,long l_2show)
{
unsigned char str2show[15];
lcd_gotoxy(x,y);
ltoa( l_2show,str2show);
lcd_puts(str2show);
}
//以下TWI的基本操作函数是从马潮老师的书中复制而来，在此表示谢意
unsigned char i2cstart(void)
{
TWCR=(1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))){};
return 1;
}
void i2cstop(void)
{
TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
}
unsigned char i2cwbyte(unsigned char c)
{
unsigned char ack=1;
TWDR=c;
TWCR=(1<<TWINT)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))){};
if((TWSR&0xF8)!=TW_MT_SLA_ACK)
ack=0;
return ack;
}
unsigned char i2crbyte(unsigned char ack)
{
if(ack)
TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWEA);
else
TWCR=(1<<TWINT)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))){};
return(TWDR);
}
unsigned char i2cread(unsigned char address)
{
unsigned char data;
i2cstart();
i2cwbyte(i2cw);
i2cwbyte(address);
i2cstart();
i2cwbyte(i2cr);
data=i2crbyte(0);
i2cstop();
return data;
}
void i2cwrite(unsigned char address,unsigned char data)
{
i2cstart();
i2cwbyte(i2cw);
i2cwbyte(address);
i2cwbyte(data);
i2cstop();
}
void readdmp(void)
{
unsigned char dmpnum;
i2cstart();
i2cwbyte(i2cw);
i2cwbyte(0x74);
i2cstart();
i2cwbyte(i2cr);
for (dmpnum=0;dmpnum<41;dmpnum++)
{
dmpdatas[dmpnum]=i2crbyte(1);
};
dmpdatas[41]=i2crbyte(0);
i2cstop();
}
long getdmplong(unsigned char address)
{
long dmptempl;
dmptempl = dmpdatas[address];
dmptempl <<= 8;
dmptempl |= dmpdatas[address+1];
dmptempl <<= 8;
dmptempl |= dmpdatas[address+2];
dmptempl <<= 8;
dmptempl |= dmpdatas[address+3];
return dmptempl;
}
void getquaternion(void)
{
quartf[0] = getdmplong(0)/1073741824.0;
quartf[1] = getdmplong(4)/1073741824.0;
quartf[2] = getdmplong(8)/1073741824.0;
quartf[3] = getdmplong(12)/1073741824.0;
}
void getgyro(void)
{
gyrof[0]=getdmplong(16)/16.384;
gyrof[1]=getdmplong(20)/16.384;
gyrof[2]=getdmplong(24)/16.384;
}
void getaccel(void)
{
accelf[0]=getdmplong(28)/32768.0;
accelf[1]=getdmplong(32)/32768.0;
accelf[2]=getdmplong(36)/32768.0;
}
void init6050(void)
{
i2cwrite(0x6b , 0b10000000);
delay_ms(200);
i2cwrite(0x6b , 0b00000001); //No sleep , 3 PLL with X axis gyroscope reference
i2cwrite(0x23 , 0b00000000); //FIFO 11111000 ,temp,gx,gy,gz,accel
i2cwrite(0x19 , 0b00000101); //SMPLRT_DIV,+1 ,要能被 FIFO rate 整除
i2cwrite(0x1a , 0b00000011); //00 111,8k ,other 1K DLPF
i2cwrite(0x1b , 0b00011000); //00011000 GYRO-CFG +-2000d/s
i2cwrite(0x1c , 0b00000000); //ACCEL +-2g/s
}
void loadfirmware(void)
{
unsigned int datanum=0;
unsigned char bank;
unsigned lastbank=1;
for (datanum=0;datanum<1929;datanum++)
{
bank=datanum/256;
if(lastbank!=bank){
i2cwrite(0x6d,bank);
i2cwrite(0x6e,0);
}
i2cwrite(0x6f,dmpmemorydata[datanum]);
lastbank=bank;
};
}
void loadcfgupd(void)
{
unsigned char line;
unsigned char bank;
unsigned char datacounts=0;
unsigned char bytes2write;
unsigned char offset;
unsigned char writingcounts;
for (line=0;line<37;line++)
{
bank=dmpcfgupddata[datacounts];
datacounts++;
offset=dmpcfgupddata[datacounts];
datacounts++;
bytes2write=dmpcfgupddata[datacounts];
i2cwrite(0x6d,bank);
i2cwrite(0x6e,offset);
for (writingcounts=0;writingcounts<bytes2write;writingcounts++)
{
datacounts++;
i2cwrite(0x6f,dmpcfgupddata[datacounts]);
};
datacounts++;
};
}
void getyawpitchroll(void)
{
// Roll = Atan2(2 *(Y * Z + W * X) , W * W -X * X -Y * Y + Z * Z)
yprf[0] = atan2(2 *(quartf[2] * quartf[3] + quartf[0] * quartf[1]) , quartf[0] * quartf[0] -quartf[1] * quartf[1] -quartf[2] * quartf[2] + quartf[3] * quartf[3])*57.3;
// Pitch = asin(-2 * (X * Z - W * Y))
yprf[1]=asin(-2*(quartf[1]*quartf[3]-quartf[0]*quartf[2]))*57.3;
// Yaw = atan2(2 * (X * Y + W * Z) ,W * W + X * X - Y * Y - Z * Z)
yprf[2]=atan2(2*(quartf[1] * quartf[2] + quartf[0] * quartf[3]) , quartf[0] * quartf[0] +quartf[1] * quartf[1] -quartf[2] * quartf[2] - quartf[3] * quartf[3])*57.3;
}
void main(void)
{
unsigned int fifocounts;
// Bit Rate: 400.000 kHz
TWBR=0x0a;
TWAR=0x00;
TWCR=0x94;
TWSR=0x00;
// Alphanumeric LCD initialization
// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTA Bit 2
// RD - PORTA Bit 1
// EN - PORTA Bit 3
// D4 - PORTA Bit 4
// D5 - PORTA Bit 5
// D6 - PORTA Bit 6
// D7 - PORTA Bit 7
// Characters/line: 16
lcd_init(16);
i2cinit();
lcd_clear();
init6050();
loadfirmware();
loadcfgupd();
i2cwrite(0x70 , 0b00000011);
i2cwrite(0x71 , 0b00000000);
i2cwrite(0x6a , 0b11001100);
while(1)
{
do
{
do {
fifocounts=i2cread(0x72);
fifocounts=fifocounts<<8;
fifocounts=fifocounts+i2cread(0x73); //读出FIFO里有多少数据
//showlong(12,3,fifocounts);
}
while (fifocounts<42);
readdmp();
fifocounts=i2cread(0x72);
fifocounts=fifocounts<<8;
fifocounts=fifocounts+i2cread(0x73);
}while(fifocounts>=42);
getquaternion();
//getgyro();
//getaccel();
getyawpitchroll();
showlong(12,0,dmpdatas[0]); //显示一下原始数据，看有没有规律
showlong(12,1,dmpdatas[1]);
showfloat(0,0,yprf[0]); //显示 ROLL PITCH YAW 值
showfloat(0,1,yprf[1]);
showfloat(0,2,yprf[2]);
delay_ms(200);
lcd_clear();
}
}

复制代码

yamank · 发表于 2014-1-20 11:05:44

哇，乐高玩具竟然可以玩的这么深入，还有程序......

方逸帆 · 发表于 2014-1-20 11:22:52

看半天没看懂……

糖伯虎 · 发表于 2014-1-20 12:31:13

我的天，这字典写的，头晕。

BPT_YCC · 发表于 2014-3-8 14:45:56

我在stm8上模仿你的思路写了代码，但是在读取fifo中的数据的时候走不出循环。
do

{

   do {

      fifocounts=i2cread(0x72);

      fifocounts=fifocounts<<8;

      fifocounts=fifocounts+i2cread(0x73); //读出FIFO里有多少数据

      //showlong(12,3,fifocounts);

   }

   while (fifocounts<42);

   readdmp();

fifocounts=i2cread(0x72);

   fifocounts=fifocounts<<8;

   fifocounts=fifocounts+i2cread(0x73);

}while(fifocounts>=42);
自己感觉是fifo的写入速度是比读取的速度还要快。请教楼主如何设置fifo的写入速度，是不是直接和mpu6050的采样速度挂钩的。
i2cwrite(0x70 , 0b00000011);

i2cwrite(0x71 , 0b00000000);

i2cwrite(0x6a , 0b11001100);
这几个寄存器是设置的什么内容，我在官方手册中只看到了0x6a是USER_CTRL，前面两个手册上找不到，官网上下的dmp库中却是有用到前面两个寄存器，但没看明白怎么用的，自己也就不知道该如何设置。
希望楼主不吝赐教

BPT_YCC · 发表于 2014-3-8 14:50:03

还有就是读取加速度accel 以及角速度gyro时，为什么是连读4个字节，看官方库以及手册应该是读取2个字节，只有在读取四元数的时候是4个字节。不知道楼主是不是写错了。因为无论是2字节还是4字节后面main函数中的内容不会受影响。

blackblue · 发表于 2014-3-25 10:55:28

楼上的,给你6050的寄存器资料(其实网上有的)

catluoq · 发表于 2014-5-21 22:07:30

本帖最后由 catluoq 于 2014-5-21 22:20 编辑

很惊奇在这里看到了这个程序，问题是 Author : 怎么是空的，应该是catluoq，就是本人啊，该说明一下，这是在CodeVisionAVR 2.05环境下编译的。另回复5、6楼BPT_YCC，MPU6050 DMP的功能在官方公开手册里是没有的，读取速度其实够快，关于fifo速率，仔细读firmware，config，update库，里面已经有说明，至于gyro,accel数据，要读4字节，没错，原因是，读的是42个字节的DMP包数据，不是正常运行状态的数据，那个当然是2个字节，再重复一遍，DMP功能的参数在官方公开手册里是没有的，不管你英语水平有多高，实际上就是没写。我在ambbs论坛里贴出来的，也有人碰到和你一样的问题，可以参考他们的解决办法，不过我建议还是自己多摸索一下，那才有乐趣。别难为楼主了。阿莫论坛我不会再上了，站长有毛病。

mf1111 · 发表于 2015-1-10 16:03:18

catluoq 发表于 2014-5-21 22:07
很惊奇在这里看到了这个程序，问题是 Author : 怎么是空的，应该是catluoq，就是本人啊，该说明一下，这是 ...

我试着在atmel studio中移植了您的代码，使用atmega16，也是在read fifo的时候有问题，走不出count < 42的循环。。。

账号		自动登录	找回密码
密码			马上注册