文号t64683
6722
2
收藏(2)
转 MPU605 DMP 使用代码。纯C 转
ksws02637852014/04/07航天技术 IP:广东

---------------------
/*****************************************************
日期：2013年7月2日
作者：
芯片类型：ATmega16的
项目类型：应用程序
AVR内核时钟频率：16.000000 MHz的
内存模型：小
外部RAM大小：0
数据堆栈大小：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();
}
}




attachment icon mpu6050DMP.attach 21.33KB ATTACH 47次下载
来自：航空航天 / 航天技术
2
全部 只看作者
已屏蔽 原因：{{ notice.reason }}已屏蔽
屏蔽解除屏蔽编辑详情
{{notice.noticeContent}}
~~空空如也

想参与大家的讨论？现在就 登录 或者 注册。

所属专业
航天技术
所属分类
制导与空间导航
上级专业
航空航天
同级专业
航空技术喷气推进火箭燃料
文件下载
加载中...
{{errorInfo}}
{{downloadWarning}}
你在 {{downloadTime}} 下载过当前文件。
文件名称：{{resource.defaultFile.name}}
下载次数：{{resource.hits}}
上传用户：{{uploader.username}}
所需积分：{{costScores}}，{{holdScores}}下载当前附件免费{{description}}
下载地址：{{l.name}}
积分不足，去充值
文件已丢失
当前账号的附件下载数量限制如下：
时段 个数
{{f.startingTime}}点 - {{f.endTime}}点 {{f.fileCount}}
视频暂不能访问，请登录试试
登录注册
仅供内部学术交流或培训使用，请先保存到本地。本内容不代表科创观点，未经原作者同意，请勿转载。
预览
音频暂不能访问，请登录试试
登录注册
支持的图片格式：jpg, jpeg, png
插入公式
评论控制
加载中...
文号：{{pid}}
投诉或举报
加载中...
{{tip}}
请选择违规类型：
{{reason.type}}

空空如也

提交成功！
加载中...
详情
详情
推送到专栏从专栏移除
设为匿名取消匿名
查看作者
回复
只看作者
加入收藏取消收藏
收藏
取消收藏
折叠回复
置顶取消置顶
评学术分
鼓励
设为精选取消精选
管理提醒
编辑
通过审核
评论控制
退修或删除
历史版本
违规记录
投诉或举报
加入黑名单移除黑名单
查看IP
{{format('YYYY/MM/DD HH:mm:ss', toc)}}