STM32心得三十七：MPU6050六轴传感器实验

[复制链接]

STMCU小助手发布时间：2022-11-18 19:00

技术帖
文章封面:
文章简介:	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格式转换为浮点数; \4 }0 w5 B9 b
q1=quat[1] / q30; // q30格式转换为浮点数
2 a; e) }7 a9 y2 i
q2=quat[2] / q30; // q30格式转换为浮点数( r/ [* j& P' e b
q3=quat[3] / q30; // q30格式转换为浮点数
! ~4 Z" Z) d+ ^: u! a- M9 ]" q
// 计算得到俯仰角/横滚角/航向角( ~9 h4 e3 F3 S$ D# D/ d7 }7 g- G; c
pitch=asin(-2 * q1 * q3 +2 * q0* q2)* 57.3; // 俯仰角
: Z6 l! C; f' f1 V4 ?6 s
roll=atan2(2 * q2 * q3 +2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // 横滚角yaw=atan2(2*(q1*q2 +
) _/ ]3 j$ H+ b* t9 p I
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头文件函数

/**! E; c: f/ s" a, X% `$ e4 _2 M! J
******************************** STM32F10x *********************************/ p- L& V0 w, b: F4 t1 j
* @文件名称： mpuiic.h
) v" q3 k- t0 z- a# k" L: z
* @修改作者： Aaron9 ?- l; v. L* _* B+ V
* @库版本号： V3.5.0: \. k9 b+ w7 S. \
* @工程版本： V1.0.0
! L& [2 O5 N( n: P
* @开发日期： 2020年11月30日! H* I. {5 n( v" Q$ |
* @摘要简述： mpuiic头文件,主要跟MPU-6050模块进行IIC通讯时用, I$ `5 w# X1 [. x) N
******************************************************************************/
i5 R9 W' a+ _% `2 Z& P# D
/*-----------------------------------------------------------------------------4 q5 e( {- f2 f4 C+ U: y
* @更新日志：
" l- M* U/ v$ I& T8 r- ?
* @无9 `+ v2 ^6 e8 o, b
* ---------------------------------------------------------------------------*/. ]) }; r! {+ x+ P* z
#ifndef __MPUIIC_H( n% x7 K6 z, N% W6 f) ~$ r
#define __MPUIIC_H4 P* A2 D' l, ~3 y( O
/* 包含的头文件 --------------------------------------------------------------*/
: l+ D* n: `) I8 B+ k% ?
#include "sys.h"
4 S; v; O" D2 w k1 V( d& T$ m
#include "stm32f10x.h"
* b# [- W( Q ?% s% S
/* 寄存器方法控制GPIO管脚输入输出模式 ----------------------------------------*/! T2 G, Y8 g3 G
#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;} // PB11 上拉输入模式 ^% z. b3 b* p+ j
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;} // PB11 通用推挽输出模式，50MHz5 g" s7 d2 \4 S+ s6 V, p2 I
/* 位操作方法定义 ------------------------------------------------------------*/
/ t8 j4 D1 ]+ L9 H- p: r
#define MPU_IIC_SCL PBout(10) // PB10对应SCL
2 Q0 c0 h* s' _! f+ S0 z
#define MPU_IIC_SDA PBout(11) // PB11对应SDA，输出SDA
' N" {+ x$ P" c/ p Q6 Q( K* q" D6 A
#define MPU_READ_SDA PBin(11) // PB11对应SDA，输入SDA
0 D0 S/ ?" ~) d$ Q: J$ P6 L% j2 ^
/* 定义IIC底层驱动程序 -------------------------------------------------------*/- U# R5 q& h( h' a. C4 y
void MPU_IIC_Delay(void); // MPU IIC延时函数，延时 2 us/ a4 S6 K! W$ O2 @* @0 p' a, n
void MPU_IIC_Init(void); // 初始化IIC的IO口
) y, S. A( s# U$ o
void MPU_IIC_Start(void); // 发送IIC开始信号
" {8 |4 X3 B! r" `! e# q
void MPU_IIC_Stop(void); // 发送IIC停止信号
% e% F* L/ n% t$ x
u8 MPU_IIC_Wait_Ack(void); // IIC等待ACK信号/ V3 C* B: g- d9 m# |/ S$ x
void MPU_IIC_Ack(void); // IIC发送ACK信号
2 Y/ `- s' U, y0 q* T2 N
void MPU_IIC_NAck(void); // IIC不发送ACK信号
. z% q! `2 r8 O. K! Q$ T* ]0 e: F
void MPU_IIC_Send_Byte(u8 txd); // IIC发送一个字节
+ A4 Y% h O$ k+ m1 [. J# P( X0 l
u8 MPU_IIC_Read_Byte(unsigned char ack); // IIC读取一个字节
& F: V O8 I8 @' f& x1 h. N1 m
#endif /* __MPUIIC_H */
G/ ?7 U- L0 ^: m
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/

复制代码

10.2 mpuiic.c源文件函数

/**! D: }; t( t3 {/ T
******************************** STM32F10x *********************************
: N4 {$ g* f6 {8 W0 q, ?) c
* @文件名称： mpuiic.c# ? \' ?( j1 Q1 p: P8 y" d
* @修改作者： Aaron ~+ F% i+ N) r7 Y% {
* @库版本号： V3.5.00 c+ W7 @, W! z5 J- W+ _* J9 k
* @工程版本： V1.0.0
/ U9 l) B# u a' M4 }; M5 u, S
* @开发日期： 2020年11月30日
; {2 x! Q* |. Y
* @摘要简述： mpuiic源文件，主要包含与MPU-6050进行IIC通讯的IIC底层驱动程序7 M0 q4 k) O) `) ^7 b$ D8 `: ^
******************************************************************************/
' Z1 T5 |- Z: e7 W2 k
/*-----------------------------------------------------------------------------
+ d# v: n- n. \$ @, d* h9 C
* @更新日志：$ U. ~3 ]3 [" j( S% q! w
* @无$ P5 t8 @" I9 @* V" p
* ---------------------------------------------------------------------------*/, u9 y# _0 o0 g& f1 b
/* 包含的头文件 --------------------------------------------------------------*/! g8 H! S- S Y5 p& {
#include "mpuiic.h"% ]: Z+ i2 h2 _1 l% C$ V2 G( B# A
#include "delay.h"$ ~: x, ?: z( v
/*********************************************************************
- c6 W/ r& M, R- b' ~0 H$ ?* N
函数名称：MPU_IIC_Delay()% i0 v0 m( x u% z0 z- g' m/ R2 v1 m% N
函数功能：延时函数，延时2微秒（us）
( z# v- j" z: o8 a% Z
入口参数：无) J+ V6 O+ x$ t0 Z$ [- p( C
返回参数：无
$ p" g! Y% A& u( t4 W
修改开发：Aaron
$ {1 a3 [2 }) q9 ]9 n2 g! h3 w0 x
**********************************************************************/, j4 F; m, E4 p: B5 P
void MPU_IIC_Delay(void)/ }/ h' |2 m, \. A Z3 I# ]
{3 s" i! a/ M+ S }; [
delay_us(2);
$ w% y3 x. f+ M+ I2 {$ [
}
3 E8 H5 N( m7 [# }1 V2 L( p, |
/*********************************************************************7 U1 k) @7 a4 b, x1 r
函数名称：MPU_IIC_Init()& L; I. V( j9 y0 h% x z; {
函数功能：MPU_IIC管脚初始化配置0 P3 |. X1 h. G) R, t
入口参数：无
) Y& F, w% c, w" X
返回参数：无
2 k( w4 J3 b7 R
修改作者：Aaron/ F* @" r( N& _
**********************************************************************/6 @9 P7 }/ ~. x9 `9 Y; M
void MPU_IIC_Init(void)6 C2 ?* l* {8 K2 B
{ + B: z3 e& C5 x% L
GPIO_InitTypeDef GPIO_InitStructure;
4 t5 c! Z3 H( r/ w
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE); // 使能外设IO PORTB时钟 0 L# O% U1 C. r9 I9 ? W' W7 y/ J/ j
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; // 端口配置，管脚10和管脚11( `# U: H, V, K
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 推挽输出8 |3 z: a4 H' ^; c/ P
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // IO口速度为50MHz
8 y) ^' K8 q; B6 N1 h% }5 M& a5 z! V
GPIO_Init(GPIOB, &GPIO_InitStructure);
1 N/ J7 }3 {2 q6 J! ?( n) \- t
GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11); // PB10(SCL),PB11（SDA）输出高 6 K! r T, \+ k6 \; {- P
}
6 j$ R; q9 L' q9 l. H2 X& a0 ]
/*********************************************************************
# N2 \/ s8 a, r' M9 ~- L1 }5 }
函数名称：MPU_IIC_Start()
, b4 Q* y5 ?1 m5 M, o1 }0 L
函数功能：MPU_IIC起始信号% d4 F7 m9 C; a- c- V0 H: Q3 j
入口参数：无
4 K; g/ h/ P5 D( H R
返回参数：无
/ U; z3 a7 q2 N: `0 V' q- s$ ]' p
修改作者：Aaron5 a: T N# k7 b' }
**********************************************************************/
: Z( N! | X7 K) e& i2 A
void MPU_IIC_Start(void)
! ?2 E3 X! W; k5 E
{4 `2 \" @$ R5 O) s# x7 f
MPU_SDA_OUT(); // SDA线输出模式
4 ~1 a) e L" e' l* ^
MPU_IIC_SDA=1; // SDA输出高
! R& x& g7 q6 S. N1 K6 C
MPU_IIC_SCL=1; // SCL输出高，此时为空闲状态 N2 ]- t$ Y( k
MPU_IIC_Delay(); // 延迟 2 us
& z6 L, m0 D' a# d% S$ x
MPU_IIC_SDA=0; // 在SCL输出高的状态下，SDA由高至低跳变，IIC开始( O# o5 l+ ] |
MPU_IIC_Delay(); // 延迟 2 us7 z; C& }0 s. Q. C4 k9 P% C
MPU_IIC_SCL=0; // 钳住I2C总线，准备发送或接收数据 1 J- F. o8 H. A# J! N6 b* z) O) W
}
0 u5 }$ V" R% a; F9 O
/*********************************************************************
; q: z1 K* h* K3 _
函数名称：MPU_IIC_Stop()
/ j+ S" d1 p7 ]5 A" ^
函数功能：MPU_IIC停止信号2 x) _! R; K0 d- G ^
入口参数：无
}' V6 E2 P, @; r, Z/ W
返回参数：无8 A, y0 {$ J4 h" P, G) |
修改作者：Aaron; D5 X, @+ O* c3 v
**********************************************************************/
5 K! f/ [* y7 y* L* }6 C! f
void MPU_IIC_Stop(void)2 K4 K3 S& l- m. f
{! V! C* y9 D3 W# C# W' C: t( K
MPU_SDA_OUT(); // SDA线输出模式
3 u/ r/ S/ P8 I" R
MPU_IIC_SCL=0; // SCL输出低
1 F* ^7 v! g* M L2 E2 Y+ I
MPU_IIC_SDA=0; // SDA输出低
5 x$ m& x4 l& R& q2 s
MPU_IIC_Delay(); // 延迟 2 us
& c# D5 B9 A5 a# o+ h! q1 V' y
MPU_IIC_SCL=1; // 先SCL输出高，在SCL输出高的状态下，SDA一直是低电平6 `( A0 H. x8 _ e M# r* P# u: _4 p
MPU_IIC_SDA=1; // SDA输出高，发送I2C总线结束信号( @* a# x6 t$ L/ t
MPU_IIC_Delay(); // 延迟 2 us
$ y0 P( i0 ~2 e& q
}
. P9 _, _/ r: A3 u* U( g
/*********************************************************************( H3 S# h: q( v( b2 _/ a+ `8 ]
函数名称：u8 MPU_IIC_Wait_Ack()
7 ~$ s. E4 b/ o; I! W9 h: ]6 u- T
函数功能：等待应答信号到来6 F) \/ t: G# x) c. ]. i* T
入口参数：无8 C/ z* Q8 c8 t% z' ]
返回参数：u8，1，接收应答失败，0，接收应答成功
& w6 S8 ? y6 `: W
修改作者：Aaron
& b' H, b" J, P" e$ m, ~2 _
**********************************************************************/4 I1 m, z! r8 n4 d; U
u8 MPU_IIC_Wait_Ack(void)/ \; B8 }. J; Q1 K- R0 k' ]" i4 @
{: E! { q6 v w
u8 ucErrTime=0;! e/ b h, O* ]& Y; c
MPU_SDA_IN(); // SDA设置为输入
# v' V3 z, `" @; q3 T' i2 ~0 J
MPU_IIC_SDA=1; // SDA设高电平
7 e7 s. Z' X& A- b; a3 y
MPU_IIC_Delay(); // 延迟 2 us
4 y! h# X1 r" ~% ~
MPU_IIC_SCL=1; // SCL设高电平% ~1 E# m7 x% W' ?1 }
MPU_IIC_Delay(); // 延迟 2 us 8 u3 ], j# r2 C' X% u9 y
while(MPU_READ_SDA) // 读取PB11的值，一直读PB11=1，高电平，则不断循环，直到溢出
8 {; d5 d% m- S$ Y) S" J
{
# j! b- H: W$ J& {# x$ A0 C
ucErrTime++;
4 q# B; ]* v+ m. q ?0 G) @/ b. o
if(ucErrTime>250)
% R! D/ {! ]; L
{+ R1 y3 S+ Y/ O! K+ p
MPU_IIC_Stop();
2 H* b8 \* W, \% G0 G/ t8 z# U0 p
return 1;
' h. m9 V2 P3 ^# A- y' t/ N5 z" @
}
+ n7 g( M: x( N& k3 u
}
' H; l$ m, O1 q6 `
MPU_IIC_SCL=0; // SCL设低电平
, a* C2 Y' ^4 y8 i) k! ?+ E/ `
return 0; / I9 M' B( g# }5 w7 m# @: M
} $ X/ U K4 M: K8 |5 ]1 w" P" i4 @
/*********************************************************************
7 h* I# \0 C; B$ d5 E5 \% b) k
函数名称：MPU_IIC_Ack()
) d. l, h+ m* I5 i
函数功能：产生ACK应答
d) J7 @8 D; g. G
入口参数：无
! F% |9 t4 x1 f( X: i
返回参数：无
) ~: n' B& X- O8 D {
修改作者：Aaron
, Q0 y+ Y$ M( f4 S+ l7 q; b
**********************************************************************/
" Z, K+ {4 Z. c" _" r9 s
void MPU_IIC_Ack(void)% [0 Z* E# ? s/ Z' U2 C
{. A0 I& p: Z- b7 ^& O. D
MPU_IIC_SCL=0; // SCL设低电平 r! n& ?+ H: T& C2 P, ]* B
MPU_SDA_OUT(); // SDA输出模式- H, I, G' s$ `
MPU_IIC_SDA=0; // SDA设低电平
5 i8 J( X. t) O. ? M
MPU_IIC_Delay(); // 延时 2 us
& w; q9 L7 o j" K# P
MPU_IIC_SCL=1; // SCL设高电平! ^7 S2 ?3 m+ q$ k
MPU_IIC_Delay(); // 延时 2 us
# w7 ]' J& H E
MPU_IIC_SCL=0; // SCL设第电平，在SCL高电平期间，SDA为低电平，则说明有效应答& ?* t' Z' c+ E1 ?& _+ i
}
) c) _1 C1 C4 h/ f
/*********************************************************************) d7 q/ [. f0 t- X% g! [
函数名称：MPU_IIC_NAck(); B/ {0 N" F) k; t W; e
函数功能：不产生ACK应答
& c; h" H3 f7 [) v7 _5 ^
入口参数：无" H" D- j5 \8 y7 S
返回参数：无8 T( @/ n8 ^' v: m- f. D
修改作者：Aaron
/ v8 k9 v+ m; c: t6 i
**********************************************************************/
; Q$ D9 H2 e, L( x( {! w
void MPU_IIC_NAck(void)7 A1 e& J' E8 B9 g
{
4 h `) i7 U8 y
MPU_IIC_SCL=0; // SCL设低电平
) I$ S* N0 A/ ?8 H" Z7 K
MPU_SDA_OUT(); // SDA输出模式
6 _* V8 H; E1 h2 ]
MPU_IIC_SDA=1; // SDA设高电平
( G( ^8 D/ }) I, T
MPU_IIC_Delay(); // 延时 2 us# y% ]' v2 n, G) y; S
MPU_IIC_SCL=1; // SCL设高电平
) ~9 C. I) R0 @1 H% e6 H" F8 D
MPU_IIC_Delay(); // 延时 2 us
% B) F" u- I6 V' Q: A, g/ y
MPU_IIC_SCL=0; // SCL设低电平，在SCL高电平期间，SDA为高电平，则说明无效应答: c8 R U0 K; o$ G. Q/ a0 D
}
( w: j4 Y1 ?! `" T& o q9 y' D
/*********************************************************************8 o! ~, C- y; [; t# M
函数名称：MPU_IIC_Send_Byte(u8 txd)
: z6 }" o, @7 k, u# H7 k
函数功能：IIC发送一个字节
( L! e3 G8 ]2 ~; `1 i) E
入口参数：u8 txd- ]! N# _% R8 B1 K7 r5 j
返回参数：无( i2 v. f* R" y8 k; U
修改作者：Aaron' [- c3 p* C2 b; c1 `* b
**********************************************************************/
# ?' `, k7 u# {9 _
void MPU_IIC_Send_Byte(u8 txd)
- ^/ J8 w) y {3 S; \2 b" L4 y& b7 {
{
; g; I* Y4 }- X
u8 t; 0 B4 h5 f2 O+ ^5 o4 E6 }: t
MPU_SDA_OUT(); // SDA输出模式
/ _" e2 } z0 }$ F- s% L- t
MPU_IIC_SCL=0; // SCL设低电平，此时可改变SDA，以实现数据的有效传输
; O& p" x X* e! I( g" f" o
for(t=0;t<8;t++) // 发送一个字节，即8个bit
3 x2 _; y" ]* A& T1 ]
{ # F' I- e& D8 _. [3 o
MPU_IIC_SDA=(txd&0x80)>>7; // 获取u8 txd的最高位（0或1），并右移7位，将最高位值给MPU_IIC_SDA. [) Q+ R1 ^. g: p1 z
txd<<=1; , f7 O4 q2 c1 b7 ^
MPU_IIC_SCL=1; // SCL设高电平, `% Q4 @( _- A5 `0 B! h
MPU_IIC_Delay(); // 延时 2 us
0 S" U L8 C' x, A, h# U* D
MPU_IIC_SCL=0; // SCL设低电平，传输SDA完成. y4 d' c) k6 z9 U7 k! Y
MPU_IIC_Delay(); // 延时 2us1 u0 M! {' `. n
} 6 h+ P7 B; y- o
} ' H3 g0 {) U9 h& G& w. X/ Y3 i
/*********************************************************************
2 N Z/ e }2 u6 ^5 c
函数名称：u8 MPU_IIC_Read_Byte(unsigned char ack)" B! y- m! e' ^4 Y1 P
函数功能：IIC读取一个字节，ack=1时，发送ACK，ack=0，发送nACK
; a, ~6 f6 B3 ^' ^+ T, j
入口参数：ack' D0 Z3 W' Z' I) ] H3 G
返回参数：u8，
( T) g8 `/ ]' K6 r) U, ~
修改作者：Aaron' B: y- I* r0 F, |) k# x' V+ v9 b
**********************************************************************/( V2 n* L, _# ?+ b
u8 MPU_IIC_Read_Byte(unsigned char ack)
4 u$ B0 z, @7 |4 A. t+ w
{! T% u Q) ^9 Z% o3 J
unsigned char i,receive=0;
% S: U5 k9 L7 ?! W" {( N
MPU_SDA_IN(); // SDA设置为输入模式
" ]- o9 V) v" f; |0 N0 i# r
for(i=0;i<8;i++ ) // 读取一个字节，即8个bit3 w8 c7 \* j1 n$ p% ]( o. k
{
7 e8 k d* x/ P) p
MPU_IIC_SCL=0; // SCL设为低电平9 {$ T' E7 P5 J% I* U
MPU_IIC_Delay(); // 延时 2us
& |* g: ?' b6 l$ D' t* K; Z3 {+ d
MPU_IIC_SCL=1; // SCL设为高电平
" v- N5 p8 K, g( o9 w( G1 E
receive<<=1;7 I0 c* O* _5 a% D+ P% m, U9 e
if(MPU_READ_SDA) // 如果读取的SDA为1，则receive最低值+1，若读取的SDA为0，则receive最低值不变，即0
* ~' Q* P8 c1 E$ n
receive++;
: a/ W6 J# |8 B0 H2 q2 I4 ]
MPU_IIC_Delay(); // 延时 2us
' Y% {: r9 w- b3 d' i- L7 @5 Q4 D
} # W3 u& [( U. [# S1 Q* w
if (!ack)3 ?# R$ T: I/ n2 U" n2 F
MPU_IIC_NAck(); // 发送nACK R* A# a+ b3 J+ o) A! i
else. l$ ?: E! m E5 H4 p8 E
MPU_IIC_Ack(); // 发送ACK * c, k$ z, m. S; u6 S" m$ u
return receive;1 h9 I2 D" d1 \3 `' E9 ~# N/ N
}( y; I) O: Y2 m- ]) g) W8 Z
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/

复制代码

10.3 mp6050.h头文件函数
代码中标记“初学”的说明是前面寄存器介绍时讲到的。

/**
1 X3 e5 ], @/ J" |! R! o% u
******************************** STM32F10x *********************************( ], n" Z! b+ Z+ T' l
* @文件名称： mpu6050.h
# ~" |4 @% q$ N3 o
* @修改作者： Aaron' h4 M* a9 d2 ^* x
* @库版本号： V3.5.0
# R3 [) { W* H! Y( @% e# Z" p4 a
* @工程版本： V1.0.0
7 b/ ^$ ?! R- E! `5 X
* @开发日期： 2020年11月30日) b. w& G6 Z. v2 ~# e
* @摘要简述： mpu6050头文件
# ^. `$ s# M! b; v
******************************************************************************/- M" u5 V5 o( U; s8 q4 @
/*------------------------------------------------------------------------------ ?8 g N7 [' j' R* v
* @更新日志：( h, T8 F2 w: n3 V6 u5 ~4 j' i
* @无
" ]) i5 X a4 z/ y5 B( f
* ---------------------------------------------------------------------------*/ o2 z, m0 r, J: ~0 p
#ifndef __MPU6050_H5 h9 j8 h* b0 H, @0 B8 R
#define __MPU6050_H
5 }( Z9 S+ S* t& M& r
/* 包含的头文件 --------------------------------------------------------------*/; F2 ]% k) d' Q9 z2 U6 D
#include "mpuiic.h" $ t& U @" `7 V
/* 位操作控制PA15,对应MPU6050 AD0的高低电平 ----------------------------------*/
# m/ ~/ X" S2 U, B! K; `
#define MPU_AD0_CTRL PAout(15) // 控制AD0电平,从而控制MPU-6050地址, A( V* m% [4 _- K
/* MPU6050 寄存器ID定义 ------------------------------------------------------*/+ U7 i5 Z* l' S2 j( X
// #define MPU_ACCEL_OFFS_REG 0X06 // accel_offs寄存器,可读取版本号,寄存器手册未提到- A% E8 ^1 K) t3 O
// #define MPU_PROD_ID_REG 0X0C // prod id寄存器,在寄存器手册未提到* U2 v% Z% Q+ u- n
#define MPU_SELF_TESTX_REG 0X0D // 自检寄存器X ACC和G$ w' N! B5 z8 `. h4 _ O( s0 |
#define MPU_SELF_TESTY_REG 0X0E // 自检寄存器Y ACC和G
+ J7 R! L' h$ |( ?# C
#define MPU_SELF_TESTZ_REG 0X0F // 自检寄存器Z ACC和G
; K y# H5 @; I1 H6 T$ Q) y
#define MPU_SELF_TESTA_REG 0X10 // 自检寄存器X,Y,Z only ACC
. n! W, S I4 g2 j
#define MPU_SAMPLE_RATE_REG 0X19 // 陀螺仪采样频率分频器 **初学**3 _4 }& w) ~+ `$ X- J
#define MPU_CFG_REG 0X1A // 配置寄存器 **初学**- L4 B; |0 `" t8 G8 y
#define MPU_GYRO_CFG_REG 0X1B // 陀螺仪配置寄存器 **初学**
) w( K9 N5 r$ p1 y: S& S
#define MPU_ACCEL_CFG_REG 0X1C // 加速度计配置寄存器 **初学**0 @5 ]7 V ]: n( O d/ X7 d: E
#define MPU_MOTION_DET_REG 0X1F // 运动检测阀值设置寄存器
, o6 T% T6 L9 E9 X1 H) D6 C
#define MPU_FIFO_EN_REG 0X23 // FIFO使能寄存器 **初学**
/ D( I, w2 ?% t- {# r: H
#define MPU_I2CMST_CTRL_REG 0X24 // IIC主机控制寄存器
1 O. L! o8 `+ e$ d0 R+ @
#define MPU_I2CSLV0_ADDR_REG 0X25 // IIC从机0器件地址寄存器
7 N3 h( e! c& d0 O" |: H4 C
#define MPU_I2CSLV0_REG 0X26 // IIC从机0数据地址寄存器
7 J7 f: Y' T$ U5 H5 i2 m
#define MPU_I2CSLV0_CTRL_REG 0X27 // IIC从机0控制寄存器
) \4 F$ f0 z0 t' Q' ?
#define MPU_I2CSLV1_ADDR_REG 0X28 // IIC从机1器件地址寄存器) |* z) [6 m. H( M
#define MPU_I2CSLV1_REG 0X29 // IIC从机1数据地址寄存器
' J6 b5 J8 J/ F1 d/ H
#define MPU_I2CSLV1_CTRL_REG 0X2A // IIC从机1控制寄存器
8 ~3 L$ W" _4 h! n
#define MPU_I2CSLV2_ADDR_REG 0X2B // IIC从机2器件地址寄存器2 q$ x1 r5 t+ c0 k$ e
#define MPU_I2CSLV2_REG 0X2C // IIC从机2数据地址寄存器$ Z. z# _1 N" i g7 D6 M
#define MPU_I2CSLV2_CTRL_REG 0X2D // IIC从机2控制寄存器8 e; f+ z$ v' I; o$ ?' M9 H+ l1 U
#define MPU_I2CSLV3_ADDR_REG 0X2E // IIC从机3器件地址寄存器
/ A% j/ v2 Z4 P: Y- h6 e! X+ Z
#define MPU_I2CSLV3_REG 0X2F // IIC从机3数据地址寄存器& Y6 c4 X* D3 c% K" t! I
#define MPU_I2CSLV3_CTRL_REG 0X30 // IIC从机3控制寄存器4 q% u! H! f/ p5 L) Q
#define MPU_I2CSLV4_ADDR_REG 0X31 // IIC从机4器件地址寄存器; x* P0 K1 A0 }0 {
#define MPU_I2CSLV4_REG 0X32 // IIC从机4数据地址寄存器5 d/ M1 q, E" k! ]+ e
#define MPU_I2CSLV4_DO_REG 0X33 // IIC从机4写数据寄存器3 I% ]+ a: V- K' R
#define MPU_I2CSLV4_CTRL_REG 0X34 // IIC从机4控制寄存器$ Q# r/ p( L! F5 v2 Z
#define MPU_I2CSLV4_DI_REG 0X35 // IIC从机4读数据寄存器
& E& w4 ?; Z/ R2 [" E' u
#define MPU_I2CMST_STA_REG 0X36 // IIC主机状态寄存器
. `% R9 d* P* Y
#define MPU_INTBP_CFG_REG 0X37 // 中断/旁路设置寄存器
. @- E; k! b/ w( `0 }
#define MPU_INT_EN_REG 0X38 // 中断使能寄存器/ S% j. X9 z4 r. W4 h
#define MPU_INT_STA_REG 0X3A // 中断状态寄存器; B7 }+ L0 E, @1 u
#define MPU_ACCEL_XOUTH_REG 0X3B // 加速度值,X轴高8位寄存器 **初学**
' J! C3 T- K6 J( v# g, j7 B3 |
#define MPU_ACCEL_XOUTL_REG 0X3C // 加速度值,X轴低8位寄存器 **初学**& U. q5 Z7 p+ d' V! ~7 Q
#define MPU_ACCEL_YOUTH_REG 0X3D // 加速度值,Y轴高8位寄存器 **初学**
5 K' r$ V1 Z5 Y# x8 C+ X
#define MPU_ACCEL_YOUTL_REG 0X3E // 加速度值,Y轴低8位寄存器 **初学**( h2 V( {/ X; w! U6 Y, P1 A- j8 N# n
#define MPU_ACCEL_ZOUTH_REG 0X3F // 加速度值,Z轴高8位寄存器 **初学**! H, ?- F+ H2 b" ^( U1 ?$ F
#define MPU_ACCEL_ZOUTL_REG 0X40 // 加速度值,Z轴低8位寄存器 **初学**" n/ R1 D# F# v$ Q* k- a
#define MPU_TEMP_OUTH_REG 0X41 // 温度值高八位寄存器 **初学**" X% P0 w0 [$ h) K# r# A( X3 F
#define MPU_TEMP_OUTL_REG 0X42 // 温度值低八位寄存器 **初学**
5 ]* D& N3 G, Q* A& f
#define MPU_GYRO_XOUTH_REG 0X43 // 陀螺仪值,X轴高8位寄存器 **初学**
: S8 `+ m; ?2 S( S; j9 q
#define MPU_GYRO_XOUTL_REG 0X44 // 陀螺仪值,X轴低8位寄存器 **初学**
5 P- s4 Y- S3 _2 o4 v* C4 C: T
#define MPU_GYRO_YOUTH_REG 0X45 // 陀螺仪值,Y轴高8位寄存器 **初学**
2 C! T- }, f1 J# A$ g, {5 C
#define MPU_GYRO_YOUTL_REG 0X46 // 陀螺仪值,Y轴低8位寄存器 **初学**
5 ^3 a3 P0 {# p( S% Y/ w$ B4 {9 \
#define MPU_GYRO_ZOUTH_REG 0X47 // 陀螺仪值,Z轴高8位寄存器 **初学**
" h) C: ^7 l- s8 F6 Z- e4 f1 B
#define MPU_GYRO_ZOUTL_REG 0X48 // 陀螺仪值,Z轴低8位寄存器 **初学**
( _% g2 ]) K& h1 L" b N
#define MPU_I2CSLV0_DO_REG 0X63 // IIC从机0数据寄存器
$ i% [; Q$ V+ h: q4 C
#define MPU_I2CSLV1_DO_REG 0X64 // IIC从机1数据寄存器
2 H _ R9 r( R; @) h' i( B- `2 h
#define MPU_I2CSLV2_DO_REG 0X65 // IIC从机2数据寄存器" j! [! U1 B: Z f* G
#define MPU_I2CSLV3_DO_REG 0X66 // IIC从机3数据寄存器
& \5 i; L& E: |6 q% o3 [1 P
#define MPU_I2CMST_DELAY_REG 0X67 // IIC主机延时管理寄存器
4 y' \) R. t$ f1 v0 {
#define MPU_SIGPATH_RST_REG 0X68 // 信号通道复位寄存器" e( T& ~& R: c g! T
#define MPU_MDETECT_CTRL_REG 0X69 // 运动检测控制寄存器
6 l1 ^6 f4 M# ~. Y! Z
#define MPU_USER_CTRL_REG 0X6A // 用户控制寄存器
6 f3 R1 H' G2 x/ B
#define MPU_PWR_MGMT1_REG 0X6B // 电源管理寄存器1 **初学**! H9 k, t, k% x. f0 H5 D
#define MPU_PWR_MGMT2_REG 0X6C // 电源管理寄存器2 **初学**
/ v" u5 O, Z) Q
#define MPU_FIFO_CNTH_REG 0X72 // FIFO计数寄存器高八位
. T2 x9 Q- L0 s5 U4 o M
#define MPU_FIFO_CNTL_REG 0X73 // FIFO计数寄存器低八位6 z; T0 B% t1 e* P3 M
#define MPU_FIFO_RW_REG 0X74 // FIFO读写寄存器
( A4 h! k* U, q) {
#define MPU_DEVICE_ID_REG 0X75 // 器件ID寄存器& {" r" T4 H6 D; h% b7 c
// 备注：如果AD0脚(9脚)接地,0,IIC地址为0X68(不包含最低位)；
7 h$ [9 l& q* |, |5 q
// 如果接V3.3,则IIC地址为0X69(不包含最低位)，$ q, z* |3 _4 I, z2 d7 h* I
// 本开发板由PA15管脚控制，另PA15为低电平
1 w) b; T' z4 O0 j5 |; _
#define MPU_ADDR 0X68
) U4 A ]! x0 J3 ^
/* 函数申明 -----------------------------------------------------*/4 ~; U: B! f' j. J
u8 MPU_Init(void); // 初始化MPU60504 Y7 l9 H. k4 D, c1 v; c4 Y+ o$ V
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf); // IIC连续写
8 b+ @5 R* M5 X( t3 @, C# _, e- W
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); // IIC连续读
5 M, q- ^+ _! _) V. Z
u8 MPU_Write_Byte(u8 reg,u8 data); // IIC写一个字节; \, D9 [1 ?9 u6 z/ v
u8 MPU_Read_Byte(u8 reg); // IIC读一个字节
3 C6 z- b" \0 c
u8 MPU_Set_Gyro_Fsr(u8 fsr);
" ?% b* {+ ]5 p4 Z( y: A; J5 H. _6 d
u8 MPU_Set_Accel_Fsr(u8 fsr);' p! J, y3 w2 {) k% c5 U
u8 MPU_Set_LPF(u16 lpf);
( ~9 S9 J; J4 i' C" u
u8 MPU_Set_Rate(u16 rate);; \0 l4 o/ T0 @' O3 y& o4 {
u8 MPU_Set_Fifo(u8 sens);
% ^) w2 t M7 P2 N: r
short MPU_Get_Temperature(void);
* C8 k3 f( v, X5 |
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
& H% ?7 z: C7 D( j! N
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
; q0 K1 N0 l! \2 {: ?% @
#endif /* __MPU6050_H */
) x+ V; H2 ^9 E: N( q! e
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/

复制代码

10.4 mpu6050.c源文件函数

/**
. L/ x' ]6 `' m& C
******************************** STM32F10x *********************************0 e5 l. W8 H( \& d# Z0 l1 I
* @文件名称： mpu6050.c, e3 Z) m: Z1 P r7 ]- L1 q
* @修改作者： Aaron
3 b4 [/ e: B3 C
* @库版本号： V3.5.05 P- c" c( |& q8 s
* @工程版本： V1.0.0
) E% z2 T) t0 [; Z
* @开发日期： 2020年11月30日
5 O/ b3 d6 E9 \7 |/ [6 @9 T9 R( k
* @摘要简述： mpu6050源文件3 }) h) i8 ?! o
******************************************************************************/
7 S+ {( y, b' {& q
/*-----------------------------------------------------------------------------6 J* v; x+ \: o9 k
* @更新日志：
# Y% p5 [* ^: f7 J' m* O. z. N
* @无
1 ]' { {0 u( w! U
* ---------------------------------------------------------------------------*/
7 r$ ^! U/ ]. d* g$ I) E: E) _
/* 包含的头文件 --------------------------------------------------------------*/
$ U! @' P4 L3 {
#include "mpu6050.h"; B( G% X- T8 ]/ v) @4 G3 z
#include "delay.h"
/ _6 W; P9 S c9 R
/*********************************************************************" k( ?2 g' i5 [* q# c1 Q" \
函数名称：u8 MPU_Init()
' W( k9 y( }- d& x' t+ g2 L1 o0 K; {
函数功能：初始化MPU6050: S/ C# s3 x: w: J: b
入口参数：无
# \3 Y( h( x: a+ {
返回参数：0，成功，1失败6 A5 m6 W. g" m0 d
修改作者：Aaron
; n3 u2 o# p7 y
**********************************************************************/. l# ?5 s, t0 q0 I1 ~7 t
u8 MPU_Init(void)
7 X& z [" d; o; A8 l) R# {" P
{
; n- g( F+ D3 N' M+ q
u8 res;! u+ l! D9 I9 m3 s' |0 Y
GPIO_InitTypeDef GPIO_InitStructure;( g5 r! F7 F! b% J/ L& C/ A
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE); // 使能AFIO时钟 9 K0 N, X6 O' ~' c
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); // 使能外设IO PORTA时钟 ; f$ C& X O: I1 ~2 j' [ J
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; // 管脚15
7 Y0 l* |4 Z) F& ^9 Y
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 推挽输出' t, p0 j* K' @7 N: y- b
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // IO口速度为50MHz
6 E( X2 ^9 J% e' x/ v" @
GPIO_Init(GPIOA, &GPIO_InitStructure); // 根据设定参数初始化PA15,对应MPU-6050芯片的AD0地址管脚! Y" S5 W' v9 j+ n9 H9 s
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); // 禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO!!!8 M* T* ?. m4 D$ y6 u' c- c
MPU_AD0_CTRL=0; // 控制MPU6050的AD0脚为低电平,则从机地址为:0X68" ^* n) W& \; @
MPU_IIC_Init(); // 初始化IIC总线4 S2 H/ Q. I6 m1 O7 ]
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); // 对电源管理寄存器1输入0x80，复位MPU6050; t' @8 Z* c# @ i0 Z
delay_ms(100);
3 S( [, |; f, t( i- ]
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); // 对电源管理寄存器1输入0x00，主要令[6]位设0，正常工作模式，即唤醒MPU6050
8 K$ {$ N9 a( u* E% U/ F: W+ [4 f$ @
MPU_Set_Gyro_Fsr(3); // 设置陀螺仪传感器满量程范围,±2000dps4 q9 l+ ?- h+ l. P, O4 Y! J
MPU_Set_Accel_Fsr(0); // 设置加速度传感器满量程范围,±2g
5 G, {- K$ e) _9 [1 q
MPU_Set_Rate(50); // 设置采样率50Hz
5 d4 [$ a. x- G8 k7 j1 O/ y
MPU_Write_Byte(MPU_INT_EN_REG,0X00); // 对中断使能寄存器输入0x00,关闭所有中断+ ^9 [4 }6 r U/ ~
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); // 对用户控制寄存器输入0x00,I2C主模式关闭3 P9 [' S2 J6 F! x/ a7 ]
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); // 对FIFO使能寄存器输入0x00,关闭FIFO1 N& V; X" L' K+ q) b& a3 b8 ^* p
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); // 中断/旁路设置寄存器输入0x80,INT引脚低电平有效
) L# |5 C# @% q
res=MPU_Read_Byte(MPU_DEVICE_ID_REG); // 读取器件ID寄存器值，传至res里: L! V- v! F; ? U$ l5 I7 \$ h2 ^6 D
if(res==MPU_ADDR) // 如果res=0x68，则器件ID正确
# n* _( \+ g/ j3 I; M
{! O' J. V" l- L; o8 [# I
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); // 对电源管理寄存器1输入0x01，设置CLKSEL,001，PLL X轴为参考
8 e, r4 H" b* s) i3 S
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); // 对电源管理寄存器2输入0x00，加速度与陀螺仪都工作（都不待机）) b1 J* g7 q0 ?
MPU_Set_Rate(50); // 设置采样率为50Hz2 t- S1 D0 e* M4 a
}
) F( G/ l- u- Q h' k: u
else
9 j. W7 o9 I- f4 a {: v; W
return 1;, q. _9 ?% {9 f2 }2 o
return 0;: P) ~* u- F, v: H& [/ @' t' J$ h
}6 ] ], j1 B1 |6 V7 p
/*********************************************************************
+ s& t' w) Q1 f. O- u
函数名称：u8 MPU_Set_Gyro_Fsr(u8 fsr)' Q, T( n' t. S" i; w
函数功能：设置MPU6050陀螺仪传感器满量程范围
2 h5 @3 y7 a* R( U4 `' D2 v
入口参数：u8 fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps& { X9 v& o( |( h( U! N( o7 K
返回参数：0，成功，1失败! A2 x. N* P2 W1 Y C
修改作者：Aaron) V/ T8 t' m4 z5 x$ c
**********************************************************************/8 b1 p$ T+ W0 u4 D; d
u8 MPU_Set_Gyro_Fsr(u8 fsr)
+ E0 R) f7 A' W, q$ V: {
{) z3 {: ^, z; \+ n/ u2 r/ C u% }
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3); // 设置陀螺仪满量程范围为±2000dps , J* r# k0 o3 @" ^8 w
}
( r8 @/ q. p# q* S# F
/*********************************************************************5 y- j. |: O3 n: r$ H4 k5 L' y
函数名称：u8 MPU_Set_Accel_Fsr(u8 fsr): u, ?: g* b6 G' k5 V3 Z5 p
函数功能：设置MPU6050加速度传感器满量程范围
+ a7 u t3 `, I8 O7 x1 d, u/ ]
入口参数：fsr:0,±2g;1,±4g;2,±8g;3,±16g: A' E* D& O6 j1 [
返回参数：0，成功，1失败; F1 q% n0 x' }! n+ P
修改作者：Aaron* J9 @' E5 X6 w S' N0 v3 m
**********************************************************************/
% `1 Z; G+ t+ j+ e! L) S5 q* Y
u8 MPU_Set_Accel_Fsr(u8 fsr)
& `6 f& P4 V$ s- a" J
{1 B$ [6 T8 D! E& _8 R b* D
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3); // 设置加速度传感器满量程范围为±2g
$ o* ]( U4 m: R# c
}2 y+ a4 `7 T7 N' w
/*********************************************************************
0 x) C5 u. b* q. l
函数名称：u8 MPU_Set_LPF(u16 lpf)0 x0 w& X! g+ q
函数功能：设置MPU6050的数字低通滤波器1 z0 W8 m' B" {) w! {
入口参数：lpf:数字低通滤波频率(Hz)+ p) G+ U' D( d! A# E6 J6 v, ^
返回参数：0，成功，1失败
; a7 ~2 {. ^+ [6 t
修改作者：Aaron
! a5 }( p& I t: m6 P3 c: ? A
**********************************************************************/9 P* A/ X4 Z+ Z8 Y
u8 MPU_Set_LPF(u16 lpf)( K% W" o7 Y" w. C$ d
{& u' v) M. A5 C. p0 j5 r0 `, A
u8 data=0;
& P, p& V6 Y- }2 m7 h+ g' G
if(lpf>=188) // 如果lpf≥188Hz，则data=14 W. G" k/ |3 _
data=1;1 L0 d$ o9 H" F
else if(lpf>=98)) y9 r" O; B/ a& C. E& N% [2 E7 Y2 V
data=2;8 j2 G2 W/ Z1 ]& y. D* g- i; u5 M; a
else if(lpf>=42)
% x6 q8 D; P ]' Z! F3 {
data=3;
' D% G: |: S7 [0 m
else if(lpf>=20)
) E' o6 ]* Y# }% K& U
data=4;
' c2 }4 R" g8 n# ]! l
else if(lpf>=10)) V6 \; I% R* M8 S: m: F, w$ F
data=5;
; W0 |9 B% h' a$ m- T, S
else
1 J* e4 ]- k* A ~1 u. J
data=6;
$ H9 E B% R9 T# P
return MPU_Write_Byte(MPU_CFG_REG,data); // 设置数字低通滤波器 8 F. _- M5 e& ?( K
}' g' y8 Q, l7 a/ W) M$ ^& O5 }
1 u, n7 s3 m% j/ @" @" X6 F1 o6 P
/*********************************************************************3 v: @) I5 G& A3 [
函数名称：u8 MPU_Set_Rate(u16 rate)( | a7 h4 n8 D) e6 j$ e& f- n
函数功能：设置MPU6050的陀螺仪采样率(只在陀螺仪输出频率Fs=1KHz时成立)
5 S3 h& \2 Z* E
入口参数：rate:4~1000(Hz)，陀螺仪采样频率
2 ~6 M( J# o/ P+ `
返回参数：0，成功，1失败
; \- Y% l w" l. r4 s/ Y- I
修改作者：Aaron
5 n2 M5 L0 D* ]" p
**********************************************************************/, o- x, Z( O* `$ N1 v) B
u8 MPU_Set_Rate(u16 rate)# f. B& ~7 Q% s, N# s7 k# i" b) q
{( v/ v+ T: k$ }: f% e7 n
u8 data;+ f' a& G9 p' [2 d1 X) c, ?0 W5 [
if(rate>1000)% A2 W% R6 O" r! Q- R
rate=1000;
0 ~, Z. s$ f' r+ p* F2 i# W3 T
if(rate<4). _0 v- ]% _# Y+ ~: i% y# [# z
rate=4;
$ i! Y1 z8 }7 _9 _3 e% Q
data=1000/rate-1;- G( j! U( U" b8 o/ a& j
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);
# u% C% a6 ?! B; M8 W
return MPU_Set_LPF(rate/2); // 自动设置LPF为采样率的一半5 }( N8 y, G- ^1 S2 F3 E& R3 q
}
/ T$ u* k* A# q/ @! R
/********************************************************************** S: Q/ N3 W- M3 N& d
函数名称：short MPU_Get_Temperature()
( F9 e5 O1 _# _8 L6 l6 ~
函数功能：得到温度值2 e- K7 h1 h* R. X( y
入口参数：rate:4~1000(Hz)，陀螺仪采样频率
2 Y" \* f" {6 x9 M: \
返回参数：温度值(扩大了100倍)4 ]* | U5 K L
修改作者：Aaron
4 V( L# G# d' l+ ~. c) o
**********************************************************************/; ]5 B0 b+ Q$ K# F# d
short MPU_Get_Temperature(void)
4 R- Q& b2 W( y1 v5 W, f2 B
{
1 X8 O' g6 ?# D# p" c5 W
u8 buf[2];
$ s. W/ P6 x) L; z# y
short raw;
8 e$ |4 j- Q0 ~- K! \- m
float temp;
# q" x3 V: n5 s0 N' F5 g( d; ~
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); // addr:器件地址；reg:寄存器地址；len:写入长度；buf:数据区1 L7 [ O1 _1 o
raw=((u16)buf[0]<<8)|buf[1]; 4 m- e& Y6 L! K. ^( b# p2 j3 z
temp=36.53+((double)raw)/340; 1 }* C" ]4 r9 x* b; T2 g7 E
return
9 p, a; _: ]. U$ ~! r/ C
temp*100;;
" I0 ]6 k7 D1 M( E
}4 l7 E2 J" E3 h; \( d: q1 I5 A
/*********************************************************************6 C( b# U! C4 x Z- V5 p
函数名称：u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)7 O, a: t! p# D% n! i
函数功能：得到陀螺仪值(原始值)1 |5 z; x' F. \
入口参数：gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号): E. C8 j4 s4 z# V
返回参数：u8 0成功，1失败
4 I5 P- o7 w6 \ E/ n4 y( \' |
修改作者：Aaron
8 k/ P* [+ j ?/ f7 g7 }& S
**********************************************************************/) _/ y$ s( \/ S! R- N8 _
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
7 n/ f9 D* Y' {# V& i
{
8 r& ^7 b- Z5 l
u8 buf[6],res;
% J$ E2 J p6 y" v9 m
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);; L* I* F# K: l, L
if(res==0). ^1 ^1 K7 ]- B+ T8 m
{
& ]2 ^# P- d `6 F1 s) o
*gx=((u16)buf[0]<<8)|buf[1]; , e. G7 R9 j/ g4 R+ X5 o
*gy=((u16)buf[2]<<8)|buf[3];
) f+ C) d8 r1 O/ K5 \
*gz=((u16)buf[4]<<8)|buf[5];4 Y- s8 @6 R2 W/ \6 r7 u
} 3 ~8 S Y( i, p" J0 V+ d$ F a0 I/ y5 c& Y
return res;;
7 i8 E! G H" }# h
}
; \9 U' ^, ?% A4 S
/*********************************************************************) j3 \2 h% y, Y7 y( C: T+ Q
函数名称：u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
5 n8 ^. I" V& Z
函数功能：得到加速度值(原始值)
0 }" W2 a# n7 S% E* i6 E! e. q2 d5 J
入口参数：gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
* d3 k8 b# `. S& \4 K2 G
返回参数：u8 0成功，1失败
/ j$ R2 Z( M& O# j7 l, k4 t; N
修改作者：Aaron
0 @' o' P% y" G3 e( [$ f7 v# u
**********************************************************************/
$ O" S# I$ x7 C$ S
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
% B, [: f6 M, l' V
{
$ T# F$ w" L( K" |4 P5 |( p
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);- K7 Q% r5 t1 _ }1 U
if(res==0)
& f5 q& x) W4 f
{: w" {7 u- Z+ H( k0 t6 K4 b8 V
*ax=((u16)buf[0]<<8)|buf[1];
. y3 w0 f& M8 n% @
*ay=((u16)buf[2]<<8)|buf[3];
t0 k. ^. W: v! \' i) A1 E
*az=((u16)buf[4]<<8)|buf[5];
1 W4 `' ]$ X3 G! e, U0 G
} 0 X5 e! h3 `, m) u. T: e2 o
return res;;" m, X4 w0 ~! h( H0 o. H" C
}
$ Z4 Q0 A9 U4 E- U7 t
/*********************************************************************
7 G% I$ F" H+ p, K; R: ^/ _9 ~
函数名称：u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
( Q( z& P f. o, @$ Z
函数功能：向MPU6050连续写入数据7 G! |- y" a# ?
入口参数：addr:器件地址；reg:寄存器地址；len:写入长度；buf:数据区: z3 z+ K* ?7 U5 ~' j# C
返回参数：u8 0成功，1失败
+ ]4 P9 P6 Y% Z9 N9 b
修改作者：Aaron
9 L- I+ @7 d& t- `
**********************************************************************/& e0 G% K( j: J7 Q* W
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)& C- s& L2 T0 s" I# v
{$ e( V& z8 X t% T+ x* q. T' Y c
u8 i; * O, M4 N) s4 W8 {1 H. K( h6 i
MPU_IIC_Start();
. ^( W4 H! _) d/ ]7 I h
MPU_IIC_Send_Byte((addr<<1)|0); // 发送器件地址+写命令
7 @9 F0 S8 }9 X
if(MPU_IIC_Wait_Ack()) // 等待应答( E1 x: f, F) c6 _% b
{: n, V! F3 j: D: _: Y
MPU_IIC_Stop(); c( W$ O) q+ _) v4 M _" l
return 1; 6 {% C$ F% J; ~* {1 w! _
}+ A9 v2 j+ X. W/ [& K
MPU_IIC_Send_Byte(reg); // 写寄存器地址
9 `/ x; y W" R0 @& l- y
MPU_IIC_Wait_Ack(); // 等待应答
8 o8 \6 }0 O5 i G( `2 W
for(i=0;i<len;i++)
4 ^/ [% Q4 ^, ?% e6 R7 W9 w
{
: t* N& ]5 _: e+ X" n' m6 D2 V
MPU_IIC_Send_Byte(buf); // 发送数据
% O' [6 W$ r- X$ A5 i9 ?/ j
if(MPU_IIC_Wait_Ack()) // 等待应答
$ n. X4 D& w1 w2 V% p
{
$ A# v" v5 K- V
MPU_IIC_Stop();
3 `( W, u5 s8 _8 v. u2 c# a
return 1;
+ t0 H& [; Y, p% p+ m% ?# C( S
} 5 O0 T: [( m, s! C
} , i9 H3 g7 z3 Q4 p- ~$ _8 g
MPU_IIC_Stop(); 4 }7 N, U/ D' g" p7 N
return 0;
# a# U! d2 M- e; c
}
, x! F3 A( Q7 {; ?9 t. v
/*********************************************************************
$ g/ p) Z% J1 Z8 O9 A
函数名称：u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)* L# L7 j, e% h+ e
函数功能：从MPU6050连续读数据* q) v! F# @# f8 l+ H
入口参数：addr:器件地址；reg:要读取的寄存器地址；len:要读取的长度；buf:读取到的数据存储区* L5 ?) i! I2 [/ A: x( b
返回参数：u8 0正常，1错误
0 f8 D6 L" c9 p, l0 _6 J
修改作者：Aaron0 \+ a& F. i( B2 l# d2 U; J" x
**********************************************************************/% m9 }% F. r4 F
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
8 e$ N1 s) \7 |, B Z. \
{
/ G I, M. b e# z, U: w
MPU_IIC_Start(); , I$ j' y c" p, e+ U) _$ t" K- B8 S
MPU_IIC_Send_Byte((addr<<1)|0); // 发送器件地址+写命令 9 W$ S* l( ^2 B8 d
if(MPU_IIC_Wait_Ack()) // 等待应答
" k+ z8 [5 u9 x+ ^0 H
{1 g/ F( `) u+ p2 L9 P- l
MPU_IIC_Stop();
, z4 _* P" P: i* w' h3 U% Z
return 1;
6 L' J5 c' S& n- w
}: t( d, k+ n7 d0 e
MPU_IIC_Send_Byte(reg); // 写寄存器地址
. H" g. C9 ]$ V9 n! I4 E: e+ s0 P7 a
MPU_IIC_Wait_Ack(); // 等待应答) ?" g/ c* `; w# `* R* m& ^: B
MPU_IIC_Start();
# _( s+ M+ k: R
MPU_IIC_Send_Byte((addr<<1)|1); // 发送器件地址+读命令 $ ^. F( x: c/ Z" o
MPU_IIC_Wait_Ack(); // 等待应答 1 N5 g% t+ w0 M
while(len)2 f4 k$ i& k5 h' s& `+ ]6 R7 `! @: v( [
{
0 T# c0 Y0 q. b' W7 W
if(len==1)*buf=MPU_IIC_Read_Byte(0); // 读数据,发送nACK
: Q9 f2 h) ?: x( D- j/ O
else *buf=MPU_IIC_Read_Byte(1); // 读数据,发送ACK
5 B5 \* N4 B, k
len--;
S# C2 z; g5 {& y
buf++;
8 R* C& [7 p7 U' e: P0 j
} 2 d5 k/ N2 Q) B# g) n: g
MPU_IIC_Stop(); // 产生一个停止条件
2 Q# f5 \: `6 u4 N4 \( S; s
return 0; 4 I. d# |% u6 a' w
}- E$ o4 P# Y: b, ~" r
/*********************************************************************
. @) R; ?+ x' R& t
函数名称：u8 MPU_Write_Byte(u8 reg,u8 data) 6 L) T$ X- F7 _/ [
函数功能：向MPU6050写一个字节 0 T8 p; A! ^( p* n
入口参数：reg:寄存器地址；data:数据7 y$ X3 ]; l, ]7 f9 H0 q9 K
返回参数：0，正常；1，失败7 s. n3 G8 e: [5 h
修改作者：Aaron. N3 k' q) Y) B# P& t: w2 S
**********************************************************************/
4 Y- C8 Q4 p; D
u8 MPU_Write_Byte(u8 reg,u8 data) " c9 Z. u. A7 W
{ ; c& j' h$ W' i0 n0 S: t" k1 S
MPU_IIC_Start(); * X) E5 e: _& e/ K! O4 H
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0); // 发送器件地址+写命令
& O" C/ y; N3 F
if(MPU_IIC_Wait_Ack()) // 等待应答，0接收应答成功，1接收应答失败( A* T/ v* n4 y6 R1 u/ L
{% B6 g( M3 e; ]: O1 ]+ f
MPU_IIC_Stop(); // MPU_IIC停止信号
$ y# B0 L+ Q8 A; K, r& u
return 1;
/ O/ u) m/ r# W% o/ I/ Y
}' ~' I C; E0 Z
MPU_IIC_Send_Byte(reg); // 写寄存器地址
" l- W$ X% H( D& c+ \
MPU_IIC_Wait_Ack(); // 等待应答 9 I" B- E- ~& w$ D4 @+ K& _
MPU_IIC_Send_Byte(data); // 发送数据1 P) G& b: k e6 Q: m
if(MPU_IIC_Wait_Ack()) // 等待应答
0 ~/ w G* x! B* U% v; a
{
2 T' T4 [2 @2 y
MPU_IIC_Stop(); 5 [; [( S" @4 C& F: ~8 F: K
return 1; % U/ q$ D8 b" O+ _/ ^9 V' G4 V
} ) Q0 k0 Q. g% [: v1 _5 u
MPU_IIC_Stop(); 5 K3 Y9 T! W8 }2 C7 C6 l
return 0;* n& O' Q1 u' O, H, a6 i5 B* ]
}( t9 h/ T7 \0 V) [# t
/*********************************************************************' l7 M6 l, E0 Y) r4 k9 r( }
函数名称：u8 MPU_Read_Byte(u8 reg)
. ]) {. v" S2 Z. I3 k7 K4 ?! B
函数功能：从MPU6050读一个字节
4 s A: ~: o6 p+ ]$ d7 r8 ^9 v" O
入口参数：reg:寄存器地址
8 u/ [& v4 N; u, m' \' g
返回参数：u8 读到的数据
$ n H. C$ ~' _
修改作者：Aaron7 M% J& a' _% W5 U# I6 q5 s, d4 @
**********************************************************************/) a5 O% U* i7 \. e6 D
u8 MPU_Read_Byte(u8 reg)
6 }! k1 ?! t5 t
{
+ o0 G# Q- Z. ~0 a- q; c/ N: r
u8 res;1 q) d8 g* ^; y* o( W8 B! f6 P a
MPU_IIC_Start(); : F, F4 f" X2 n' ~/ C0 m& {
// 开始标志（S）发出后，主设备会传送一个 7 位的 Slave 地址，并且后面跟着一个第 8 位，称为 Read/Write 位。 0写，1读。3 D4 w6 \! i0 d$ U
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0); // 发送器件地址+写命令
' k, \- y9 w. y( ~+ _6 h
MPU_IIC_Wait_Ack(); // 等待应答 $ y2 u: [! F" @( X
MPU_IIC_Send_Byte(reg); // 写入要读取字节的寄存器首地址
s* m& A( J; z3 F; Z+ F
MPU_IIC_Wait_Ack(); // 等待应答& e: a6 W( r% z0 P+ u9 f+ l$ O# X1 _
MPU_IIC_Start(); [$ U7 d$ P! R) z8 k
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1); // 发送器件地址+读命令 & k p m+ L8 @1 [0 v4 Q) v. Y
MPU_IIC_Wait_Ack(); // 等待应答 2 A8 C2 I, g. o8 R
res=MPU_IIC_Read_Byte(0); // 读取一个字节的数据,发送nACK % }3 t3 u% c4 N
MPU_IIC_Stop(); // 产生一个停止条件
( H& \9 U: e z k6 b+ z8 Q8 }
return res; , k& x5 _" I6 D2 m- z
} U4 ^( o5 J: r+ J, x

复制代码

10.5 main.c主函数

/**& O* m- V9 t W
******************************** STM32F10x *********************************& e+ ^9 Y/ |9 n3 A2 s. I) ^5 ~
* @文件名称： main.c; t) O$ q( i9 `
* @修改作者： Aaron
% `5 b% j- q" Y8 s- @
* @库版本号： V3.5.0
$ Y g# L8 j+ X
* @工程版本： V1.0.0, z6 v7 n/ ^1 F! m. N6 ~
* @开发日期： 2020年11月30日; x! b6 k. P: s( J. u+ N& q0 ]2 Q
* @摘要简述：主函数，增加了在LCD显示6轴的原始数据4 u. c% |) v$ V& C
******************************************************************************/ S2 S/ q. j9 A/ E" K
/*-----------------------------------------------------------------------------
3 n" y" u( }, `
* @更新日志：
3 ]% N, ^* e/ @8 Z& l/ l
* @无 l" d% |' a! W1 i
* ---------------------------------------------------------------------------*/7 g0 S/ ?/ X1 U) o* I2 l8 A* j% ?
/* 包含的头文件 --------------------------------------------------------------*/5 ]+ e: u# T; a" O$ A3 M! i
#include "led.h"% C6 |, A: c( V* x6 I
#include "delay.h"
1 o3 a5 l) R( m7 s$ F. \: @. A
#include "key.h"1 y j; e( U2 x& D
#include "sys.h"% [/ l# R$ [4 H. Q
#include "lcd.h"/ c6 R1 R( l, |4 V: A
#include "serial_communication.h"
: Q) k2 `( S4 K4 t3 j$ O6 g! W
#include "mpu6050.h"
$ t( P9 L1 L8 D& M! a) f9 S
#include "inv_mpu.h"
r: x* r( _; `# E8 q q
#include "inv_mpu_dmp_motion_driver.h"
( e. q1 y$ f' ?
/*********************************************************************) z2 m, P6 V. M v; J& `
函数名称：usart1_send_char(u8 c)
) c" k6 H5 Y- }6 ?' `
函数功能：串口1发送1个字符6 @6 M$ b( O9 L( B
入口参数：c:要发送的字符$ O4 ^% n% r1 {' s
返回参数：0，成功，1失败
$ E4 o# u" M0 p4 g7 b8 W7 `7 H
修改作者：Aaron3 R/ p1 w; i, M' R2 {; F; C9 D5 `/ @) _
**********************************************************************/
- r5 p' j0 e% `% i+ G
void usart1_send_char(u8 c)
% x0 M1 `2 a! r9 m
{
9 W# ~4 I# R4 y* s9 a# o
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); // 循环发送,直到发送完毕 6 [& B4 c7 k/ Q* [2 `* N/ {# `3 ]
USART_SendData(USART1,c);
c7 Q5 j# w- ]5 h5 D
}
5 |8 Y6 t4 J, C: C3 o4 ^, F
/*********************************************************************
8 D& U7 J) V2 Q7 U
函数名称：usart1_niming_report(u8 fun,u8*data,u8 len)
: ~4 f4 _+ Q4 [
函数功能：传送数据给匿名四轴上位机软件
4 d5 ~9 L" W. B
入口参数：fun:功能字.0XA0~0XAF；data:数据缓存区,最多28字节；len:data区有效数据个数: L' a* o7 j/ b! m, s( n# I
返回参数：无1 D( ~0 v5 p) S) K2 e7 h! ^2 A+ Q$ H
修改作者：Aaron
* ^8 p: W0 N" @7 v. x8 I5 F
**********************************************************************/
; Q* U- x: U% \" I% ]. H- b1 X
void usart1_niming_report(u8 fun,u8*data,u8 len)
; Q& @! j7 H8 K N
{% @2 m! G3 C s0 y1 N, v( y1 P' m6 q) h: B
u8 send_buf[32];) y+ @0 E4 o# ?
u8 i;6 |% b! Z2 a! p- ]/ O
if(len>28)return; // 最多28字节数据 ' H- W4 }: A2 u8 D# ]& ^
send_buf[len+3]=0; // 校验数置零, d1 {9 E% v- g# E
send_buf[0]=0X88; // 帧头
& g; C* f2 l; D/ [$ u2 y" o
send_buf[1]=fun; // 功能字4 z! |8 ~2 r9 C
send_buf[2]=len; // 数据长度
: T1 L7 g2 m/ t. P& Z% n' `3 f
for(i=0;i<len;i++)send_buf[3+i]=data; // 复制数据
4 i$ E, l8 C. g8 @
for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf; // 计算校验和 & r( p7 O- b% A5 z6 ^% Q
for(i=0;i<len+4;i++)usart1_send_char(send_buf); // 发送数据到串口1 * P9 ?- m# O/ B# c5 O, `" X. Y5 O
}
' L/ ?. X3 \" m6 @2 f9 I: E
/********************************************************************* I- j8 b. _: k! u
函数名称：mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)4 O: V K- I5 D' V
函数功能：发送加速度传感器数据和陀螺仪数据8 R5 X6 k4 ~* [
入口参数：aacx,aacy,aacz:x,y,z三个方向上面的加速度值；gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
3 o3 E- H/ g; q: Q. }# K$ m4 A
返回参数：无9 [4 c$ j7 g! ~' m9 H
修改作者：Aaron9 ^2 G7 k3 `9 f S4 l7 m q. j
**********************************************************************/
) g8 D1 |. y5 P9 g7 S. [
void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz), \2 U& G- L. r
{' N4 A+ L/ C; [# N# U, G% c) y
u8 tbuf[12];
& p# k( y6 f/ d0 E5 j0 h
tbuf[0]=(aacx>>8)&0XFF;
/ b/ R' l5 x+ z5 a. p/ F* E% u. V
tbuf[1]=aacx&0XFF;
8 u' G7 t* H! G" m6 L
tbuf[2]=(aacy>>8)&0XFF;2 U* ?0 Y' }" i# t- Z
tbuf[3]=aacy&0XFF;
! Z$ |. J. _) q$ ~
tbuf[4]=(aacz>>8)&0XFF; N2 ?3 E3 U" w2 b. z' S
tbuf[5]=aacz&0XFF;
) c; K' D* e1 {; K5 L; O
tbuf[6]=(gyrox>>8)&0XFF;
' q/ t8 B- u# g+ l
tbuf[7]=gyrox&0XFF;
" t; B8 o6 W3 y+ p+ X. H
tbuf[8]=(gyroy>>8)&0XFF;
5 ]5 M Q2 K% w5 P% \6 h) k# }
tbuf[9]=gyroy&0XFF;( F2 w' }# k2 C! a1 u
tbuf[10]=(gyroz>>8)&0XFF;
# u. s% C* o2 E, c! x/ I. o5 P
tbuf[11]=gyroz&0XFF;5 `4 j0 y. u( r! |& c7 U' C- g
usart1_niming_report(0XA1,tbuf,12); // 自定义帧,0XA1
" t6 c. ?+ M9 W: P% s( E L+ M
}
1 L0 ]4 w" y# k1 G( s% m
/*********************************************************************
6 b2 c+ c- J& x. h+ V1 p
函数名称：mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)# M+ B( Y; b9 V
函数功能：通过串口1上报结算后的姿态数据给电脑
入口参数：aacx,aacy,aacz:x,y,z三个方向上面的加速度值；gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值；
7 P4 _: v8 J+ f( b
roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度；
$ O9 i& X: o+ L
pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度
8 R0 ^$ ~; }, U
yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度
/ }. |4 [9 h2 [( C' u
返回参数：无6 R# ^ V. g/ S/ X- \& o0 A
修改作者：Aaron
# {( r+ p8 B! h
**********************************************************************/
5 c" U% ~9 B7 b4 I' \
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)' I+ d9 e U) r0 M
{+ a+ _1 l n$ J
u8 tbuf[28];
6 b4 X. d9 }1 g- S0 z2 G/ D' ]
u8 i;
4 o9 f) ]) W' R. z S3 |* `
for(i=0;i<28;i++)tbuf=0; // 清0. X6 _' K+ Y4 K0 q; ?
tbuf[0]=(aacx>>8)&0XFF;
Y3 a$ t+ f B
tbuf[1]=aacx&0XFF;1 B: j9 g$ z+ U
tbuf[2]=(aacy>>8)&0XFF;" M$ Q; [: X4 w# b
tbuf[3]=aacy&0XFF;6 @6 X, R3 Y$ a! m$ l* E$ j0 \
tbuf[4]=(aacz>>8)&0XFF;' X3 ]* w0 f; e5 d6 z! u( y3 E: a
tbuf[5]=aacz&0XFF;
/ P! h' b0 Y! @/ i- z
tbuf[6]=(gyrox>>8)&0XFF;
* T% I# E0 n2 {0 _
tbuf[7]=gyrox&0XFF;
. ~5 U" s9 R" j& U' b: _4 z" N
tbuf[8]=(gyroy>>8)&0XFF;
/ U' P) r* m6 r3 k5 R7 \/ O' L. h
tbuf[9]=gyroy&0XFF;0 r1 j* U0 r, j7 V3 z6 o
tbuf[10]=(gyroz>>8)&0XFF;
0 \: j/ q$ y8 w. I% S* e
tbuf[11]=gyroz&0XFF;
1 n4 s/ U' m; t# x
tbuf[18]=(roll>>8)&0XFF;1 ?: V$ m# x v, `: m5 Y8 E8 J' p, N
tbuf[19]=roll&0XFF;$ k& G7 @/ S! \1 u3 K, G; j
tbuf[20]=(pitch>>8)&0XFF;
- l6 c" M. _4 V2 b( w; }
tbuf[21]=pitch&0XFF;
# Q0 E1 {6 {9 T s" J
tbuf[22]=(yaw>>8)&0XFF;1 k+ D9 o; y% w7 r
tbuf[23]=yaw&0XFF;
8 J& `9 f! B. ]& t# Z+ p# v) l
usart1_niming_report(0XAF,tbuf,28); // 飞控显示帧,0XAF$ U" D5 W' }2 W
}
5 y3 z- i# w& V7 M; e
/*********************************************************************, W8 s" Z. }2 W6 e* r, z8 H' U& o. ~2 [
函数名称：int main()7 }6 X" W+ e0 k0 s2 e
函数功能：主函数,增加了在LCD屏上显示6轴原始数据的功能$ J/ N+ m' \* g/ `; P7 \2 [8 Z4 [
入口参数：无
% N4 }5 h/ b+ K# I% S9 i6 A
返回参数：无1 V. s1 u; P6 Y: O$ \/ p& H* U
修改作者：Aaron
4 x* z+ \% s5 V
**********************************************************************/
0 {+ I4 b1 [ _2 k7 h b% Z
int main(void)
2 J! e2 q4 m* g" a R- {1 ]
{
7 E: m' H8 V8 V+ ?+ B" F/ V
u8 t=0,report=1; // 默认开启上报，发给匿名四轴上位机软件. _( D$ a5 w4 n% k) n) G
u8 key;/ `1 p& r* v% H; Q
float pitch,roll,yaw; // 欧拉角
9 g8 r6 U9 x6 n* B$ H" d. {
short aacx,aacy,aacz; // 加速度传感器原始数据
3 M4 U2 M) b" N( P* T# v# Q
short gyrox,gyroy,gyroz; // 陀螺仪原始数据
. E7 C8 v# p1 B8 V; v7 R$ J; A( I+ A
short temp; // 温度
% {0 _6 p# {3 y/ D1 |, B/ q3 @
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 设置NVIC中断分组2:2位抢占优先级，2位响应优先级
( u( Z! w% v. b5 a+ u) @
My_USART1_Init(); // 串口初始化为500000
3 L3 }' x9 o% j* M3 d7 \: s
delay_init(); // 延时初始化
0 w7 Q. h7 ?" j* R6 t- I0 A
LED_Init(); // 初始化与LED连接的硬件接口. T' J* k2 Y8 u1 \3 a' {4 X2 F5 k
KEY_Init(); // 初始化按键
$ A3 H: y+ d0 p2 h1 j2 E W
LCD_Init(); // 初始化LCD $ z2 W: A2 `( T6 k5 w1 V
MPU_Init(); // 初始化MPU6050
8 a" C4 t6 b9 h- W' d
POINT_COLOR=RED; // 设置字体为红色 . E% F) A5 G0 z9 M- h9 Q0 W
LCD_ShowString(30,70,200,16,16,"MPU6050 TEST");
" t j& ~1 Q7 b0 S& X
LCD_ShowString(30,90,200,16,16,"2020/11/30"); 8 ?8 [; W; R% M) R
while(mpu_dmp_init())
) W& k9 m+ ?4 |1 `* q4 y: _
{
) u" m5 d" d/ |+ t# A
LCD_ShowString(30,110,200,16,16,"MPU6050 Error");
) ^' P F$ G9 j- s" _& ]" L
delay_ms(200);
+ j3 W- [3 G4 J- v% Z4 _% E P
LCD_Fill(30,110,239,130+16,WHITE);7 n0 v, f, e+ f( F2 {
delay_ms(200);
4 H: I3 M+ m9 d' o F& n
} ! I! d6 ?2 H; a- v. r& W0 g; q; H+ F
LCD_ShowString(30,110,200,16,16,"MPU6050 OK");
LCD_ShowString(30,130,200,16,16,"KEY0:UPLOAD ON/OFF");
& M5 j- N7 A( A7 u* U
POINT_COLOR=BLUE; // 设置字体为蓝色
/ X# V) |7 F B; h. |- x
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
3 D* J/ U5 H6 o" @* f
LCD_ShowString(30,200,200,16,16," Temp: . C");
% r" l0 b# g# ]5 T" s# F
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
$ a2 a N0 J' R
LCD_ShowString(30,240,200,16,16," Roll: . C"); 1 @" m, R. f- ~/ |$ Z
LCD_ShowString(30,260,200,16,16," Yaw : . C"); k% C! [' a0 p
while(1)$ P& r; W: [7 v1 @9 I
{
4 o" U& t$ }0 m( u2 R, |
key=KEY_Scan(0);
/ o/ ^: C9 T, V" N
if(key==KEY0_PRES)6 G) J8 x9 Q+ h# ~# g+ U5 k
{
; D: f( v+ N/ X! N' Y% c
report=!report;! k# ]9 I9 d' Y
if(report)
' C: A7 N) F" C7 S* e: Y4 Y! R
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");9 X* t. l* j- J( ^
else . o2 n# v" ]. }, J3 t P8 C
LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
$ i \5 b( z* S* Z
}
$ Z- J, \1 \+ Z6 M
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
' m ^( m M: P* f( e5 x
{
6 @- s0 k" x+ F
temp=MPU_Get_Temperature(); // 得到温度值 7 T1 r8 F1 i" L0 I" {! a( z
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 得到加速度传感器数据
+ u; q/ y2 u6 m. T7 m
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); // 得到陀螺仪数据6 m3 E' E- s; k U
if(report)
' _( I! m6 B8 q x% v) g
mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz); // 用自定义帧发送加速度和陀螺仪原始数据" w2 G1 y- O V# L1 `' J
if(report)
4 t; B+ Q8 `! M+ R, g8 h5 ]6 |
usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));/ y! J- }: _& |% f
LCD_ShowString(30,300,200,16,16," aacx: "); ! ~3 c. s/ `+ @
if(aacx<0)* c V+ i) M7 E. N0 Q% T
{
* J& K& X. c" e$ c6 F
LCD_ShowChar(30+48,300,'-',16,0); // 显示负号
3 k( ^& _) @- P- \ s
aacx=-aacx; // 转为正数- `# O5 \- i, M: j: y( X0 l8 X
}$ N" J. u/ i( u5 L. g& w
else ' i. X0 p) g4 h" G; \1 n5 W
LCD_ShowChar(30+48,300,' ',16,0); // 去掉负号
! l7 ~- y' i( i% Q0 t
LCD_ShowNum(30+48+8,300,aacx,10,16); // 显示aacx
% F6 r* x f8 U( ?( i; Y. Q, T
LCD_ShowString(30,320,200,16,16," aacy: ");
2 w8 C2 p3 q O' i
if(aacy<0)
) ]: j5 L7 P- E- _ Z
{! y5 h' Z* G! A4 s+ @4 ^/ t; H
LCD_ShowChar(30+48,320,'-',16,0); // 显示负号
2 \9 N- B: D9 Z4 X" Z5 |/ Y* W
aacy=-aacy; // 转为正数* k6 {0 O: L' f
}7 V2 ~! X* O) q) {% ?
else
* d0 b* N. T7 F0 T
LCD_ShowChar(30+48,320,' ',16,0); // 去掉负号 - S$ V7 A0 D1 i1 t4 U( n
LCD_ShowNum(30+48+8,320,aacy,10,16); // 显示aacy $ @6 I' k3 ~4 W& P6 i& b1 |% i
LCD_ShowString(30,340,200,16,16," aacz: "); 5 x* j- j Y( W
if(aacz<0)
& i" W; I x5 q
{
% z; s8 m- z( }$ b# b# l9 Q# w
LCD_ShowChar(30+48,340,'-',16,0); // 显示负号
4 p4 F J1 d5 \: v( G; l
aacz=-aacz; // 转为正数
. Y% J) d, K. W6 y5 `- z6 N
}
$ w/ |1 d% ?+ C0 D E0 J
else
! E4 ~2 Z# }6 O- Y; h$ @$ l
LCD_ShowChar(30+48,340,' ',16,0); // 去掉负号 6 ?. {! E3 B N9 x( W
LCD_ShowNum(30+48+8,340,aacz,10,16); // 显示aacz , Z6 K1 F5 |1 A: J, D
LCD_ShowString(30,360,200,16,16,"gyrox: ");
9 {: x( o# Z' Z, z$ }
if(gyrox<0)% P1 r( j6 w S s
{
4 g2 h% [3 @/ ]1 A
LCD_ShowChar(30+48,360,'-',16,0); // 显示负号3 T$ C* F9 |! d) j! f g
gyrox=-gyrox; // 转为正数
: S3 t( ?, T4 A# P! y
}
. _/ W0 T2 k* b# s' |$ N
else + i+ B/ P& a; l5 b g
LCD_ShowChar(30+48,360,' ',16,0); // 去掉负号
& s. k& e1 l$ j0 l* U- K
LCD_ShowNum(30+48+8,360,gyrox,10,16); // 显示gyrox - r. u* Z$ w- j- u
LCD_ShowString(30,380,200,16,16,"gyroy: ");
7 i, L" ~/ ?" r% M3 u
if(gyroy<0)- {& H A/ u% [+ T, |
{
' G7 G+ a& Q6 y: G- T' ^* i0 T1 d' A& t
LCD_ShowChar(30+48,380,'-',16,0); // 显示负号
; P+ t# e0 a4 R% ^. }8 E
gyroy=-gyroy; // 转为正数
+ ]' M. g% M$ U+ M3 W
}0 T& S. M- l+ F, t6 V1 I
else
% X7 q( M. K4 x; S5 J
LCD_ShowChar(30+48,380,' ',16,0); // 去掉负号 , j6 P0 H: m2 R
LCD_ShowNum(30+48+8,380,gyroy,10,16); // 显示gyroy 0 i- f" Y/ m/ D' A' O5 x
LCD_ShowString(30,400,200,16,16,"gyroz: ");
/ g* {# N( H( f, t: |- V1 `
if(gyroz<0): ^4 A' `* x- ?6 Y, \! E
{
! J3 T3 g Z& o- P7 K9 h* [
LCD_ShowChar(30+48,400,'-',16,0); // 显示负号
# Z+ {, h1 [% Z& Y7 c7 r
gyroz=-gyroz; // 转为正数/ d8 y+ B/ u- N' z+ _7 E0 O
}
+ y5 N, @' T; B- E5 f! `+ E
else
@ \% z9 @4 ]4 B
LCD_ShowChar(30+48,400,' ',16,0); // 去掉负号 # g: E5 R: A0 b8 S8 ]* N4 g
LCD_ShowNum(30+48+8,400,gyroz,10,16); // 显示gyroz
5 x0 [% w" q# X, k
if((t%10)==0)
9 O3 V+ @3 W/ }4 x# l3 }
{ 5 D2 T8 u) C+ y: x' s, q4 \# C
if(temp<0)
1 ?- Q; C% }2 O7 Y- s* m
{$ e# C; O3 j+ {' n0 K
LCD_ShowChar(30+48,200,'-',16,0); // 显示负号
6 }6 M: A6 e( h6 E9 n S- P7 k
temp=-temp; // 转为正数1 b0 H: B1 j- A( v2 x5 _
}
P& _' r) x/ X' U/ |2 T
else $ K3 |" b, j0 w9 }2 a
LCD_ShowChar(30+48,200,' ',16,0); // 去掉负号
' R! a2 ^) \( Q+ s7 e4 k+ _
LCD_ShowNum(30+48+8,200,temp/100,3,16); // 显示整数部分 5 J9 `6 i k$ U7 d5 _
LCD_ShowNum(30+48+40,200,temp%10,1,16); // 显示小数部分 # @! H9 B: T$ n% O+ A' ~4 {
temp=pitch*10;
$ \: ]) l- W1 m! k
if(temp<0)/ a1 q" n- O& D: d4 v+ V1 p0 |
{, U4 D+ T: z B: f3 K, m
LCD_ShowChar(30+48,220,'-',16,0); // 显示负号5 F, o2 p) v0 N! z+ M/ _
temp=-temp; // 转为正数
9 [# j( s0 u6 q; i
}
; D# U; }+ C1 {0 ]& u2 @$ B& I
else + z5 U- s4 a; p: o' K
LCD_ShowChar(30+48,220,' ',16,0); // 去掉负号 7 d1 C% s1 B5 Y$ A# }; z. k
LCD_ShowNum(30+48+8,220,temp/10,3,16); // 显示整数部分
# _' V4 }3 m- p6 y( v( a
LCD_ShowNum(30+48+40,220,temp%10,1,16); // 显示小数部分 0 T! r0 {2 P4 J
temp=roll*10;; D- u6 O1 H: C( ]" g4 S
if(temp<0)
' s' ?. K7 u4 X
{
0 z3 f% E" u: m/ y
LCD_ShowChar(30+48,240,'-',16,0); // 显示负号
# p. X2 t4 N3 [5 H9 A9 j- b
temp=-temp; // 转为正数1 }: X" ~3 s% g/ r) p: y- o
}0 T, ~& m: G) y. ?1 e" {
else
9 @3 z( k% v+ Z$ @; ~+ z! {
LCD_ShowChar(30+48,240,' ',16,0); // 去掉负号
7 ^/ c. U: X A* D" E+ X7 A2 b
LCD_ShowNum(30+48+8,240,temp/10,3,16); // 显示整数部分 3 R+ H" @. e+ W$ X. V! p3 ?
LCD_ShowNum(30+48+40,240,temp%10,1,16); // 显示小数部分
0 y/ h7 Y$ ?* A( J+ w: H
temp=yaw*10;
- K. j) w" i% X: C
printf("temp=%d\r\n",temp);
+ X% B, y8 w6 l5 Q
if(temp<0)
) V1 j3 E# a, T9 K& y6 J" V; t8 B
{
& F- H2 Q/ A. b D o( Z$ [# T0 ^
LCD_ShowChar(30+48,260,'-',16,0); // 显示负号: @0 E. E2 V" H; a* J
temp=-temp; // 转为正数; x# Y/ J e$ ^, N" b
}3 _# A# \1 }( x4 S" x- s* Z
else $ l8 v& f/ ] W4 ?" N; M
LCD_ShowChar(30+48,260,' ',16,0); // 去掉负号 & [) R2 j; ~: y+ I5 t$ x' I, A
LCD_ShowNum(30+48+8,260,temp/10,3,16); // 显示整数部分
- }7 ]* \8 w7 g* m5 @8 _& K
LCD_ShowNum(30+48+40,260,temp%10,1,16); // 显示小数部分 4 T& z9 Y. o6 m5 u
t=0;8 p2 N: U( ?! `: [0 g
DS0=!DS0; // LED闪烁
4 j: z# ?1 U1 L; q+ Z
}) [/ ^" m- l' `- n( R- ^
}
H" U. {; a) G! U- A, k1 c" D
t++; 9 }. `/ ~ V. D! c* o: u3 P1 u
} , S' @+ \ R$ e" ?# Z d7 S0 g
}
8 M3 D. g; A; o/ d4 ] ?/ z

复制代码

10.6 移植文件
文件中代码太多，这里主要关注：
1）inv_mpu.c源文件中定义了四个函数：

#define i2c_write MPU_Write_Len // 对应mpu6050.c源文件里的MPU_Write_Len
( M0 S$ _2 D+ ?. o, z4 z" f
#define i2c_read MPU_Read_Len // 对应mpu6050.c源文件里的MPU_Read_Len! x/ |9 F) p: u O/ t6 M
#define delay_ms delay_ms // 对应delay.c源文件里的delay_ms
& P2 O; T* s' D2 a% Z
#define get_ms mget_ms // 这是一个空函数

复制代码

2）inv_mpu.c源文件中u8 mpu_dmp_init()初始化函数：

u8 mpu_dmp_init(void)
3 m x$ X' u9 C0 P3 _0 b- M( {2 a: Q
{
* S) H- B2 L2 k, v+ m- y. N
u8 res=0;
! {$ f- T0 Z2 U8 R, t. Q
MPU_IIC_Init(); // 初始化IIC总线; u. l) L& n. q. C. ]
if(mpu_init()==0) // 初始化MPU6050
6 d) |" f5 y [
{
, p0 X5 b4 ?9 w7 Y
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL); // 设置所需要的传感器
|/ }1 X' V- P: f: ]! y% r0 [
if(res)return 1; 2 H {4 q, ~3 R6 x* n
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL); // 设置FIFO
2 v2 G7 }2 e v: ~' I* G
if(res)return 2; ! t4 d4 ^8 ^: I3 b; |
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); // 设置采样率% u) t9 q. U% x! l* s
if(res)return 3;
e- Y* d- N5 x8 q
res=dmp_load_motion_driver_firmware(); // 加载dmp固件
9 M. o* k ~# I% ?! z* n8 v
if(res)return 4; . G" n0 x' Y7 f8 y% B
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪方向3 Z. |( D4 ^: b1 }( u2 ]
if(res)return 5;
5 t7 X( d8 a* I- L9 J6 t
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| // 设置dmp功能% ?8 s8 B4 d4 v8 k8 Q. @$ I9 S
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
; _' s) q& m! c9 G
DMP_FEATURE_GYRO_CAL);# x$ g* F9 }0 l7 L- B* X
if(res)return 6; 0 t* O# j' \, X5 W# t" c* `2 q
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); // 设置DMP输出速率(最大不超过200Hz)
0 D8 l( r' m4 u% z, O; I: I. o
if(res)return 7;
8 K6 Y) F4 Q% g* Y9 s" h$ J6 {
res=run_self_test(); // 自检0 A+ j. I7 I2 I2 K2 e9 y
if(res)return 8; 6 G; J/ K- [: h' {& c
res=mpu_set_dmp_state(1); // 使能DMP
' \4 z- u# w. D& G U& Z5 ^# Q; y
if(res)return 9;
" N7 v3 g- Q& @/ V( \
}else return 10;
' X& e8 g5 A. C
return 0;
+ }6 _2 ?' y! ]* y( u/ x9 O d0 W- c
}

复制代码

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)
: U6 y' K" W, L9 N: }
{0 r, ~$ g- v( N" z1 O
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;# o! Y X% W+ ~
unsigned long sensor_timestamp;0 _7 b8 X( r+ n) w" h
short gyro[3], accel[3], sensors;; L5 _2 P! _- ~0 c* l
unsigned char more;
& X# `" o" ?8 D$ j+ U, W6 L
long quat[4]; % A( i" f* M2 W$ i- c4 g
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more)) z9 [1 O7 a) J4 ~8 d4 J
return 1;
* F" w+ ~. W) ]/ q" g. x0 ~
ors&INV_WXYZ_QUAT) 6 ] d5 A( V9 }% T$ B
{
- s* F6 ^" e9 K% c( W! \
q0 = quat[0] / q30; // q30格式转换为浮点数
, Y5 {+ Z: L2 m6 f; q
q1 = quat[1] / q30;
r8 e4 J- L" G: [! G3 `+ y N3 X
q2 = quat[2] / q30;
; R- W( k3 p0 o B( R9 W. Q
q3 = quat[3] / q30;
7 }. z' Q3 v# f+ g8 q3 l/ B- i8 r
// 计算得到俯仰角/横滚角/航向角
4 _" f T/ q& m4 y$ J# t1 ^% Z
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch2 n' W, H+ J9 c1 A# R
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
3 q" C7 d2 b4 M* H' r( X
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
! K( X# F! Z1 m8 j: ?
}
% t* o D$ g, o& @
else
G9 D9 h# U8 t7 j
return 2;' X1 Q4 Z( K5 l
return 0;7 p. w7 c1 Y) h, ^% m) W* g
}