首页 » 单片机 » 正文
单片机

【51单片机快速入门指南】4.3.1： MPU6050调用DMP库获取四元数和欧拉角

普中51-单核-A2
STC89C52
Keil uVision V5.29.0.0
PK51 Prof.Developers Kit Version:9.60.0.0
上位机：Vofa+ 1.3.10

相关介绍

       摘自《ATK-MPU6050六轴传感器模块用户手册_V1.0》

       MPU6050 自带了数字运动处理器，即 DMP，并且，InvenSense 提供了一个 MPU6050 的嵌入式运动驱动库，结合 MPU6050 的 DMP，可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到 yaw、roll 和 pitch。
       使用内置的 DMP，大大简化了四轴的代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。

pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; //俯仰角
roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; //横滚角
yaw   = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; //航向角

       上述计算公式的 57.3 是弧度转换为角度，即 180/π，这样得到的结果就是以度（°）为单位的。

DMP库相关

       作者：ksws0263785大佬 —— 51单片机使用 mpu6050DMP

DMP加载步骤：

在这里插入图片描述

DMP设置数据写入

在这里插入图片描述

更新DMP

在这里插入图片描述

DMP数据包结构

在这里插入图片描述

程序实现

       小改自ksws0263785大佬的51单片机使用 mpu6050DMP，使之调用统一的I2C函数，方便移植到其它单片机。

       stdint.h见【51单片机快速入门指南】1：基础知识和工程创建
       软件I2C程序见【51单片机快速入门指南】4： 软件 I2C
       串口部分见【51单片机快速入门指南】3.3：USART 串口通信
       MPU6050.c、MPU6050.h见【51单片机快速入门指南】4.3： I2C读取MPU6050陀螺仪的原始数据

DMP.c

/*****************************************************
功能:	采集MPU6050 DMP 数据
CPU:	STC89C54RD+
晶震:	11.0592
环境:	Keli2.0
语言: 	c
作者：	XW1005
来源：	移植 Jeff Jrowberg 公开的程序。
目的：
sda 或则scl 为高时是:释放总线
*****************************************************/
#include "./Software_I2C/Software_I2C.h"
#include "MPU6050.h"

float Q[4];	//四元数

uint8_t dmpdatas[42];	//DMP数据

//以下的 firmware 及 config update 数据来自于 Jeff Jrowberg 公开的程序
/* ================================================================================================ *
| Default MotionApps v2.0 42-byte FIFO packet structure:                                           |
|                                                                                                  |
| [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
|   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
|                                                                                                  |
| [GYRO Z][      ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ]                         |
|  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41                          |
* ================================================================================================ */
code uint8_t 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 uint8_t 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 uint8_t 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

};

/*
加载 DMP代码到
返回值  (1=成功,0=失败)
*/
uint8_t loadfirmware(void)
{
	uint16_t datanum = 0;	//DMP固件写入标志位
	uint8_t ye, i/*, j*/;
	uint8_t bank = 0;	//段（256个数据一段）
	uint8_t addr = 0;

	for (; bank<8; bank++)
	{
		if (bank == 7)	//这里的作用就是区分最后一段数据
			i = 8;
		else
			i = 16;
		for (ye = 0; ye<i; ye++)
		{
			MPU_Write_Byte(0x6d, bank);
			MPU_Write_Byte(0x6e, addr);
			MPU_Write_Len(0x6f, 16, dmpmemorydata + datanum);
			datanum += 16;
			addr += 16;
		}
	}
	MPU_Write_Byte(0x6d, 7);
	MPU_Write_Byte(0x6e, addr);
	MPU_Write_Len(0x6f, 9, dmpmemorydata + datanum);
	datanum += 9;
	return 1;
}

uint8_t loadcfgupd(void)	//DMP设置
{
	uint8_t line;	//一共需要写入30条设置数据
	uint8_t bank;	//页
	uint8_t datacounts = 0;	//DMP设置数据标志位
	uint8_t bytes2write;	//数据长度。
	uint8_t offset;	//偏移地址
	uint8_t writingcounts;	//数据写入标志与bytes2write一同使用
	uint8_t special;

	for (line = 0; line<30; line++)
	{
		bank = dmpcfgupddata[datacounts++];
		offset = dmpcfgupddata[datacounts++];
		bytes2write = dmpcfgupddata[datacounts++];
		MPU_Write_Byte(0x6d, bank);
		MPU_Write_Byte(0x6e, offset);
		MPU_Write_Len(0x6f, bytes2write, dmpcfgupddata + datacounts);
		writingcounts = bytes2write;
		datacounts += bytes2write;
		if (0 == bytes2write)
		{
			special = dmpcfgupddata[datacounts++];
			if (0x01 == special)
			{
				//设置零运动中断启用（真）;
				//设置FIFO缓冲区溢出启用（真）;
				//设置DMP启用（真）;
				MPU_Write_Byte(MPU_INT_EN_REG, 0x32);
			}
			else
				return 0;
		}
	}
	return 1;
}

/*最后更新DMP*/
uint8_t xdmpUpdates(uint8_t datacounts)
{
	uint8_t writingcounts, bank, offset, bytes2write;
	bank = dmpUpdates[datacounts++];
	offset = dmpUpdates[datacounts++];
	bytes2write = dmpUpdates[datacounts++];
	MPU_Write_Byte(0x6d, bank);
	MPU_Write_Byte(0x6e, offset);
	MPU_Write_Len(0x6f, bytes2write, dmpUpdates + datacounts);
	writingcounts = bytes2write;
	datacounts += bytes2write;
	return 1;
}

/*读取 FIFO 计数*/
uint16_t getFIFOCount()
{
	uint8_t i[2];
	MPU_Read_Len(MPU_FIFO_CNTH_REG, 2, i);
	return ((i[0] << 8) + i[1]);
}

/*FIFO数据读取
参数 *Data	存储数据的地址
返回值 (1=读取成功,0读取失败)
*/
uint8_t readdmp(uint8_t *Data)
{
	return !MPU_Read_Len(MPU_FIFO_RW_REG, 42, Data);
}

//加载并配置 DMP 数字运动处理引擎
uint8_t dmpInitialize(void)
{
	uint8_t hwRevision, otpValid, mpuIntStatus/*fifoBuffer[128]*/;
	uint8_t xgOffsetTC, ygOffsetTC, zgOffsetTC;
	uint16_t fifoCount;
	MPU_Write_Bit(MPU_PWR_MGMT1_REG, 7, 1);	//复位 MPU6050
	MPU_Delay_Ms(30);
	MPU_Write_Bit(MPU_PWR_MGMT1_REG, 6, 0);	//禁止睡眠模式
	MPU_Write_Byte(0x6D, 0x70);	//写入一个字节数据到0x6d寄存器(选择用户 bank)
	MPU_Write_Byte(0x6E, 0x06);	//写入一个字节数据到0x6e寄存器(选择存储字节)
	MPU_Read_Len(0x6F, 1, &hwRevision);	//读取 
	MPU_Write_Byte(0x6D, 0);	//重置内存 bank 选择
	MPU_Read_Bit(0x00, 0, &otpValid);	//读取 OTP bank 有效标志
	MPU_Read_Bits(0x00, 6, 6, &xgOffsetTC);	//读陀螺偏置TC值 X
	MPU_Read_Bits(0x01, 6, 6, &ygOffsetTC);	//读陀螺偏置TC值 Y)
	MPU_Read_Bits(0x02, 6, 6, &zgOffsetTC);	//读陀螺偏置TC值 Z
	//MPU_Write_Byte(MPU_I2CSLV0_ADDR_REG,0x7f);	//设置从0地址 0x7
	//MPU_Write_Bit(MPU_USER_CTRL_REG,5,0);	//禁用I2C主模式
	//MPU_Write_Byte(MPU_I2CSLV0_ADDR_REG,0x68);	//这里可能要改。还没有弄明白这里
	MPU_Write_Bit(MPU_USER_CTRL_REG, 1, 1);	//I2C总线主控复位
	MPU_Delay_Ms(20);
	if ((loadfirmware()) == 0) 
		return 0;	//加载 
	if ((loadcfgupd()) == 0) 
		return 0;	//配置DMP
	MPU_Write_Bits(MPU_PWR_MGMT1_REG, 2, 3, 0x03);	//设置时钟脉冲源Z陀螺
	MPU_Write_Byte(MPU_INT_EN_REG, 0x12);	//设置DMP和FIFO_OFLOW启用中断
	MPU_Write_Byte(MPU_SAMPLE_RATE_REG, 4);	//设置采样率为200 hz  (1khz / (1 + 4) = 200 Hz)
	MPU_Write_Bits(MPU_CFG_REG, 5, 3, 0x1);	//设置外部帧同步TEMP_OUT_L[0]
	MPU_Write_Bits(MPU_CFG_REG, 2, 3, 0x03);	//设置DLPF带宽42赫兹
	MPU_Write_Bits(MPU_GYRO_CFG_REG, 4, 2, 0x03);	//陀螺灵敏度设置为+ / - 2000 deg/sec
	MPU_Write_Byte(0x70, 0x03);	//设置DMP配置字节（功能未知）
	MPU_Write_Byte(0x71, 0x00);	//设置DMP配置字节（功能未知）
	MPU_Write_Bit(0x00, 0, 0);	//清除OTP Bank 标志
	MPU_Write_Bits(0x00, 6, 6, 0);	//设置X 陀螺抵消TCs之前的值
	MPU_Write_Bits(0x01, 6, 6, 0);	//设置Y 陀螺抵消TCs之前的值
	MPU_Write_Bits(0x02, 6, 6, 0);	//设置Z 陀螺抵消TCs之前的值
	if (xdmpUpdates(0) == 0) 
		return 0;	//最后更新1/7(函数未知)dmpUpdates数组第一行
	if (xdmpUpdates(5) == 0) 
		return 0;	//最后更新2/7(函数未知)dmpUpdates数组第二行
	MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);	//复位 FIFO
	fifoCount = getFIFOCount();	//读取 FIFO 计数
								//readdmp(fifoCount,fifoBuffer);	//读取FIFO里的数据
	MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);	//复位 FIFO

	MPU_Write_Byte(MPU_MOTION_DET_REG, 2);	//运动检测阈值设置为2
	MPU_Write_Byte(0x21, 156);	//零运动检测阈值为156
	MPU_Write_Byte(0x20, 80);	//设置运动检测持续时间至80
	MPU_Write_Byte(0x22, 0);	//设置零运动检测时间0
	MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);	//复 位 FIFO
	MPU_Write_Bit(MPU_USER_CTRL_REG, 6, 1);	//使能 FIFO
	MPU_Write_Bit(MPU_USER_CTRL_REG, 7, 1);	//使能 DMP
	MPU_Write_Bit(MPU_USER_CTRL_REG, 3, 1);	//复位 DMP
	if (xdmpUpdates(12) == 0) 
		return 0; 	//最后更新3/7(函数未知)dmpUpdates数组第三行
	if (xdmpUpdates(17) == 0) 
		return 0;	//最后更新4/7(函数未知)dmpUpdates数组第四行
	if (xdmpUpdates(28) == 0) 
		return 0;	//最后更新5/7(函数未知)dmpUpdates数组第五行
	while ((fifoCount = getFIFOCount()) < 3);	//等待 FIFO 计数 > 2
	MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);			//复位 FIFO
	//readdmp(fifoCount,fifoBuffer);	//读取FIFO里的数据
	MPU_Read_Len(MPU_INT_STA_REG, 1, &mpuIntStatus);		//读取中断状态
	if (xdmpUpdates(35) == 0) 
		return 0;	//最后更新6/7(函数未知)dmpUpdates数组第六行
	while ((fifoCount = getFIFOCount()) < 3);	//等待 FIFO 计数 > 2
	MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);	//复位 FIFO
	//readdmp(fifoCount,fifoBuffer);	//读取FIFO里的数据
	MPU_Read_Len(MPU_INT_STA_REG, 1, &mpuIntStatus);	//读取中断状态
	if (xdmpUpdates(40) == 0) 
		return 0;	//最后更新7/7(函数未知)dmpUpdates数组第七行
	MPU_Write_Bit(MPU_USER_CTRL_REG, 7, 0);	//禁用DMP(稍后您打开它)
	MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);	//复位 FIFO
	MPU_Read_Len(MPU_INT_STA_REG, 1, &mpuIntStatus);
	//星期六 (2014/06/28)
	return 1;
}

/*初始化MPU6050*/
void initMPU6050(void)
{
	MPU_Write_Bits(MPU_PWR_MGMT1_REG, 2, 3, 0x01);	//电源管理
	MPU_Write_Bits(MPU_GYRO_CFG_REG, 4, 2, 0x00);	//设置陀螺仪量程 250/s
	MPU_Write_Bits(MPU_ACCEL_CFG_REG, 4, 2, 0x00);	//设置加速度量程 2G
	MPU_Write_Bit(MPU_PWR_MGMT1_REG, 6, 1);	//电源管理MUP进入睡眠模式
}

/*验证MPU6050连接*/
uint8_t getDeviceID(void)
{
	uint8_t b = 0;	//临时变量
	MPU_Read_Bits(MPU_DEVICE_ID_REG, 6, 6, &b);	//读取i2c固定地址，去掉最高位和最低位这两位数据
	return b == 0x34;	//判断B是否等于0x34，如果等于返回1，不等于返回0(库的是0x38)

}

void MPU6050_DMP_Init(void)
{
	initMPU6050();	//初始化
	if (getDeviceID())	//验证连接是否正常(读取MPU6050的I2C地址)
		while(!(dmpInitialize()));	//加载并配置运动库
	MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);	//复位 FIFO
	MPU_Write_Bit(MPU_USER_CTRL_REG, 7, 1);	//使能DMP
}

void MPU6050_Refresh_DMP(void)
{
	static uint8_t zd;
	static uint16_t i;
	i = getFIFOCount();//读取FIFO计数
	MPU_Read_Len(MPU_INT_STA_REG, 1, &zd);	//读取中断状态
	if ((zd & 0x10) || i >= 840)	//判断FIFO是否溢出
	{
		MPU_Write_Bit(MPU_USER_CTRL_REG, 2, 1);	//复位 FIFO
	}
	else if (zd & 0x02)
	{
		while (i<42) 
			i = getFIFOCount();
		if (readdmp(dmpdatas))	//读取FIFO数据
		{
			Q[0] = ((int16_t)dmpdatas[0] << 8 | dmpdatas[1])/16384.;
			Q[1] = ((int16_t)dmpdatas[4] << 8 | dmpdatas[5])/16384.;
			Q[2] = ((int16_t)dmpdatas[8] << 8 | dmpdatas[9])/16384.;
			Q[3] = ((int16_t)dmpdatas[12] << 8 | dmpdatas[13])/16384.;
		}
	}
}

DMP.h

#ifndef DMP_H_
#define DMP_H_

extern float Q[4];	//四元数
extern uint8_t dmpdatas[42];	//DMP数据

void MPU6050_DMP_Init(void);
void MPU6050_Refresh_DMP(void);

#endif

测试程序

四元数

#include <STC89C5xRC.H>
#include "intrins.h"
#include "stdint.h"
#include "USART.h"
#include "./MPU6050/MPU6050.h"
#include "./MPU6050/DMP.h"

void Delay1ms()		//@11.0592MHz
{
	unsigned char i, j;

	_nop_();
	i = 2;
	j = 199;
	do
	{
		while (--j);
	} while (--i);
}

void Delay_ms(int i)
{
	while(i--)
		Delay1ms();
}

void main(void)
{
	USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 11059200, 57600, DOUBLE_BAUD_ENABLE, USART_TIMER_1); 
	MPU6050_DMP_Init();
	while(1)
	{	
		MPU6050_Refresh_DMP();
		SendFloat(&Q[0]);
		SendFloat(&Q[1]);
		SendFloat(&Q[2]);
		SendFloat(&Q[3]);
		SendEnd();
	}
}

实验现象

内存占用情况：
在这里插入图片描述

上位机为Vofa+ 1.3.10，协议为JustFloat，配置如下
在这里插入图片描述

在这里插入图片描述

欧拉角的获取

由于调用了math.h，生成的程序较大，注释掉大量没用到的程序才勉强跑起来。
内存使用情况：
在这里插入图片描述

void main(void)
{
	float Pitch, Roll, Yaw;
	UartInit();	
	MPU6050_DMP_Init();
	while(1)
	{	
		MPU6050_Refresh_DMP();

		Pitch = asin(-2 * Q[1] * Q[3] + 2 * Q[0]* Q[2])* 57.3;	// pitch
		Roll  = atan2(2 * Q[2] * Q[3] + 2 * Q[0] * Q[1], -2 * Q[1] * Q[1] - 2 * Q[2]* Q[2] + 1)* 57.3;	// roll
		Yaw   = atan2(2 * (Q[1] * Q[2] + Q[0] * Q[3]), Q[0] * Q[0] + Q[1] * Q[1] - Q[2] * Q[2] - Q[3] * Q[3]) * 57.3;	//yaw

		SendFloat(&Pitch);
		SendFloat(&Roll);
		SendFloat(&Yaw);
		SendEnd();
	}
}

在这里插入图片描述

版权声明：本文为CSDN博主「乙酸氧铍」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_44457994/article/details/121522530

标签： · · · · ·
生成海报
点击下载海报
加载中
点赞 0

乙酸氧铍

我还没有学会写个人说明！

暂无评论

发表评论

相关推荐

分享到：