查看: 9531|回复: 8
6050(DMP)C代码,来自网友，试着移植了，不成功.....
本帖最后由 blackblue 于
15:56 编辑
上回说到准备用NXT S4的高速端口,扩展出一个高速率数字陀螺,估计这里有兴趣的人也是有限的,我这里一步步讲一些我的做法和思路;
加顾上一贴的,将NXT S4扩展成TTL,有了TTL,我们可以方便地与其他MCU相连了;
这一贴是讲,使用ATMEL的MCU,打开MPU6050的DMP功能，MPU6050能以400K的速率输出姿态数据（FIFO，一共42个字节）；DMP有个好处，
不需要经过复杂的滤波过程，出来的数据，特别是四位元和YRP数据，是直接可用，而且效果比简化KALMAN滤波的要好和稳定，响应速度很快很快；
下面是代码：有兴趣的可以直接用........
---------------------
/*****************************************************
Date& & :
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;
&&
}
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)
{
&&
&&i2cstart();
&&i2cwbyte(i2cw);
&&i2cwbyte(address);
&&i2cstart();
&&i2cwbyte(i2cr);
&&data=i2crbyte(0);
&&i2cstop();
&&
}
void i2cwrite(unsigned char address,unsigned char data)
{
&&i2cstart();
&&i2cwbyte(i2cw);
&&i2cwbyte(address);
&&i2cwbyte(data);
&&i2cstop();
}
void readdmp(void)
{
&&
&&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)
{
&&
&&dmptempl = dmpdatas[address];
&&dmptempl &&= 8;
&&dmptempl |= dmpdatas[address+1];
&&dmptempl &&= 8;
&&dmptempl |= dmpdatas[address+2];
&&dmptempl &&= 8;
&&dmptempl |= dmpdatas[address+3];
&&
}
void getquaternion(void)
{& &
&&quartf[0] = getdmplong(0)/.0;&&
&&quartf[1] = getdmplong(4)/.0;
&&quartf[2] = getdmplong(8)/.0;
&&quartf[3] = getdmplong(12)/.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 , 0b);& && && && && && && && &&&
&&delay_ms(200);
&&i2cwrite(0x6b , 0b);& && && && && && && && &&&//No sleep&&, 3 PLL with X axis gyroscope reference
&&i2cwrite(0x23 , 0b);& && && && && && && && &&&//FIFO
,temp,gx,gy,gz,accel
&&i2cwrite(0x19 , 0b);& && && && && && && && &&&//SMPLRT_DIV,+1 ,要能被 FIFO rate 整除
&&i2cwrite(0x1a , 0b);& && && && && && && && &&&//00 111,8k ,other&&1K&&DLPF
&&i2cwrite(0x1b , 0b);& && && && && && && && &&&//&&GYRO-CFG +-2000d/s
&&i2cwrite(0x1c , 0b);& && && && && && && && &&&//ACCEL&&+-2g/s
void loadfirmware(void)
{
&&unsigned int datanum=0;
&&
&&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=
& & };
}
void loadcfgupd(void)
{
&&
&&
&&unsigned char datacounts=0;
&&unsigned char bytes2
&&
&&unsigne
& &&&
&&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&bytes2writingcounts++)
{
& &&&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)
{
uns
// 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 , 0b);& && && && && && && && &
i2cwrite(0x71 , 0b);& && && && && && && && &
i2cwrite(0x6a , 0b);&&
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();
}
}
复制代码
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
哇，乐高玩具竟然可以玩的这么深入，还有程序......
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
看半天没看懂……
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
我的天，这字典写的，头晕。
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
我在stm8上模仿你的思路写了代码，但是在读取fifo中的数据的时候走不出循环。
& && & 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 , 0b);& && && && && && && && &
i2cwrite(0x71 , 0b);& && && && && && && && &
i2cwrite(0x6a , 0b);&&
这几个寄存器是设置的什么内容，我在官方手册中只看到了0x6a是USER_CTRL，前面两个手册上找不到，官网上下的dmp库中却是有用到前面两个寄存器，但没看明白怎么用的，自己也就不知道该如何设置。
希望楼主不吝赐教
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
还有就是读取加速度accel 以及 角速度gyro时，为什么是连读4个字节，看官方库以及手册应该是读取2个字节，只有在读取四元数的时候是4个字节。不知道楼主是不是写错了。因为无论是2字节还是4字节后面main函数中的内容不会受影响。
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
楼上的,给你6050的寄存器资料(其实网上有的)
10:55 上传
点击文件名下载附件
650.26 KB, 下载次数: 18
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
本帖最后由 catluoq 于
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论坛里贴出来的，也有人碰到和你一样的问题，可以参考他们的解决办法，不过我建议还是自己多摸索一下，那才有乐趣。别难为楼主了。阿莫论坛我不会再上了，站长有毛病。
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
很惊奇在这里看到了这个程序，问题是 Author&&: 怎么是空的，应该是catluoq，就是本人啊，该说明一下，这是 ...
我试着在atmel studio中移植了您的代码，使用atmega16，也是在read fifo的时候有问题，走不出count & 42的循环。。。
如果您觉得我的帖子对您有用，请不吝给我一个“赞”！
Powered by
Template By