STM32学习心得三十七：MPU6050六轴传感器实验
记录一下，方便以后翻阅~
主要内容：
1）MPU6050简介；
2）MPU6050相关寄存器介绍；
3）相关实验代码解读（除了案例可实现的功能外，主函数增加了在LCD屏上显示6轴的原始数据（含正负号）的功能）。
一、什么是MPU6050?
MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件，内带3轴陀螺仪和3轴加速度传感器，并且含有一个第二IIC接口，可用于连接外部磁力传感器，利用自带数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主IIC接口，可以向应用端输出完整的9轴姿态融合演算数据。
二、MPU6050特点
1）自带数字运动处理（DMP: Digital Motion Processing），输出6轴或9轴（需外接磁传感器）姿态解算数据；
2）集成可程序控制，测量范围为±250、±500、±1000与±2000°/sec 的3轴角速度感测器(陀螺仪)；
3）集成可程序控制，范围为±2g、±4g、±8g和±16g的3轴加速度传感器；
4）自带数字温度传感器；
5）可输出中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G中断、零动作感应、触击感应、摇动感应功能；
6）自带1024字节FIFO，有助于降低系统功耗；
7）高达400Khz的IIC通信接口；
8）超小封装尺寸：4x4x0.9mm（QFN）。
三、MPU6050框图
AD0=0对应器件物理地址=0X68
AD0=1对应器件物理地址=0X69
四、MPU6050初始化
1）初始化IIC接口；
2）复位MPU6050，由电源管理寄存器1(0X6B)控制；
3）设置角速度和加速度传感器的满量程范围，由陀螺仪配置寄存器(0X1B)和加速度传感器配置寄存器(0X1C)设置 ；
4）设置其他参数：
设置中断，由中断使能寄存器(0X38)控制；
设置AUX IIC接口，由户控制寄存器(0X6A)控制；
设置FIFO，由FIFO使能寄存器(0X23)控制；
设置陀螺仪采样率 ，由采样率分频寄存器(0X19)控制；
设置数字低通滤波器，由配置寄存器(0X1A)控制；
5）设置系统时钟。由电源管理寄存器1(0X6B)控制。一般选择x轴陀螺PLL作为时钟源，以获得更高精度的时钟；
6）使能角速度传感器(陀螺仪)和加速度传感器。由电源管理寄存器2(0X6C)控制。
初始化完成后，即可读取陀螺仪、加速度传感器和温度传感器的数据了！！
五、主要的寄存器介绍
5.1）电源管理寄存器1（0X6B）
DEVICE_RESE=1，复位MPU6050，复位完成后，自动清零；
SLEEP=1，进入睡眠模式；SLEEP=0，正常工作模式；
TEMP_DIS，用于设置是否使能温度传感器，设置为0，则使能；
CLKSEL[2:0]，用于选择系统时钟源，如下表所示：
5.2）陀螺仪配置寄存器（0X1B）
主要关注FS_SEL[1:0]这两个位，用于设置陀螺仪的满量程范围：
0，±250°/S；1，±500°/S；2，±1000°/S；3，±2000°/S；
一般设置为3，即±2000°/S，因为陀螺仪的ADC为16位分辨率，所以灵敏度为：65536/4000=16.4LSB/(°/S)。
5.3）加速度传感器配置寄存器（0X1C）
主要关注AFS_SEL[1:0]这两个位，用于设置加速度传感器的满量程范围：
0，±2g；1，±4g；2，±8g；3，±16g；
一般设置为0，即±2g，因为加速度传感器的ADC是16位，所以灵敏度为：65536/4=16384LSB/g。
5.4）FIFO使能寄存器（0X23）
该寄存器用于控制FIFO使能，在简单读取传感器数据的时候，可以不用FIFO，设置对应位为：0，即可禁止FIFO，设置为1，则使能FIFO。
注意：加速度传感器的3个轴，全由1个位（ACCEL_FIFO_EN）控制，只要该位置1，则加速度传感器的三个通道都开启FIFO了。
5.5）陀螺仪采样率分频寄存器（0X19）
该寄存器用于设置MPU6050的陀螺仪采样频率，计算公式为：
采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)
这里陀螺仪的输出频率，是1Khz或者8Khz，与数字低通滤波器（DLPF）的设置有关，当DLPF_CFG=0或7的时候，频率为8Khz，其他情况是1Khz。而且DLPF滤波频率一般设置为采样率的一半。采样率，我们假定设置为50Hz，那么：SMPLRT_DIV=1000/50-1=19。
5.6）配置寄存器（0X1A）
重点看数字低通滤波器（DLPF）的设置位：DLPF_CFG[2:0]，加速度计和陀螺仪，都是根据这三个位的配置进行过滤的，如下表所示：
备注：带宽=1/2采样率
5.7）电源管理寄存器2（0X6C）
该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率；
剩下的6位，分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式；
本例程全部设置为：0 ，即可。
5.8）加速度传感器数据输出寄存器（0X3B~0X40）
总共由6个寄存器组成，输出X/Y/Z三个轴的加速度传感器值，高字节在前，低字节在后。
5.9）陀螺仪数据输出寄存器（0X43~0X48）
总共由6个寄存器组成，输出X/Y/Z三个轴的陀螺仪传感器数据，高字节在前，低字节在后。
5.10）温度传感器数据输出寄存器（0X41~0X42）
通过读取0X41（高8位）和0X42（低8位）寄存器得到，温度换算公式为：
Temperature = 36.53 +regval/340
其中，Temperature为计算得到的温度值，单位为℃，regval为从0X41和0X42读到的温度传感器值。
六、DMP使用介绍
实际使用时（比如做四轴），我们更希望得到姿态数据，即欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。
要利用原始数据，需要进行姿态融合解算。MPU6050自带数字运动处理器，即DMP，并且InvenSense提供了一个MPU6050的嵌入式运动驱动库，结合MPU6050的DMP，可以基于原始数据直接转换成四元数输出，而得到四元数之后，便可很方便的计算出欧拉角，从而得到yaw、roll和pitch。
备注：InvenSense提供的MPU6050运动驱动库是基于MSP430的，需要将其移植后才可以用到STM32上面。
移植细节：
官方DMP驱动库移植，主要是实现这4个函数：i2c_write、i2c_read、delay_ms和get_ms。
移植后的驱动代码如下图：
MPU6050 DMP输出的是姿态解算后的四元数，采用q30格式，即2的30次方，要得到欧拉角，需要做一个转换，代码如下：
q0=quat[0] / q30; // q30格式转换为浮点数
q1=quat[1] / q30; // q30格式转换为浮点数
q2=quat[2] / q30; // q30格式转换为浮点数
q3=quat[3] / q30; // q30格式转换为浮点数
// 计算得到俯仰角/横滚角/航向角
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; // 航向角
上述代码中：quat[0]~quat[3]：是MPU6050的DMP解算后的四元数，q30格式； q30：是一个常量：1073741824，即2的30次方； 57.3：是弧度转换为角度，即180/π，这样结果就是以度（°）为单位的。
七、ATK-MPU6050模块
ATK-MPU6050模块主要由MPU-6050芯片和RT9193-33芯片组成，原理图如下图所示：
开发板通过ATK-MODULE接口外接MPU6050模块，硬件连接原理图如下：
八、重点关注的相关代码
8.1）MPU-6050驱动代码
1，MPU6050 IIC接口驱动代码
2，MPU_Init函数
3，MPU_Get_Gyroscope函数
4，MPU_Get_Accelerometer函数
5，MPU_Get_Temperature函数
8.2）DMP驱动代码
1，DMP移植相关代码
i2c_write、i2c_read、delay_ms和get_ms.
2， mpu_dmp_init函数
3， mpu_dmp_get_data函数
九、MPU6050读写时序
9.1）单字节写入时序
9.2）连续写入时序
9.3）单字节读出时序
9.4）连续读出时序
十、部分实验代码解读
10.1 mpuiic.h头文件函数
/**
******************************** STM32F10x *********************************
* @文件名称： mpuiic.h
* @修改作者： Aaron
* @库版本号： V3.5.0
* @工程版本： V1.0.0
* @开发日期： 2020年11月30日
* @摘要简述： mpuiic头文件,主要跟MPU-6050模块进行IIC通讯时用
******************************************************************************/
/*-----------------------------------------------------------------------------
* @更新日志：
* @无
* ---------------------------------------------------------------------------*/
#ifndef __MPUIIC_H
#define __MPUIIC_H
/* 包含的头文件 --------------------------------------------------------------*/
#include "sys.h"
#include "stm32f10x.h"
/* 寄存器方法控制GPIO管脚输入输出模式 ----------------------------------------*/
#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;} // PB11 上拉输入模式
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;} // PB11 通用推挽输出模式，50MHz
/* 位操作方法定义 ------------------------------------------------------------*/
#define MPU_IIC_SCL PBout(10) // PB10对应SCL
#define MPU_IIC_SDA PBout(11) // PB11对应SDA，输出SDA
#define MPU_READ_SDA PBin(11) // PB11对应SDA，输入SDA
/* 定义IIC底层驱动程序 -------------------------------------------------------*/
void MPU_IIC_Delay(void); // MPU IIC延时函数，延时 2 us
void MPU_IIC_Init(void); // 初始化IIC的IO口
void MPU_IIC_Start(void); // 发送IIC开始信号
void MPU_IIC_Stop(void); // 发送IIC停止信号
u8 MPU_IIC_Wait_Ack(void); // IIC等待ACK信号
void MPU_IIC_Ack(void); // IIC发送ACK信号
void MPU_IIC_NAck(void); // IIC不发送ACK信号
void MPU_IIC_Send_Byte(u8 txd); // IIC发送一个字节
u8 MPU_IIC_Read_Byte(unsigned char ack); // IIC读取一个字节
#endif /* __MPUIIC_H */
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
10.2 mpuiic.c源文件函数
/**
******************************** STM32F10x *********************************
* @文件名称： mpuiic.c
* @修改作者： Aaron
* @库版本号： V3.5.0
* @工程版本： V1.0.0
* @开发日期： 2020年11月30日
* @摘要简述： mpuiic源文件，主要包含与MPU-6050进行IIC通讯的IIC底层驱动程序
******************************************************************************/
/*-----------------------------------------------------------------------------
* @更新日志：
* @无
* ---------------------------------------------------------------------------*/
/* 包含的头文件 --------------------------------------------------------------*/
#include "mpuiic.h"
#include "delay.h"
/*********************************************************************
函数名称：MPU_IIC_Delay()
函数功能：延时函数，延时2微秒（us）
入口参数：无
返回参数：无
修改开发：Aaron
**********************************************************************/
void MPU_IIC_Delay(void)
{
delay_us(2);
}
/*********************************************************************
函数名称：MPU_IIC_Init()
函数功能：MPU_IIC管脚初始化配置
入口参数：无
返回参数：无
修改作者：Aaron
**********************************************************************/
void MPU_IIC_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE); // 使能外设IO PORTB时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; // 端口配置，管脚10和管脚11
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // IO口速度为50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11); // PB10(SCL),PB11（SDA） 输出高
}
/*********************************************************************
函数名称：MPU_IIC_Start()
函数功能：MPU_IIC起始信号
入口参数：无
返回参数：无
修改作者：Aaron
**********************************************************************/
void MPU_IIC_Start(void)
{
MPU_SDA_OUT(); // SDA线输出模式
MPU_IIC_SDA=1; // SDA输出高
MPU_IIC_SCL=1; // SCL输出高，此时为空闲状态
MPU_IIC_Delay(); // 延迟 2 us
MPU_IIC_SDA=0; // 在SCL输出高的状态下，SDA由高至低跳变，IIC开始
MPU_IIC_Delay(); // 延迟 2 us
MPU_IIC_SCL=0; // 钳住I2C总线，准备发送或接收数据
}
/*********************************************************************
函数名称：MPU_IIC_Stop()
函数功能：MPU_IIC停止信号
入口参数：无
返回参数：无
修改作者：Aaron
**********************************************************************/
void MPU_IIC_Stop(void)
{
MPU_SDA_OUT(); // SDA线输出模式
MPU_IIC_SCL=0; // SCL输出低
MPU_IIC_SDA=0; // SDA输出低
MPU_IIC_Delay(); // 延迟 2 us
MPU_IIC_SCL=1; // 先SCL输出高，在SCL输出高的状态下，SDA一直是低电平
MPU_IIC_SDA=1; // SDA输出高，发送I2C总线结束信号
MPU_IIC_Delay(); // 延迟 2 us
}
/*********************************************************************
函数名称：u8 MPU_IIC_Wait_Ack()
函数功能：等待应答信号到来
入口参数：无
返回参数：u8，1，接收应答失败，0，接收应答成功
修改作者：Aaron
**********************************************************************/
u8 MPU_IIC_Wait_Ack(void)
{
u8 ucErrTime=0;
MPU_SDA_IN(); // SDA设置为输入
MPU_IIC_SDA=1; // SDA设高电平
MPU_IIC_Delay(); // 延迟 2 us
MPU_IIC_SCL=1; // SCL设高电平
MPU_IIC_Delay(); // 延迟 2 us
while(MPU_READ_SDA) // 读取PB11的值，一直读PB11=1，高电平，则不断循环，直到溢出
{
ucErrTime++;
if(ucErrTime>250)
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_SCL=0; // SCL设低电平
return 0;
}
/*********************************************************************
函数名称：MPU_IIC_Ack()
函数功能：产生ACK应答
入口参数：无
返回参数：无
修改作者：Aaron
**********************************************************************/
void MPU_IIC_Ack(void)
{
MPU_IIC_SCL=0; // SCL设低电平
MPU_SDA_OUT(); // SDA输出模式
MPU_IIC_SDA=0; // SDA设低电平
MPU_IIC_Delay(); // 延时 2 us
MPU_IIC_SCL=1; // SCL设高电平
MPU_IIC_Delay(); // 延时 2 us
MPU_IIC_SCL=0; // SCL设第电平，在SCL高电平期间，SDA为低电平，则说明有效应答
}
/*********************************************************************
函数名称：MPU_IIC_NAck()
函数功能：不产生ACK应答
入口参数：无
返回参数：无
修改作者：Aaron
**********************************************************************/
void MPU_IIC_NAck(void)
{
MPU_IIC_SCL=0; // SCL设低电平
MPU_SDA_OUT(); // SDA输出模式
MPU_IIC_SDA=1; // SDA设高电平
MPU_IIC_Delay(); // 延时 2 us
MPU_IIC_SCL=1; // SCL设高电平
MPU_IIC_Delay(); // 延时 2 us
MPU_IIC_SCL=0; // SCL设低电平，在SCL高电平期间，SDA为高电平，则说明无效应答
}
/*********************************************************************
函数名称：MPU_IIC_Send_Byte(u8 txd)
函数功能：IIC发送一个字节
入口参数：u8 txd
返回参数：无
修改作者：Aaron
**********************************************************************/
void MPU_IIC_Send_Byte(u8 txd)
{
u8 t;
MPU_SDA_OUT(); // SDA输出模式
MPU_IIC_SCL=0; // SCL设低电平，此时可改变SDA，以实现数据的有效传输
for(t=0;t<8;t++) // 发送一个字节，即8个bit
{
MPU_IIC_SDA=(txd&0x80)>>7; // 获取u8 txd的最高位（0或1），并右移7位，将最高位值给MPU_IIC_SDA
txd<<=1;
MPU_IIC_SCL=1; // SCL设高电平
MPU_IIC_Delay(); // 延时 2 us
MPU_IIC_SCL=0; // SCL设低电平，传输SDA完成
MPU_IIC_Delay(); // 延时 2us
}
}
/*********************************************************************
函数名称：u8 MPU_IIC_Read_Byte(unsigned char ack)
函数功能：IIC读取一个字节，ack=1时，发送ACK，ack=0，发送nACK
入口参数：ack
返回参数：u8，
修改作者：Aaron
**********************************************************************/
u8 MPU_IIC_Read_Byte(unsigned char ack)
{
unsigned char i,receive=0;
MPU_SDA_IN(); // SDA设置为输入模式
for(i=0;i<8;i++ ) // 读取一个字节，即8个bit
{
MPU_IIC_SCL=0; // SCL设为低电平
MPU_IIC_Delay(); // 延时 2us
MPU_IIC_SCL=1; // SCL设为高电平
receive<<=1;
if(MPU_READ_SDA) // 如果读取的SDA为1，则receive最低值+1，若读取的SDA为0，则receive最低值不变，即0
receive++;
MPU_IIC_Delay(); // 延时 2us
}
if (!ack)
MPU_IIC_NAck(); // 发送nACK
else
MPU_IIC_Ack(); // 发送ACK
return receive;
}
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
10.3 mp6050.h头文件函数
代码中标记“初学”的说明是前面寄存器介绍时讲到的。
/**
******************************** STM32F10x *********************************
* @文件名称： mpu6050.h
* @修改作者： Aaron
* @库版本号： V3.5.0
* @工程版本： V1.0.0
* @开发日期： 2020年11月30日
* @摘要简述： mpu6050头文件
******************************************************************************/
/*-----------------------------------------------------------------------------
* @更新日志：
* @无
* ---------------------------------------------------------------------------*/
#ifndef __MPU6050_H
#define __MPU6050_H
/* 包含的头文件 --------------------------------------------------------------*/
#include "mpuiic.h"
/* 位操作控制PA15,对应MPU6050 AD0的高低电平 ----------------------------------*/
#define MPU_AD0_CTRL PAout(15) // 控制AD0电平,从而控制MPU-6050地址
/* MPU6050 寄存器ID定义 ------------------------------------------------------*/
// #define MPU_ACCEL_OFFS_REG 0X06 // accel_offs寄存器,可读取版本号,寄存器手册未提到
// #define MPU_PROD_ID_REG 0X0C // prod id寄存器,在寄存器手册未提到
#define MPU_SELF_TESTX_REG 0X0D // 自检寄存器X ACC和G
#define MPU_SELF_TESTY_REG 0X0E // 自检寄存器Y ACC和G
#define MPU_SELF_TESTZ_REG 0X0F // 自检寄存器Z ACC和G
#define MPU_SELF_TESTA_REG 0X10 // 自检寄存器X,Y,Z only ACC
#define MPU_SAMPLE_RATE_REG 0X19 // 陀螺仪采样频率分频器 **初学**
#define MPU_CFG_REG 0X1A // 配置寄存器 **初学**
#define MPU_GYRO_CFG_REG 0X1B // 陀螺仪配置寄存器 **初学**
#define MPU_ACCEL_CFG_REG 0X1C // 加速度计配置寄存器 **初学**
#define MPU_MOTION_DET_REG 0X1F // 运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG 0X23 // FIFO使能寄存器 **初学**
#define MPU_I2CMST_CTRL_REG 0X24 // IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG 0X25 // IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG 0X26 // IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG 0X27 // IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG 0X28 // IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG 0X29 // IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG 0X2A // IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG 0X2B // IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG 0X2C // IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG 0X2D // IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG 0X2E // IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG 0X2F // IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG 0X30 // IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG 0X31 // IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG 0X32 // IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG 0X33 // IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG 0X34 // IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG 0X35 // IIC从机4读数据寄存器
#define MPU_I2CMST_STA_REG 0X36 // IIC主机状态寄存器
#define MPU_INTBP_CFG_REG 0X37 // 中断/旁路设置寄存器
#define MPU_INT_EN_REG 0X38 // 中断使能寄存器
#define MPU_INT_STA_REG 0X3A // 中断状态寄存器
#define MPU_ACCEL_XOUTH_REG 0X3B // 加速度值,X轴高8位寄存器 **初学**
#define MPU_ACCEL_XOUTL_REG 0X3C // 加速度值,X轴低8位寄存器 **初学**
#define MPU_ACCEL_YOUTH_REG 0X3D // 加速度值,Y轴高8位寄存器 **初学**
#define MPU_ACCEL_YOUTL_REG 0X3E // 加速度值,Y轴低8位寄存器 **初学**
#define MPU_ACCEL_ZOUTH_REG 0X3F // 加速度值,Z轴高8位寄存器 **初学**
#define MPU_ACCEL_ZOUTL_REG 0X40 // 加速度值,Z轴低8位寄存器 **初学**
#define MPU_TEMP_OUTH_REG 0X41 // 温度值高八位寄存器 **初学**
#define MPU_TEMP_OUTL_REG 0X42 // 温度值低八位寄存器 **初学**
#define MPU_GYRO_XOUTH_REG 0X43 // 陀螺仪值,X轴高8位寄存器 **初学**
#define MPU_GYRO_XOUTL_REG 0X44 // 陀螺仪值,X轴低8位寄存器 **初学**
#define MPU_GYRO_YOUTH_REG 0X45 // 陀螺仪值,Y轴高8位寄存器 **初学**
#define MPU_GYRO_YOUTL_REG 0X46 // 陀螺仪值,Y轴低8位寄存器 **初学**
#define MPU_GYRO_ZOUTH_REG 0X47 // 陀螺仪值,Z轴高8位寄存器 **初学**
#define MPU_GYRO_ZOUTL_REG 0X48 // 陀螺仪值,Z轴低8位寄存器 **初学**
#define MPU_I2CSLV0_DO_REG 0X63 // IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG 0X64 // IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG 0X65 // IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG 0X66 // IIC从机3数据寄存器
#define MPU_I2CMST_DELAY_REG 0X67 // IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG 0X68 // 信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG 0X69 // 运动检测控制寄存器
#define MPU_USER_CTRL_REG 0X6A // 用户控制寄存器
#define MPU_PWR_MGMT1_REG 0X6B // 电源管理寄存器1 **初学**
#define MPU_PWR_MGMT2_REG 0X6C // 电源管理寄存器2 **初学**
#define MPU_FIFO_CNTH_REG 0X72 // FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG 0X73 // FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG 0X74 // FIFO读写寄存器
#define MPU_DEVICE_ID_REG 0X75 // 器件ID寄存器
// 备注：如果AD0脚(9脚)接地,0,IIC地址为0X68(不包含最低位)；
// 如果接V3.3,则IIC地址为0X69(不包含最低位)，
// 本开发板由PA15管脚控制，另PA15为低电平
#define MPU_ADDR 0X68
/* 函数申明 -----------------------------------------------------*/
u8 MPU_Init(void); // 初始化MPU6050
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf); // IIC连续写
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); // IIC连续读
u8 MPU_Write_Byte(u8 reg,u8 data); // IIC写一个字节
u8 MPU_Read_Byte(u8 reg); // IIC读一个字节
u8 MPU_Set_Gyro_Fsr(u8 fsr);
u8 MPU_Set_Accel_Fsr(u8 fsr);
u8 MPU_Set_LPF(u16 lpf);
u8 MPU_Set_Rate(u16 rate);
u8 MPU_Set_Fifo(u8 sens);
short MPU_Get_Temperature(void);
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
#endif /* __MPU6050_H */
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
10.4 mpu6050.c源文件函数
/**
******************************** STM32F10x *********************************
* @文件名称： mpu6050.c
* @修改作者： Aaron
* @库版本号： V3.5.0
* @工程版本： V1.0.0
* @开发日期： 2020年11月30日
* @摘要简述： mpu6050源文件
******************************************************************************/
/*-----------------------------------------------------------------------------
* @更新日志：
* @无
* ---------------------------------------------------------------------------*/
/* 包含的头文件 --------------------------------------------------------------*/
#include "mpu6050.h"
#include "delay.h"
/*********************************************************************
函数名称：u8 MPU_Init()
函数功能：初始化MPU6050
入口参数：无
返回参数：0，成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Init(void)
{
u8 res;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE); // 使能AFIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); // 使能外设IO PORTA时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; // 管脚15
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // IO口速度为50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); // 根据设定参数初始化PA15,对应MPU-6050芯片的AD0地址管脚
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); // 禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO!!!
MPU_AD0_CTRL=0; // 控制MPU6050的AD0脚为低电平,则从机地址为:0X68
MPU_IIC_Init(); // 初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); // 对电源管理寄存器1输入0x80，复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); // 对电源管理寄存器1输入0x00，主要令[6]位设0，正常工作模式，即唤醒MPU6050
MPU_Set_Gyro_Fsr(3); // 设置陀螺仪传感器满量程范围,±2000dps
MPU_Set_Accel_Fsr(0); // 设置加速度传感器满量程范围,±2g
MPU_Set_Rate(50); // 设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); // 对中断使能寄存器输入0x00,关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); // 对用户控制寄存器输入0x00,I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); // 对FIFO使能寄存器输入0x00,关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); // 中断/旁路设置寄存器输入0x80,INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG); // 读取器件ID寄存器值，传至res里
if(res==MPU_ADDR) // 如果res=0x68，则器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); // 对电源管理寄存器1输入0x01，设置CLKSEL,001，PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); // 对电源管理寄存器2输入0x00，加速度与陀螺仪都工作（都不待机）
MPU_Set_Rate(50); // 设置采样率为50Hz
}
else
return 1;
return 0;
}
/*********************************************************************
函数名称：u8 MPU_Set_Gyro_Fsr(u8 fsr)
函数功能：设置MPU6050陀螺仪传感器满量程范围
入口参数：u8 fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
返回参数：0，成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3); // 设置陀螺仪满量程范围为±2000dps
}
/*********************************************************************
函数名称：u8 MPU_Set_Accel_Fsr(u8 fsr)
函数功能：设置MPU6050加速度传感器满量程范围
入口参数：fsr:0,±2g;1,±4g;2,±8g;3,±16g
返回参数：0，成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3); // 设置加速度传感器满量程范围为±2g
}
/*********************************************************************
函数名称：u8 MPU_Set_LPF(u16 lpf)
函数功能：设置MPU6050的数字低通滤波器
入口参数：lpf:数字低通滤波频率(Hz)
返回参数：0，成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188) // 如果lpf≥188Hz，则data=1
data=1;
else if(lpf>=98)
data=2;
else if(lpf>=42)
data=3;
else if(lpf>=20)
data=4;
else if(lpf>=10)
data=5;
else
data=6;
return MPU_Write_Byte(MPU_CFG_REG,data); // 设置数字低通滤波器
}
/*********************************************************************
函数名称：u8 MPU_Set_Rate(u16 rate)
函数功能：设置MPU6050的陀螺仪采样率(只在陀螺仪输出频率Fs=1KHz时成立)
入口参数：rate:4~1000(Hz)，陀螺仪采样频率
返回参数：0，成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Set_Rate(u16 rate)
{
u8 data;
if(rate>1000)
rate=1000;
if(rate<4)
rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);
return MPU_Set_LPF(rate/2); // 自动设置LPF为采样率的一半
}
/*********************************************************************
函数名称：short MPU_Get_Temperature()
函数功能：得到温度值
入口参数：rate:4~1000(Hz)，陀螺仪采样频率
返回参数：温度值(扩大了100倍)
修改作者：Aaron
**********************************************************************/
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); // addr:器件地址；reg:寄存器地址；len:写入长度；buf:数据区
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return
temp*100;;
}
/*********************************************************************
函数名称：u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
函数功能：得到陀螺仪值(原始值)
入口参数：gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
返回参数：u8 0成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
/*********************************************************************
函数名称：u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
函数功能：得到加速度值(原始值)
入口参数：gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
返回参数：u8 0成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
/*********************************************************************
函数名称：u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
函数功能：向MPU6050连续写入数据
入口参数：addr:器件地址；reg:寄存器地址；len:写入长度；buf:数据区
返回参数：u8 0成功，1失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0); // 发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) // 等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); // 写寄存器地址
MPU_IIC_Wait_Ack(); // 等待应答
for(i=0;i<len;i++)
{
MPU_IIC_Send_Byte(buf[i]); // 发送数据
if(MPU_IIC_Wait_Ack()) // 等待应答
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
/*********************************************************************
函数名称：u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
函数功能：从MPU6050连续读数据
入口参数：addr:器件地址；reg:要读取的寄存器地址；len:要读取的长度；buf:读取到的数据存储区
返回参数：u8 0正常，1错误
修改作者：Aaron
**********************************************************************/
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0); // 发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) // 等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); // 写寄存器地址
MPU_IIC_Wait_Ack(); // 等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1); // 发送器件地址+读命令
MPU_IIC_Wait_Ack(); // 等待应答
while(len)
{
if(len==1)*buf=MPU_IIC_Read_Byte(0); // 读数据,发送nACK
else *buf=MPU_IIC_Read_Byte(1); // 读数据,发送ACK
len--;
buf++;
}
MPU_IIC_Stop(); // 产生一个停止条件
return 0;
}
/*********************************************************************
函数名称：u8 MPU_Write_Byte(u8 reg,u8 data)
函数功能：向MPU6050写一个字节
入口参数：reg:寄存器地址；data:数据
返回参数：0，正常；1，失败
修改作者：Aaron
**********************************************************************/
u8 MPU_Write_Byte(u8 reg,u8 data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0); // 发送器件地址+写命令
if(MPU_IIC_Wait_Ack()) // 等待应答，0接收应答成功，1接收应答失败
{
MPU_IIC_Stop(); // MPU_IIC停止信号
return 1;
}
MPU_IIC_Send_Byte(reg); // 写寄存器地址
MPU_IIC_Wait_Ack(); // 等待应答
MPU_IIC_Send_Byte(data); // 发送数据
if(MPU_IIC_Wait_Ack()) // 等待应答
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
/*********************************************************************
函数名称：u8 MPU_Read_Byte(u8 reg)
函数功能：从MPU6050读一个字节
入口参数：reg:寄存器地址
返回参数：u8 读到的数据
修改作者：Aaron
**********************************************************************/
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
MPU_IIC_Start();
// 开始标志（S）发出后，主设备会传送一个 7 位的 Slave 地址，并且后面跟着一个第 8 位，称为 Read/Write 位。 0写，1读。
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0); // 发送器件地址+写命令
MPU_IIC_Wait_Ack(); // 等待应答
MPU_IIC_Send_Byte(reg); // 写入要读取字节的寄存器首地址
MPU_IIC_Wait_Ack(); // 等待应答
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1); // 发送器件地址+读命令
MPU_IIC_Wait_Ack(); // 等待应答
res=MPU_IIC_Read_Byte(0); // 读取一个字节的数据,发送nACK
MPU_IIC_Stop(); // 产生一个停止条件
return res;
}
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
10.5 main.c主函数
/**
******************************** STM32F10x *********************************
* @文件名称： main.c
* @修改作者： Aaron
* @库版本号： V3.5.0
* @工程版本： V1.0.0
* @开发日期： 2020年11月30日
* @摘要简述： 主函数，增加了在LCD显示6轴的原始数据
******************************************************************************/
/*-----------------------------------------------------------------------------
* @更新日志：
* @无
* ---------------------------------------------------------------------------*/
/* 包含的头文件 --------------------------------------------------------------*/
#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "lcd.h"
#include "serial_communication.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
/*********************************************************************
函数名称：usart1_send_char(u8 c)
函数功能：串口1发送1个字符
入口参数：c:要发送的字符
返回参数：0，成功，1失败
修改作者：Aaron
**********************************************************************/
void usart1_send_char(u8 c)
{
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); // 循环发送,直到发送完毕
USART_SendData(USART1,c);
}
/*********************************************************************
函数名称：usart1_niming_report(u8 fun,u8*data,u8 len)
函数功能：传送数据给匿名四轴上位机软件
入口参数：fun:功能字.0XA0~0XAF；data:数据缓存区,最多28字节；len:data区有效数据个数
返回参数：无
修改作者：Aaron
**********************************************************************/
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
u8 send_buf[32];
u8 i;
if(len>28)return; // 最多28字节数据
send_buf[len+3]=0; // 校验数置零
send_buf[0]=0X88; // 帧头
send_buf[1]=fun; // 功能字
send_buf[2]=len; // 数据长度
for(i=0;i<len;i++)send_buf[3+i]=data[i]; // 复制数据
for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; // 计算校验和
for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); // 发送数据到串口1
}
/*********************************************************************
函数名称：mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
函数功能：发送加速度传感器数据和陀螺仪数据
入口参数：aacx,aacy,aacz:x,y,z三个方向上面的加速度值；gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
返回参数：无
修改作者：Aaron
**********************************************************************/
void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
{
u8 tbuf[12];
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
usart1_niming_report(0XA1,tbuf,12); // 自定义帧,0XA1
}
/*********************************************************************
函数名称：mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
函数功能：通过串口1上报结算后的姿态数据给电脑
入口参数：aacx,aacy,aacz:x,y,z三个方向上面的加速度值；gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值；
roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度；
pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度
yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度
返回参数：无
修改作者：Aaron
**********************************************************************/
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
{
u8 tbuf[28];
u8 i;
for(i=0;i<28;i++)tbuf[i]=0; // 清0
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
tbuf[18]=(roll>>8)&0XFF;
tbuf[19]=roll&0XFF;
tbuf[20]=(pitch>>8)&0XFF;
tbuf[21]=pitch&0XFF;
tbuf[22]=(yaw>>8)&0XFF;
tbuf[23]=yaw&0XFF;
usart1_niming_report(0XAF,tbuf,28); // 飞控显示帧,0XAF
}
/*********************************************************************
函数名称：int main()
函数功能：主函数,增加了在LCD屏上显示6轴原始数据的功能
入口参数：无
返回参数：无
修改作者：Aaron
**********************************************************************/
int main(void)
{
u8 t=0,report=1; // 默认开启上报，发给匿名四轴上位机软件
u8 key;
float pitch,roll,yaw; // 欧拉角
short aacx,aacy,aacz; // 加速度传感器原始数据
short gyrox,gyroy,gyroz; // 陀螺仪原始数据
short temp; // 温度
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 设置NVIC中断分组2:2位抢占优先级，2位响应优先级
My_USART1_Init(); // 串口初始化为500000
delay_init(); // 延时初始化
LED_Init(); // 初始化与LED连接的硬件接口
KEY_Init(); // 初始化按键
LCD_Init(); // 初始化LCD
MPU_Init(); // 初始化MPU6050
POINT_COLOR=RED; // 设置字体为红色
LCD_ShowString(30,70,200,16,16,"MPU6050 TEST");
LCD_ShowString(30,90,200,16,16,"2020/11/30");
while(mpu_dmp_init())
{
LCD_ShowString(30,110,200,16,16,"MPU6050 Error");
delay_ms(200);
LCD_Fill(30,110,239,130+16,WHITE);
delay_ms(200);
}
LCD_ShowString(30,110,200,16,16,"MPU6050 OK");
LCD_ShowString(30,130,200,16,16,"KEY0:UPLOAD ON/OFF");
POINT_COLOR=BLUE; // 设置字体为蓝色
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
LCD_ShowString(30,200,200,16,16," Temp: . C");
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
LCD_ShowString(30,240,200,16,16," Roll: . C");
LCD_ShowString(30,260,200,16,16," Yaw : . C");
while(1)
{
key=KEY_Scan(0);
if(key==KEY0_PRES)
{
report=!report;
if(report)
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
else
LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
}
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); // 得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); // 得到陀螺仪数据
if(report)
mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz); // 用自定义帧发送加速度和陀螺仪原始数据
if(report)
usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
LCD_ShowString(30,300,200,16,16," aacx: ");
if(aacx<0)
{
LCD_ShowChar(30+48,300,'-',16,0); // 显示负号
aacx=-aacx; // 转为正数
}
else
LCD_ShowChar(30+48,300,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,300,aacx,10,16); // 显示aacx
LCD_ShowString(30,320,200,16,16," aacy: ");
if(aacy<0)
{
LCD_ShowChar(30+48,320,'-',16,0); // 显示负号
aacy=-aacy; // 转为正数
}
else
LCD_ShowChar(30+48,320,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,320,aacy,10,16); // 显示aacy
LCD_ShowString(30,340,200,16,16," aacz: ");
if(aacz<0)
{
LCD_ShowChar(30+48,340,'-',16,0); // 显示负号
aacz=-aacz; // 转为正数
}
else
LCD_ShowChar(30+48,340,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,340,aacz,10,16); // 显示aacz
LCD_ShowString(30,360,200,16,16,"gyrox: ");
if(gyrox<0)
{
LCD_ShowChar(30+48,360,'-',16,0); // 显示负号
gyrox=-gyrox; // 转为正数
}
else
LCD_ShowChar(30+48,360,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,360,gyrox,10,16); // 显示gyrox
LCD_ShowString(30,380,200,16,16,"gyroy: ");
if(gyroy<0)
{
LCD_ShowChar(30+48,380,'-',16,0); // 显示负号
gyroy=-gyroy; // 转为正数
}
else
LCD_ShowChar(30+48,380,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,380,gyroy,10,16); // 显示gyroy
LCD_ShowString(30,400,200,16,16,"gyroz: ");
if(gyroz<0)
{
LCD_ShowChar(30+48,400,'-',16,0); // 显示负号
gyroz=-gyroz; // 转为正数
}
else
LCD_ShowChar(30+48,400,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,400,gyroz,10,16); // 显示gyroz
if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0); // 显示负号
temp=-temp; // 转为正数
}
else
LCD_ShowChar(30+48,200,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,200,temp/100,3,16); // 显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16); // 显示小数部分
temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0); // 显示负号
temp=-temp; // 转为正数
}
else
LCD_ShowChar(30+48,220,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,220,temp/10,3,16); // 显示整数部分
LCD_ShowNum(30+48+40,220,temp%10,1,16); // 显示小数部分
temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0); // 显示负号
temp=-temp; // 转为正数
}
else
LCD_ShowChar(30+48,240,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,240,temp/10,3,16); // 显示整数部分
LCD_ShowNum(30+48+40,240,temp%10,1,16); // 显示小数部分
temp=yaw*10;
printf("temp=%d\r\n",temp);
if(temp<0)
{
LCD_ShowChar(30+48,260,'-',16,0); // 显示负号
temp=-temp; // 转为正数
}
else
LCD_ShowChar(30+48,260,' ',16,0); // 去掉负号
LCD_ShowNum(30+48+8,260,temp/10,3,16); // 显示整数部分
LCD_ShowNum(30+48+40,260,temp%10,1,16); // 显示小数部分
t=0;
DS0=!DS0; // LED闪烁
}
}
t++;
}
}
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
10.6 移植文件
文件中代码太多，这里主要关注：
1）inv_mpu.c源文件中定义了四个函数：
#define i2c_write MPU_Write_Len // 对应mpu6050.c源文件里的MPU_Write_Len
#define i2c_read MPU_Read_Len // 对应mpu6050.c源文件里的MPU_Read_Len
#define delay_ms delay_ms // 对应delay.c源文件里的delay_ms
#define get_ms mget_ms // 这是一个空函数
2）inv_mpu.c源文件中u8 mpu_dmp_init()初始化函数：
u8 mpu_dmp_init(void)
{
u8 res=0;
MPU_IIC_Init(); // 初始化IIC总线
if(mpu_init()==0) // 初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL); // 设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL); // 设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); // 设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); // 加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| // 设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); // 设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test(); // 自检
if(res)return 8;
res=mpu_set_dmp_state(1); // 使能DMP
if(res)return 9;
}else return 10;
return 0;
}
3）inv_mpu.c源文件中u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)获取欧拉角的函数：
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))
return 1;
ors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; // q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
// 计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}
else
return 2;
return 0;
}
十一、实验结果
该程序可用匿名四轴上位机软件实时查看6轴的原始数据和姿态数据，即欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。（这里就不放截图了）。
另外在LCD屏上，也可实时查看6轴的原始数据和姿态数据，如下图：
