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格式转换为浮点数, y. W9 L) e# e- q/ ~; h
q1=quat[1] / q30; // q30格式转换为浮点数7 ~) @8 u& w+ W, f" U. U
q2=quat[2] / q30; // q30格式转换为浮点数
8 X. C& ?; _) x; v8 W0 J" ?/ [1 R9 J1 U
q3=quat[3] / q30; // q30格式转换为浮点数5 i+ O: a4 i' r
// 计算得到俯仰角/横滚角/航向角
; Z" ]9 C2 n/ n0 t7 |
pitch=asin(-2 * q1 * q3 +2 * q0* q2)* 57.3; // 俯仰角7 y' y Y3 I: k3 y2 J0 C
roll=atan2(2 * q2 * q3 +2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // 横滚角yaw=atan2(2*(q1*q2 +& w) w4 s; L+ s
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头文件函数

/**
7 D' x7 Q, q: q2 N; z
******************************** STM32F10x *********************************8 t% h- ^. [2 s! f8 I- r* ]# K
* @文件名称： mpuiic.h
1 Y" t6 I8 `0 p6 K d
* @修改作者： Aaron/ @' n9 D+ @4 q2 \$ J
* @库版本号： V3.5.08 h3 L& V( R' g& Q* `, j6 m
* @工程版本： V1.0.0
- }6 M" w9 T& ]+ w2 [2 P! I
* @开发日期： 2020年11月30日
, a2 r4 E; H* i
* @摘要简述： mpuiic头文件,主要跟MPU-6050模块进行IIC通讯时用 U, N! k$ Y1 T! S) U H
******************************************************************************/
$ Z9 X4 |! V6 M( E- Q t6 K2 G
/*-----------------------------------------------------------------------------3 ]3 S# c; T8 \) j
* @更新日志：
$ u, f) V/ v1 n; C3 L5 Q* H! Y0 i( b
* @无
; y/ k4 h# x5 d& Z
* ---------------------------------------------------------------------------*/
+ n0 {' N: {3 W1 u& @+ k T* j
#ifndef __MPUIIC_H
6 X* `6 y9 M3 O$ ?/ }; [% U
#define __MPUIIC_H2 K3 R- u6 B2 ~) g2 ?
/* 包含的头文件 --------------------------------------------------------------*/
$ V7 _9 }4 n. O6 O7 P: H
#include "sys.h"3 z" A) Z- `% |9 b; n
#include "stm32f10x.h"# O1 X1 Z# p2 i9 K
/* 寄存器方法控制GPIO管脚输入输出模式 ----------------------------------------*/
3 o, y4 H; @& Z: H8 D* s
#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;} // PB11 上拉输入模式4 `) j+ R" _6 i. Y, |
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;} // PB11 通用推挽输出模式，50MHz0 S$ u; f: V2 t4 T! _
/* 位操作方法定义 ------------------------------------------------------------*/ % o; k! v" t! E' u* j2 Y: S' Z
#define MPU_IIC_SCL PBout(10) // PB10对应SCL0 {/ @- z* H& ?
#define MPU_IIC_SDA PBout(11) // PB11对应SDA，输出SDA
, _* B7 T' C* s' K J) f
#define MPU_READ_SDA PBin(11) // PB11对应SDA，输入SDA
( ?* ?7 i1 ^1 c& {
/* 定义IIC底层驱动程序 -------------------------------------------------------*/: `6 C1 u: }: T4 y% o/ J- ^
void MPU_IIC_Delay(void); // MPU IIC延时函数，延时 2 us; \, x6 E8 U+ p: K; L
void MPU_IIC_Init(void); // 初始化IIC的IO口
+ z' K; P" z( T3 @0 t3 V" P
void MPU_IIC_Start(void); // 发送IIC开始信号
5 A) V8 t+ ~5 ` ~' Y
void MPU_IIC_Stop(void); // 发送IIC停止信号$ \4 W- h- R$ O
u8 MPU_IIC_Wait_Ack(void); // IIC等待ACK信号
! Q. [7 z: e o F0 N z! \
void MPU_IIC_Ack(void); // IIC发送ACK信号
void MPU_IIC_NAck(void); // IIC不发送ACK信号' T) x. E N- @. Y( U7 \+ E2 `& }
void MPU_IIC_Send_Byte(u8 txd); // IIC发送一个字节
( p m0 z5 A, @5 f) ~; |
u8 MPU_IIC_Read_Byte(unsigned char ack); // IIC读取一个字节
) t3 p, _ K2 U7 ]
#endif /* __MPUIIC_H */+ @7 g8 @- Z& q$ ?6 l1 j3 [
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/

复制代码

10.2 mpuiic.c源文件函数

/**
6 {6 a- P! O: @
******************************** STM32F10x *********************************
9 L9 w' u' S& _
* @文件名称： mpuiic.c
6 U I* T) f/ w* o$ T$ b
* @修改作者： Aaron
; I2 w, [5 j; w0 I, \3 L& H
* @库版本号： V3.5.0, w8 T; g$ I& U- c; {- c' W
* @工程版本： V1.0.0
4 w% K# t# ~8 z z0 W
* @开发日期： 2020年11月30日
J. k8 ]8 [- M p
* @摘要简述： mpuiic源文件，主要包含与MPU-6050进行IIC通讯的IIC底层驱动程序7 M+ }+ b% D. ?: P- o
******************************************************************************/) b7 q# c/ D7 A% G
/*-----------------------------------------------------------------------------# d- J U& Q$ {: {) n: d. d/ V
* @更新日志：+ Q+ w' X" l. I2 d# p8 u% U
* @无
5 g5 k5 V1 N% H; X% @
* ---------------------------------------------------------------------------*/
9 e, ?" k. C5 n( q7 i9 l
/* 包含的头文件 --------------------------------------------------------------*/* W% S [* D- m x- ~$ D& B- ]
#include "mpuiic.h"
( [; W! N# l# M. }& L" N$ { ~% f
#include "delay.h"
; d3 x) L& A6 {) `% T( E2 c2 I9 I
/*********************************************************************
* C" i! y" M/ D' o. X6 p' J
函数名称：MPU_IIC_Delay()5 U! t5 O& s' d5 @6 R q! ]+ t
函数功能：延时函数，延时2微秒（us） `/ V$ [: v) }& c0 C" w
入口参数：无
! c" ^, S2 Q$ s; g6 l3 `4 {+ P
返回参数：无; p! m( k6 h; B+ v
修改开发：Aaron1 G: _4 d' T9 v8 C4 W: _
**********************************************************************/5 G9 a5 \, h; v6 g
void MPU_IIC_Delay(void)* p' w% ~) a6 c l( k) p) V
{+ H, M. X, H, Z/ k9 P
delay_us(2);3 s, e9 S! ~8 g& `/ Z0 s* }# V) c
}8 i( Y0 F' T6 N; m. v* p+ c
/*********************************************************************4 x: x- \2 M3 M& X: p1 x
函数名称：MPU_IIC_Init(). |- _& Z# p* `7 E! l& f. y0 j
函数功能：MPU_IIC管脚初始化配置
$ D: }' I* S' `: l* j% G
入口参数：无% [: G; E- _ f4 } _* d
返回参数：无
0 K9 P3 Q' n Y& ^
修改作者：Aaron
8 [" i! _( f$ q) e5 p( k, g
**********************************************************************/
0 @$ U2 D6 {% q8 c# A/ U4 n
void MPU_IIC_Init(void)/ e0 _* i' `+ d) ]4 _1 h$ k
{
) R9 O" R/ A3 S; R A+ k
GPIO_InitTypeDef GPIO_InitStructure;
- U& l- `- n8 J& l0 F% Z: d
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE); // 使能外设IO PORTB时钟
+ O7 ^- w( g# Q5 S, D( Z
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; // 端口配置，管脚10和管脚11
# c& ]* ^, w* C3 {
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 推挽输出$ }5 ]' G& W1 l
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // IO口速度为50MHz2 d3 E2 J7 [% \" L1 v9 e' n6 Q
GPIO_Init(GPIOB, &GPIO_InitStructure);
- C. w% u! w. X. B' \$ Z- R" s
GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11); // PB10(SCL),PB11（SDA）输出高 9 L+ M. A5 N& p. x) E# N; Z
} ?5 [4 |0 u# l; M1 g
/*********************************************************************2 H4 U; a' V) m b1 o
函数名称：MPU_IIC_Start()) C# r. ?. X: q
函数功能：MPU_IIC起始信号
7 K5 V4 `. D8 F8 {
入口参数：无
' |' d& ?: L3 {9 ^
返回参数：无7 W! {6 V2 `$ m0 f2 `: t) s
修改作者：Aaron+ X( z# R% o; ~
**********************************************************************/; c! v. D% }- A$ Z1 j5 T- O
void MPU_IIC_Start(void)" |) A! ], k& k! F- h* T
{" m/ i; k+ U; U! C$ Z
MPU_SDA_OUT(); // SDA线输出模式$ {% R0 j+ U! z5 I
MPU_IIC_SDA=1; // SDA输出高 ; O! I! r8 h0 I$ Z, ^
MPU_IIC_SCL=1; // SCL输出高，此时为空闲状态
$ d7 h. Z I# v# V
MPU_IIC_Delay(); // 延迟 2 us+ p0 \) a8 v% x
MPU_IIC_SDA=0; // 在SCL输出高的状态下，SDA由高至低跳变，IIC开始0 k' z5 l" z7 J v- q( J
MPU_IIC_Delay(); // 延迟 2 us1 \* N* _- y6 J* `+ G5 x/ e
MPU_IIC_SCL=0; // 钳住I2C总线，准备发送或接收数据
1 `5 W* ~: [- ~7 ]' d3 R
}
& i7 y5 ~1 A; J1 l' D7 q. T
/*********************************************************************
. x8 D( M6 D0 Q2 u3 ?. m* w
函数名称：MPU_IIC_Stop()2 O& ]$ S7 L4 a# p' @
函数功能：MPU_IIC停止信号
/ f4 x% p9 W# j" [8 [4 u/ F
入口参数：无
5 ^$ _* r! I X
返回参数：无* d! E# ?: @! B7 s0 p. J7 `+ l$ f
修改作者：Aaron
1 e& M7 l* @1 D: V" e! k0 W
**********************************************************************/
/ }3 G: J( b9 Q0 W/ T2 n
void MPU_IIC_Stop(void)
$ M* z r, D- O3 @1 p3 |9 |
{
# b2 r& C3 Y; o" n4 P h
MPU_SDA_OUT(); // SDA线输出模式+ e; o0 b' A; E3 l
MPU_IIC_SCL=0; // SCL输出低0 c }# a3 _+ w1 C8 B! x; U
MPU_IIC_SDA=0; // SDA输出低! Y1 a" |. K5 F9 V- x! b
MPU_IIC_Delay(); // 延迟 2 us
* v8 G- ?9 l' p' k7 B* B& U+ T. |# r
MPU_IIC_SCL=1; // 先SCL输出高，在SCL输出高的状态下，SDA一直是低电平/ m+ @! U& f, X% G& H/ r
MPU_IIC_SDA=1; // SDA输出高，发送I2C总线结束信号
& q3 I) z2 t) J5 i
MPU_IIC_Delay(); // 延迟 2 us
% a, P+ k, e* ~0 t+ H2 x6 @
}2 S1 |! _8 }: Y4 @) ]6 s* V
/*********************************************************************2 r- U! O4 P1 ~( P
函数名称：u8 MPU_IIC_Wait_Ack()+ f; v+ S' e; s# U
函数功能：等待应答信号到来: f. D* R) Y- V4 b! t* n) O
入口参数：无
3 {' B J$ K# H8 b8 e( V6 D
返回参数：u8，1，接收应答失败，0，接收应答成功
$ O" _% `0 {+ T) Z
修改作者：Aaron0 n1 _, e" h. q; J( @/ {
**********************************************************************/* H; ]8 I- k U4 W
u8 MPU_IIC_Wait_Ack(void)
- \8 \8 R- \8 C0 n' C) `& Z* v7 L O
{- z4 C" \, y$ k. Z
u8 ucErrTime=0;" p: r- t7 I7 k6 t5 y5 Z3 N. W# g
MPU_SDA_IN(); // SDA设置为输入 ' N' s2 ~2 R$ @4 D. ~6 J
MPU_IIC_SDA=1; // SDA设高电平) h& v* q2 v3 ], u, l1 d$ z
MPU_IIC_Delay(); // 延迟 2 us
! j; {5 w, ~$ u# ~/ `
MPU_IIC_SCL=1; // SCL设高电平' I l1 b+ Z; O$ n4 @
MPU_IIC_Delay(); // 延迟 2 us 7 t+ @; @2 i) B( \! F
while(MPU_READ_SDA) // 读取PB11的值，一直读PB11=1，高电平，则不断循环，直到溢出
6 R& s' h: @8 u, u e4 M( u/ v" e
{
1 s) p8 l6 S/ L
ucErrTime++;
" x: {- ~6 U/ B# d& @
if(ucErrTime>250)
0 [9 U3 i, j' o+ W( l7 R7 M
{" _; Y4 I$ F& _- h- l
MPU_IIC_Stop();3 J/ S, F& w# \
return 1;
& s+ E- `8 e6 u) e
}
( }, k# r' H& l
}; F4 o% N. I1 t) i) d+ i, Y
MPU_IIC_SCL=0; // SCL设低电平 9 R# D/ V2 a0 s/ `1 [
return 0;
( Z( A- q9 @9 r7 M8 V( F
}
5 F3 J9 K3 [) s9 X
/*********************************************************************
. Z% ^% J* z/ Q7 V) }/ s; U+ O, p: X
函数名称：MPU_IIC_Ack()
+ O2 v/ H: w a% x( U9 R
函数功能：产生ACK应答
- m: P' T+ v' @- y+ f+ [+ I
入口参数：无+ h; H7 r4 r1 u; Z
返回参数：无
1 g& p1 b- T- [( i% M
修改作者：Aaron
* K1 d! J& e7 f; U* `- h
**********************************************************************/) I5 N! C1 `5 ~+ Y
void MPU_IIC_Ack(void)9 {. G- l4 Z$ v! e$ R! ^* a1 m8 o
{8 N# f {% x" H: `5 K" @" A
MPU_IIC_SCL=0; // SCL设低电平6 E* V$ ]: ~. b! G' t. z
MPU_SDA_OUT(); // SDA输出模式
: l% p6 y# f( M7 R
MPU_IIC_SDA=0; // SDA设低电平
% p+ V8 u: i" o: W7 U0 q) ^ _
MPU_IIC_Delay(); // 延时 2 us/ d& u3 G a: t7 z) ]* D8 w3 i8 _
MPU_IIC_SCL=1; // SCL设高电平
8 C$ Z0 c& o- Q& m. g! d
MPU_IIC_Delay(); // 延时 2 us1 a- F# ^% x$ y' {: Y ~7 X2 S
MPU_IIC_SCL=0; // SCL设第电平，在SCL高电平期间，SDA为低电平，则说明有效应答
R n, d+ h) L0 l/ p0 j7 r& G
}
5 F4 ]! i9 T) a" m: H
/*********************************************************************
~+ W: X: [/ B3 C4 {4 h* n; G5 N
函数名称：MPU_IIC_NAck()) L( R- s( |+ C* @; D1 T& g3 N
函数功能：不产生ACK应答" k) ~& Y5 z% u$ O) V" [/ s
入口参数：无8 ~3 Z& B2 _8 v# X+ S7 N
返回参数：无9 M. t8 E3 F8 u1 b; m% F' G2 b4 I8 P
修改作者：Aaron
) f1 p: o" o1 ?5 `, G% n
**********************************************************************/3 P, j. z/ z: W% j& X* @
void MPU_IIC_NAck(void)
6 _2 [0 C5 n6 h8 F# | u2 s
{0 l: k. H- L; D5 J
MPU_IIC_SCL=0; // SCL设低电平6 A. J: G2 m* s
MPU_SDA_OUT(); // SDA输出模式
! M& n$ L9 X# @5 G% ~' t. N
MPU_IIC_SDA=1; // SDA设高电平 v9 N2 I5 }9 V3 G
MPU_IIC_Delay(); // 延时 2 us* g5 @3 F8 I9 ]8 V+ [
MPU_IIC_SCL=1; // SCL设高电平6 {; Z2 e" G) |/ ]
MPU_IIC_Delay(); // 延时 2 us" h0 D! E) ^4 Y2 e O- G
MPU_IIC_SCL=0; // SCL设低电平，在SCL高电平期间，SDA为高电平，则说明无效应答
2 c2 J9 g- K+ a1 A9 V; l" v
}
; q, a) Q5 d6 p& f$ \
/*********************************************************************; ?6 _" F, m) i$ N
函数名称：MPU_IIC_Send_Byte(u8 txd)
, U! [9 A, P! c
函数功能：IIC发送一个字节
6 ~7 |) g# T n8 E
入口参数：u8 txd$ m+ ~% u& H1 P+ H
返回参数：无6 X1 p% N: r0 S5 s0 r
修改作者：Aaron* O0 [. p7 S% s2 ?6 z" r9 S
**********************************************************************/
4 K; \9 @& D# C( B
void MPU_IIC_Send_Byte(u8 txd)
( Z- M7 ^* y8 n( D. c& u
{ , ]; |2 f6 b2 K! C
u8 t;
' i6 e+ V" i1 Q5 k3 T0 f
MPU_SDA_OUT(); // SDA输出模式
O' g4 E$ D3 o8 x/ r" n
MPU_IIC_SCL=0; // SCL设低电平，此时可改变SDA，以实现数据的有效传输+ {- H: x0 `+ o, a7 y- C/ }$ ^
for(t=0;t<8;t++) // 发送一个字节，即8个bit
1 w7 o. z! d; Y$ v$ D
{ / w% E! F0 L! I8 r4 c
MPU_IIC_SDA=(txd&0x80)>>7; // 获取u8 txd的最高位（0或1），并右移7位，将最高位值给MPU_IIC_SDA. y% B$ b3 D5 X
txd<<=1;
3 Q) b( [$ i2 K8 l$ F- |
MPU_IIC_SCL=1; // SCL设高电平
+ y! R% t4 e' a d& c- d% I3 O4 r
MPU_IIC_Delay(); // 延时 2 us) i* U7 ?( i/ X4 O$ V) L
MPU_IIC_SCL=0; // SCL设低电平，传输SDA完成
$ ^( K& h- ]3 D' ^% Y! d
MPU_IIC_Delay(); // 延时 2us% v) w/ {8 _% r: x8 x2 l
}
|2 b$ W( q* h$ l+ K
}
; @8 P* Y: a5 C; l
/*********************************************************************
@: H4 B8 \6 H" T+ w
函数名称：u8 MPU_IIC_Read_Byte(unsigned char ack)
7 w- @+ a3 e/ B
函数功能：IIC读取一个字节，ack=1时，发送ACK，ack=0，发送nACK $ M$ m9 v3 I( Y+ x* x
入口参数：ack
/ }# W3 h$ f- i/ g( I
返回参数：u8，" L; e% ?7 V& u2 u8 h4 }' D; z% z
修改作者：Aaron3 a: q1 m1 v; S: _! z% |
**********************************************************************/. f! e, Z& A/ m0 S4 J% @
u8 MPU_IIC_Read_Byte(unsigned char ack)$ b4 d" b$ v, C3 i# s0 ~
{1 z- @4 n' V `8 _
unsigned char i,receive=0;% M' i- B3 F5 E8 `1 E* w# B
MPU_SDA_IN(); // SDA设置为输入模式
4 ]5 ]# b% |' {% ~$ J& S* m
for(i=0;i<8;i++ ) // 读取一个字节，即8个bit
8 c# T( @. e$ O
{
7 @1 x4 u6 [* z$ Z$ v9 z
MPU_IIC_SCL=0; // SCL设为低电平; {7 Q; M9 A: I; u3 a3 q. ]) a3 S
MPU_IIC_Delay(); // 延时 2us
* [. L1 U! u3 m( [
MPU_IIC_SCL=1; // SCL设为高电平
. r. x- I- T+ g/ } s. q; H& D
receive<<=1;
! F" w: q2 b* @$ P
if(MPU_READ_SDA) // 如果读取的SDA为1，则receive最低值+1，若读取的SDA为0，则receive最低值不变，即0
$ S8 L6 q+ N; q' R5 {+ I2 t0 ?
receive++;
! h! _4 l) r1 _+ P3 B
MPU_IIC_Delay(); // 延时 2us; C# p* p* k3 e9 o& n
}
1 u2 z. b1 w" y+ d) }
if (!ack). `% I: w) B6 S8 ^. B& K
MPU_IIC_NAck(); // 发送nACK6 t# b3 G0 b% T2 }% r: \
else2 Q( I7 G% J+ L1 m
MPU_IIC_Ack(); // 发送ACK
- w, V7 n) P7 A) B
return receive;2 @ y+ i d4 w. w) {1 Z% R4 f
}# w7 F5 m& y" E$ u
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/

复制代码

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

/**
* g; `- [; H9 Y+ w: |! T2 B
******************************** STM32F10x *********************************
' U% T; k; ?; N' V
* @文件名称： mpu6050.h$ E4 w3 C; f& g3 g$ d
* @修改作者： Aaron2 c0 C4 j0 I0 H! c
* @库版本号： V3.5.0
0 N& S1 D4 K( L5 L
* @工程版本： V1.0.02 C, a/ p( q1 t: i% R. ?3 _0 d Y
* @开发日期： 2020年11月30日
: u' Z/ }: S$ l4 S5 }* M
* @摘要简述： mpu6050头文件2 H" n/ z# E2 J' |8 U6 v4 \2 k/ o+ G
******************************************************************************/9 `+ f2 O/ g7 |9 R7 g* ]. @6 g
/*-----------------------------------------------------------------------------
) o3 B! [) p m
* @更新日志：
4 C* v2 ]* d! z% v) N1 ^. x2 O; R
* @无6 [% o o/ \4 Y& [
* ---------------------------------------------------------------------------*/
- S1 v# q( g! E
#ifndef __MPU6050_H4 U! |, V5 F( L* f& @
#define __MPU6050_H9 [9 E$ x4 R7 h- G
/* 包含的头文件 --------------------------------------------------------------*/
$ X) r$ A4 z! g7 `
#include "mpuiic.h"
. K) I, ^- q7 U7 ^3 h. v
/* 位操作控制PA15,对应MPU6050 AD0的高低电平 ----------------------------------*/
& V9 c! L) P" X: q3 g' T
#define MPU_AD0_CTRL PAout(15) // 控制AD0电平,从而控制MPU-6050地址8 t, @/ F# P1 H7 K; m4 \
/* MPU6050 寄存器ID定义 ------------------------------------------------------*// s) I) U4 q& p/ X; |" \
// #define MPU_ACCEL_OFFS_REG 0X06 // accel_offs寄存器,可读取版本号,寄存器手册未提到
% e6 w1 b- v. I5 {4 E: {
// #define MPU_PROD_ID_REG 0X0C // prod id寄存器,在寄存器手册未提到
6 g: f' K# j6 Y3 M1 Y3 z
#define MPU_SELF_TESTX_REG 0X0D // 自检寄存器X ACC和G" K' E% A' _/ b5 E O4 ?: W6 L( \
#define MPU_SELF_TESTY_REG 0X0E // 自检寄存器Y ACC和G' o& `1 `. J$ C8 ~' P
#define MPU_SELF_TESTZ_REG 0X0F // 自检寄存器Z ACC和G* c5 _3 F, W) Q- b( Z5 k
#define MPU_SELF_TESTA_REG 0X10 // 自检寄存器X,Y,Z only ACC, x3 A* E' w2 U2 E9 |2 Q
#define MPU_SAMPLE_RATE_REG 0X19 // 陀螺仪采样频率分频器 **初学**
+ p q d4 m+ h, Z! T7 D
#define MPU_CFG_REG 0X1A // 配置寄存器 **初学**
7 K- r$ W3 o5 ~( @) t
#define MPU_GYRO_CFG_REG 0X1B // 陀螺仪配置寄存器 **初学**- b+ \0 }4 r( w, g
#define MPU_ACCEL_CFG_REG 0X1C // 加速度计配置寄存器 **初学**2 D, i+ \2 T; C7 t
#define MPU_MOTION_DET_REG 0X1F // 运动检测阀值设置寄存器
2 E! j$ |6 T+ A6 O* L# h' _
#define MPU_FIFO_EN_REG 0X23 // FIFO使能寄存器 **初学**9 |6 p" L6 m0 K2 B
#define MPU_I2CMST_CTRL_REG 0X24 // IIC主机控制寄存器
9 n: f' N& e [% A
#define MPU_I2CSLV0_ADDR_REG 0X25 // IIC从机0器件地址寄存器
# \ H Q1 Q$ i$ O9 s! T0 _
#define MPU_I2CSLV0_REG 0X26 // IIC从机0数据地址寄存器: I0 i( x: k. E9 Y9 N
#define MPU_I2CSLV0_CTRL_REG 0X27 // IIC从机0控制寄存器) O1 I+ k0 f" r+ _& G/ q
#define MPU_I2CSLV1_ADDR_REG 0X28 // IIC从机1器件地址寄存器
9 n8 M$ c" }6 {& b9 |; N
#define MPU_I2CSLV1_REG 0X29 // IIC从机1数据地址寄存器
3 U6 ]) T3 [7 |; A
#define MPU_I2CSLV1_CTRL_REG 0X2A // IIC从机1控制寄存器/ S: ~9 M: V/ F* ^0 v* A
#define MPU_I2CSLV2_ADDR_REG 0X2B // IIC从机2器件地址寄存器' t/ K4 n! r# i
#define MPU_I2CSLV2_REG 0X2C // IIC从机2数据地址寄存器
* u2 {: M" X7 o5 X
#define MPU_I2CSLV2_CTRL_REG 0X2D // IIC从机2控制寄存器
! p t& d# A+ B5 u6 V, v! M- K$ p
#define MPU_I2CSLV3_ADDR_REG 0X2E // IIC从机3器件地址寄存器2 G2 h; X3 @, R3 @" l% ?, o
#define MPU_I2CSLV3_REG 0X2F // IIC从机3数据地址寄存器
/ l5 q, Q* p. m, F$ F+ n9 ?( n
#define MPU_I2CSLV3_CTRL_REG 0X30 // IIC从机3控制寄存器0 v9 o: e1 _6 O/ F8 k
#define MPU_I2CSLV4_ADDR_REG 0X31 // IIC从机4器件地址寄存器
# h( L% _. H! o) V
#define MPU_I2CSLV4_REG 0X32 // IIC从机4数据地址寄存器. g3 R: B! e2 H1 l3 w" q& Y/ w8 u9 i
#define MPU_I2CSLV4_DO_REG 0X33 // IIC从机4写数据寄存器
) J: \1 y F# [/ f- h1 r1 F
#define MPU_I2CSLV4_CTRL_REG 0X34 // IIC从机4控制寄存器" a X1 D- A8 F8 N* e" M- a
#define MPU_I2CSLV4_DI_REG 0X35 // IIC从机4读数据寄存器5 h' o+ F7 {( }" _' F/ w8 b
#define MPU_I2CMST_STA_REG 0X36 // IIC主机状态寄存器" Z9 h. \; S/ J
#define MPU_INTBP_CFG_REG 0X37 // 中断/旁路设置寄存器
" U' l2 R7 g. @5 }& N- r3 d' t& L
#define MPU_INT_EN_REG 0X38 // 中断使能寄存器; o3 @1 |% `8 n6 M v
#define MPU_INT_STA_REG 0X3A // 中断状态寄存器7 V3 N4 j0 o) |4 ~8 P
#define MPU_ACCEL_XOUTH_REG 0X3B // 加速度值,X轴高8位寄存器 **初学**
) S! h7 J9 r o3 o& a' F
#define MPU_ACCEL_XOUTL_REG 0X3C // 加速度值,X轴低8位寄存器 **初学** B. h) Q: E) s# y1 l' n& G
#define MPU_ACCEL_YOUTH_REG 0X3D // 加速度值,Y轴高8位寄存器 **初学**
1 Y7 i: b% ^2 b8 }# k
#define MPU_ACCEL_YOUTL_REG 0X3E // 加速度值,Y轴低8位寄存器 **初学**1 d: W& M& J$ T5 a
#define MPU_ACCEL_ZOUTH_REG 0X3F // 加速度值,Z轴高8位寄存器 **初学**
! T! U) X% _3 t* W5 I
#define MPU_ACCEL_ZOUTL_REG 0X40 // 加速度值,Z轴低8位寄存器 **初学**1 [: K n6 o! f9 ~$ j% ]
#define MPU_TEMP_OUTH_REG 0X41 // 温度值高八位寄存器 **初学**
0 \0 \6 [5 [$ `7 K, @% w8 q
#define MPU_TEMP_OUTL_REG 0X42 // 温度值低八位寄存器 **初学**
4 e! h- [; H$ t! }0 X1 J
#define MPU_GYRO_XOUTH_REG 0X43 // 陀螺仪值,X轴高8位寄存器 **初学**
7 U/ B8 I9 p/ |" Y2 G2 |
#define MPU_GYRO_XOUTL_REG 0X44 // 陀螺仪值,X轴低8位寄存器 **初学**
$ j! \! A C4 V- U1 h2 S
#define MPU_GYRO_YOUTH_REG 0X45 // 陀螺仪值,Y轴高8位寄存器 **初学**
* r* c" ]3 w, d2 t/ B) Y+ |
#define MPU_GYRO_YOUTL_REG 0X46 // 陀螺仪值,Y轴低8位寄存器 **初学**4 N5 Q( M+ E. d p
#define MPU_GYRO_ZOUTH_REG 0X47 // 陀螺仪值,Z轴高8位寄存器 **初学**) k2 b$ B5 W5 j5 S) i% j
#define MPU_GYRO_ZOUTL_REG 0X48 // 陀螺仪值,Z轴低8位寄存器 **初学**7 E2 O, I$ c% C
#define MPU_I2CSLV0_DO_REG 0X63 // IIC从机0数据寄存器 h5 f7 U, X+ ?( {) t+ w& M
#define MPU_I2CSLV1_DO_REG 0X64 // IIC从机1数据寄存器0 y9 D; E, u) @! @( d0 P$ ]" O0 I
#define MPU_I2CSLV2_DO_REG 0X65 // IIC从机2数据寄存器7 R0 d; g8 P8 u
#define MPU_I2CSLV3_DO_REG 0X66 // IIC从机3数据寄存器
P6 [% j9 W0 @( o. n
#define MPU_I2CMST_DELAY_REG 0X67 // IIC主机延时管理寄存器
1 R4 l. ^" r, U8 W
#define MPU_SIGPATH_RST_REG 0X68 // 信号通道复位寄存器
; X" i; p; u! G5 ^! n0 t O
#define MPU_MDETECT_CTRL_REG 0X69 // 运动检测控制寄存器
+ H* ^! p( z3 n3 H& E5 I
#define MPU_USER_CTRL_REG 0X6A // 用户控制寄存器! \9 j4 @0 \. Y( U
#define MPU_PWR_MGMT1_REG 0X6B // 电源管理寄存器1 **初学**5 E; n+ ^6 m5 O) w
#define MPU_PWR_MGMT2_REG 0X6C // 电源管理寄存器2 **初学**+ E( [, @1 P. \% z% f2 h
#define MPU_FIFO_CNTH_REG 0X72 // FIFO计数寄存器高八位
8 _, z q* D/ X) }- Y8 J; M& ^
#define MPU_FIFO_CNTL_REG 0X73 // FIFO计数寄存器低八位
1 G0 ?- D, k% u/ G: w3 O
#define MPU_FIFO_RW_REG 0X74 // FIFO读写寄存器. N" N R+ E! e' a& ^: B
#define MPU_DEVICE_ID_REG 0X75 // 器件ID寄存器5 q, ]( S. U: ?9 H& J" g; `
// 备注：如果AD0脚(9脚)接地,0,IIC地址为0X68(不包含最低位)；
8 _8 v. ?3 L9 o( S" [* r
// 如果接V3.3,则IIC地址为0X69(不包含最低位)，
( V* `% ~/ v. O" N I# L0 s- C
// 本开发板由PA15管脚控制，另PA15为低电平 + y( Z$ X, L2 v0 x* ^
#define MPU_ADDR 0X68
5 D8 `: O9 T- a: A; K, F- ]( D
/* 函数申明 -----------------------------------------------------*/. k5 A% i1 y5 s! s* T0 G
u8 MPU_Init(void); // 初始化MPU6050
2 Y. t$ q$ L7 N4 ~
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf); // IIC连续写& h8 U- i! Y9 ]
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); // IIC连续读
# c/ Z( \1 e/ E# i' a5 |# y
u8 MPU_Write_Byte(u8 reg,u8 data); // IIC写一个字节; f' Q q8 L9 a1 R' m( G
u8 MPU_Read_Byte(u8 reg); // IIC读一个字节
2 p, a3 G* L0 J" A) ~4 }, d
u8 MPU_Set_Gyro_Fsr(u8 fsr);
, M' ^2 b+ l2 k5 O7 l B
u8 MPU_Set_Accel_Fsr(u8 fsr);
7 k$ j7 \ a& Q2 ]7 T2 h# s- o
u8 MPU_Set_LPF(u16 lpf);: B: Y1 S; z; A: ]
u8 MPU_Set_Rate(u16 rate);
6 Q/ P2 Q) x; y! R- P$ r" ~* {
u8 MPU_Set_Fifo(u8 sens);
5 G$ i6 P; y. }& _, E, P. N
short MPU_Get_Temperature(void);4 z( N/ Q& C3 b) Q8 F; N( G2 _
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);8 Q( m4 i' t3 Y
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
9 \) Z! c9 H$ N3 B, E" }
#endif /* __MPU6050_H */) t. d9 k) |" ^8 _* v" m; v
/****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/

复制代码

10.4 mpu6050.c源文件函数

/**, ^4 b7 p3 x% m# q( y
******************************** STM32F10x ********************************* [( h4 i+ R: a- H
* @文件名称： mpu6050.c
# r" d: q" L5 q$ U- l
* @修改作者： Aaron q2 G) y5 v7 _6 N$ x; m8 l
* @库版本号： V3.5.0
/ y9 }4 D) [7 r2 K f
* @工程版本： V1.0.00 V ~8 s \9 z- n6 C
* @开发日期： 2020年11月30日* o5 c' u3 {' C0 [7 R3 _
* @摘要简述： mpu6050源文件
, c5 u" P, y6 b( t5 X3 J, {6 y2 A
******************************************************************************/
, U. K* x! i) u# c8 T
/*-----------------------------------------------------------------------------( N( Z' o3 l4 m! ]/ I
* @更新日志：
: T, p& j- W0 ?4 C; p7 w
* @无
2 X# i1 v2 y0 p. _
* ---------------------------------------------------------------------------*/
" m h% V9 n1 d3 R0 J
/* 包含的头文件 --------------------------------------------------------------*/
9 u$ p" c( ~8 G H) o! e
#include "mpu6050.h"
p1 @. N. |/ H- J! q; c
#include "delay.h"
1 ?% q+ B7 d+ ~: W5 R" V$ J( ?
/********************************************************************** t6 I5 i4 I6 @" l6 }1 W. [
函数名称：u8 MPU_Init()
3 I* s0 T; A6 b1 F$ ~8 T
函数功能：初始化MPU6050
: y6 T! d6 c# e& z6 V7 ]& E7 D
入口参数：无' {9 t& x: B* S
返回参数：0，成功，1失败0 |/ P6 i$ ]7 F- @' B
修改作者：Aaron
' r! _1 x7 w4 Q3 s2 h
**********************************************************************/
( f* C" T) x' ?3 P% J+ k, d
u8 MPU_Init(void)0 j" ~: h' o, m! C2 t' \9 }9 j
{
1 P/ y8 k' n. L E, c
u8 res;& p% ?4 N; M/ K- B9 H) t' ^; c
GPIO_InitTypeDef GPIO_InitStructure;
# j! u8 {1 A: E; V3 U
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE); // 使能AFIO时钟
2 a6 c# K: m6 S" u: q$ w
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); // 使能外设IO PORTA时钟 8 `* c O- y0 s
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; // 管脚151 k( Z9 y% W2 K' W: }
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 推挽输出
5 m" f! B, O2 r2 o/ z
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // IO口速度为50MHz
4 \' F" I3 m4 H1 @' \ Z9 v
GPIO_Init(GPIOA, &GPIO_InitStructure); // 根据设定参数初始化PA15,对应MPU-6050芯片的AD0地址管脚2 }8 Y! K: l" w9 i) D4 e
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); // 禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO!!!. P4 H7 G5 K' }$ R. L- \' n
MPU_AD0_CTRL=0; // 控制MPU6050的AD0脚为低电平,则从机地址为:0X68- a. b. A% o/ Y1 |- s
MPU_IIC_Init(); // 初始化IIC总线2 p6 ]! @7 _1 b+ W
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); // 对电源管理寄存器1输入0x80，复位MPU6050
% I6 S8 u+ o& X% r4 u) L6 u( h
delay_ms(100);
- a) \: ~& [+ I$ F: ]
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); // 对电源管理寄存器1输入0x00，主要令[6]位设0，正常工作模式，即唤醒MPU6050 8 f8 o* H) u+ [
MPU_Set_Gyro_Fsr(3); // 设置陀螺仪传感器满量程范围,±2000dps
4 p1 @% [0 ?# c* t
MPU_Set_Accel_Fsr(0); // 设置加速度传感器满量程范围,±2g
- [ c# G1 [9 N# Z9 M* W
MPU_Set_Rate(50); // 设置采样率50Hz( l/ o N: H$ U
MPU_Write_Byte(MPU_INT_EN_REG,0X00); // 对中断使能寄存器输入0x00,关闭所有中断) |, g3 }/ O6 V- D3 o+ L# R
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); // 对用户控制寄存器输入0x00,I2C主模式关闭$ [) u% P% a2 c- R/ _1 T+ J
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); // 对FIFO使能寄存器输入0x00,关闭FIFO8 S; ]- i. m8 u0 u6 F% s
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); // 中断/旁路设置寄存器输入0x80,INT引脚低电平有效
2 B j; R$ V! G1 L* `. O+ z9 b
res=MPU_Read_Byte(MPU_DEVICE_ID_REG); // 读取器件ID寄存器值，传至res里& z+ a! G7 Z; c7 Z* g% p
if(res==MPU_ADDR) // 如果res=0x68，则器件ID正确6 o7 T! p, }9 R5 w
{
" V( K! [2 q. ?& w
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); // 对电源管理寄存器1输入0x01，设置CLKSEL,001，PLL X轴为参考2 h2 z a8 }9 a% @1 | V8 N
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); // 对电源管理寄存器2输入0x00，加速度与陀螺仪都工作（都不待机）
3 j8 Q) K1 [8 V) X: c
MPU_Set_Rate(50); // 设置采样率为50Hz
; _0 e( g' Y6 i8 i ]; W
}" d3 ?; i4 q& L, b
else
$ q/ ]0 e) S8 ~! w, r6 D: n
return 1;4 d6 r& b, w4 S& C/ e
return 0;7 o) b. r6 ?( H" J0 N. H) _
}6 d* r8 q/ q4 s: X7 W% b
/*********************************************************************
& `+ E: z4 t8 }4 X
函数名称：u8 MPU_Set_Gyro_Fsr(u8 fsr)
( f4 b% t. p3 S0 L% ^- `& {
函数功能：设置MPU6050陀螺仪传感器满量程范围
7 j9 k3 l& U5 W+ }3 q$ v) [# k+ v
入口参数：u8 fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
3 W( [% P& W, t6 b& j: i
返回参数：0，成功，1失败
: ~# d/ q y# I5 z# G6 l: Y
修改作者：Aaron
5 s f0 U7 ]* ]% I z; u" Y% v; B
**********************************************************************/
% b" @$ Y& K- B
u8 MPU_Set_Gyro_Fsr(u8 fsr)
% X. X" r; v9 H: R2 d
{
" N( R- x6 S7 f! @' h' l- X
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3); // 设置陀螺仪满量程范围为±2000dps " A: K& a: L* ^
}
/ J: O# N- [$ p7 t6 Z
/*********************************************************************
- v* O+ w3 s# M: B
函数名称：u8 MPU_Set_Accel_Fsr(u8 fsr)
& J$ [+ t! [0 u/ K# _5 J! z! r
函数功能：设置MPU6050加速度传感器满量程范围
; W( n! @$ G3 ^$ u, H% y1 u
入口参数：fsr:0,±2g;1,±4g;2,±8g;3,±16g
% P. g4 Y& I+ c. s
返回参数：0，成功，1失败
9 e8 ?. m# k2 E0 [( [' E Q( r0 c
修改作者：Aaron& R! }4 b- E( Q* o
**********************************************************************/
* L7 r& d2 c. P, |# }; @7 I
u8 MPU_Set_Accel_Fsr(u8 fsr)
, w5 _' B) q) V; @% B% u
{( r! z/ p4 w- j% {' G1 ^9 z/ q" P9 e( q
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3); // 设置加速度传感器满量程范围为±2g
% f* \6 p1 t& z4 ~6 M( _0 Z
}5 i! P! D; Z9 z9 c7 ]: u- q
/*********************************************************************& k5 [# K) U9 b! `4 _
函数名称：u8 MPU_Set_LPF(u16 lpf)- X) f* O0 O8 d( S7 L% \) C1 x% @
函数功能：设置MPU6050的数字低通滤波器* t( g) x1 Y, M- k/ h) d* c6 A3 R
入口参数：lpf:数字低通滤波频率(Hz)# m% x) z7 o7 U* f X& S
返回参数：0，成功，1失败
' I; j5 \+ l( Q% X$ L
修改作者：Aaron
6 d3 p! e1 e. L, C5 q- r
**********************************************************************/
3 V1 W& T D" S2 A! [
u8 MPU_Set_LPF(u16 lpf)4 x4 G U5 t( g' x- ^% o
{
: e* n8 V- }! V* Y0 a! A8 G
u8 data=0;* X2 {9 t5 Z0 t- t. n" B6 @7 ^
if(lpf>=188) // 如果lpf≥188Hz，则data=1
# s1 \* A: H8 \4 {! u
data=1;7 `1 r0 I9 A! A' S+ B
else if(lpf>=98)8 p; w/ a* D' y( D
data=2;. \1 z& k: @7 E( o& G0 \& C, d
else if(lpf>=42)
9 _7 }$ b4 W& } f, X( ~/ T
data=3;+ k' F9 i" I. i% L
else if(lpf>=20)% C- e7 Z' Z8 J7 ?
data=4;
4 V9 s- z' d9 Z) ~8 U5 j( a, b
else if(lpf>=10)
+ }; J) A8 _/ K, b4 g
data=5;) F1 k9 Y) z2 n1 p/ E# l3 K
else 5 E! `# s. @) b! Q- \$ i! u
data=6;
, P% t1 j2 w6 M
return MPU_Write_Byte(MPU_CFG_REG,data); // 设置数字低通滤波器
. v( G9 ^8 f+ Y8 C/ f1 F
}
; X+ d1 q3 B" g# ~) B' z$ [! i" T- n
- v2 D/ s4 L) U5 }! e
/*********************************************************************7 X1 b& X& f! n
函数名称：u8 MPU_Set_Rate(u16 rate)0 A( c' q. p1 @" ^3 |
函数功能：设置MPU6050的陀螺仪采样率(只在陀螺仪输出频率Fs=1KHz时成立)$ P, Z% f3 `3 F
入口参数：rate:4~1000(Hz)，陀螺仪采样频率3 y2 [1 g: K- L4 [0 K
返回参数：0，成功，1失败7 Z$ J8 t; r" R$ O K2 U8 T R
修改作者：Aaron$ D! J i( Z- T3 j7 F
**********************************************************************/6 I O6 Y. ^4 \9 v
u8 MPU_Set_Rate(u16 rate)* v& ^* e# e Z) h, J) n" k' @+ Z
{
, J F. H3 g2 ^( O
u8 data;( W) _/ ^- @/ H) u) @# d8 S
if(rate>1000)) `- n; l6 g1 W( D( k
rate=1000;; J; ^1 ~* H% m. o
if(rate<4)4 I7 V9 H& H8 }* f+ t! N( K
rate=4;
. c7 z0 t+ g: ]# C4 q. W
data=1000/rate-1;& ^- h- q9 u5 b- I( s0 I& C' _
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); + `! ^ z9 @' }% P
return MPU_Set_LPF(rate/2); // 自动设置LPF为采样率的一半
; P/ b: Z8 P+ c% p: ~0 q
}
+ A& I* O, l w3 T( j- Y8 x
/********************************************************************** H% v2 q( e7 |) y# }' Z
函数名称：short MPU_Get_Temperature()
6 h: }" D& W, G
函数功能：得到温度值
: H7 Q+ i# f2 w3 S1 p* l. A. } `
入口参数：rate:4~1000(Hz)，陀螺仪采样频率
0 T9 L0 L! {: S7 N. \
返回参数：温度值(扩大了100倍)0 I, A6 }0 N2 c1 k% v/ W4 Q5 o* @9 Y
修改作者：Aaron8 M* y5 W4 s( T& ]- n+ V
**********************************************************************/
+ M; p9 e( l- k+ ?) I% }; y' L# k
short MPU_Get_Temperature(void)3 m+ ^. g* A' V
{
H% a& B& n- @' L5 k' L$ v
u8 buf[2];
# k" L( b+ i# M0 o; [) q' H
short raw;
" V/ }- _& l. ?. f' {& b
float temp;
7 W/ k5 u* s& P5 H( z: |, ~/ H
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); // addr:器件地址；reg:寄存器地址；len:写入长度；buf:数据区# G, B" y! {4 N: E- U" r' U! p
raw=((u16)buf[0]<<8)|buf[1]; , a5 _2 `) W* F+ A: r7 ]: A. i
temp=36.53+((double)raw)/340; ; c2 [% [% \. @- G0 ]
return * u9 V$ }* n; T1 k$ k* ~
temp*100;;
# I$ o; r! Y2 l7 `, n
}
U9 T) o$ t2 `
/*********************************************************************
$ Y9 k8 Y% W5 z5 u( c1 B* E/ m
函数名称：u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)6 B7 _5 ]5 K* j. ^, s
函数功能：得到陀螺仪值(原始值)' ~. B. e( w) l7 i. y
入口参数：gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)' T! V' u5 ]6 l
返回参数：u8 0成功，1失败
1 i; `: @: B8 _
修改作者：Aaron
/ G- u% d! b0 ^1 W
**********************************************************************/
! u( T2 V2 S+ @" \+ p5 B2 {
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
1 s( B/ x7 x/ o
{
: B$ W8 d' z* B+ D
u8 buf[6],res;
' G$ J" H6 O8 K' v& L
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);0 |1 V# K4 W( C" S6 B7 D
if(res==0)& Z" N9 @( s+ S7 x
{. S" T' u% ~0 Y# N
*gx=((u16)buf[0]<<8)|buf[1];
7 q) \. g6 }7 R' Z. y, [# K- D
*gy=((u16)buf[2]<<8)|buf[3];
% y: ^8 c7 x. l4 d0 R# t
*gz=((u16)buf[4]<<8)|buf[5];
; g7 x4 M4 `& `. D7 p4 h0 |
}
0 b/ Y+ z' L) Q' R# M2 E
return res;;+ e2 h' k4 r [; k% y. l
}* F3 f, C4 h) b! H' l0 `& f2 V
/*********************************************************************" c" U; I3 m4 p2 v( f& A
函数名称：u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
- t; K0 l; D3 E/ J
函数功能：得到加速度值(原始值)& b. `& V4 k3 J9 Y+ p8 P
入口参数：gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
( u: O8 I1 ~- Q* o" ?1 j6 e
返回参数：u8 0成功，1失败4 k% X* i7 s* @ x* ~5 w
修改作者：Aaron8 y5 x1 a/ Z" r
**********************************************************************/) F% e, G; y0 t
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)/ s/ O, `9 _, f% z! [4 p
{
Y4 M* Y* C: [
u8 buf[6],res; + m a, Z4 S9 d- j! t
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
2 n+ H: n; X6 h4 T5 {) j* s
if(res==0)& S( U M3 L+ C4 L+ `5 J! G
{8 l7 M8 |' R) i, N
*ax=((u16)buf[0]<<8)|buf[1]; 4 b) S6 }$ a( w/ a/ P. u
*ay=((u16)buf[2]<<8)|buf[3]; 7 C4 C: }5 }( k: J/ m
*az=((u16)buf[4]<<8)|buf[5];
9 z7 X: m: X. A& F9 `
} 9 }" @: Q2 K% g R* r4 K
return res;;
5 w; W) M4 z: E/ j- \# S
}
" l, x2 U) X" C/ B1 s$ _
/*********************************************************************+ x# m5 h# o* C4 ~. w
函数名称：u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)0 |! I! Z' i6 t! B' i
函数功能：向MPU6050连续写入数据
/ c7 ~3 V( f: i! S; s6 M/ m
入口参数：addr:器件地址；reg:寄存器地址；len:写入长度；buf:数据区6 }- `" f/ K% h( j; g
返回参数：u8 0成功，1失败
: W3 _) p; q5 C5 k; `
修改作者：Aaron: ]$ J; ]1 y' K( B' R/ e2 ~9 N
**********************************************************************/
9 \6 k, b" x) Y
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
4 l6 v: ^/ y1 B* ^1 D' K9 u' J) F
{
8 g/ C- ?! e6 t8 R0 Z
u8 i; 4 l$ W% N* C) j( W2 w/ ^- p
MPU_IIC_Start(); , `' Y# s2 a p: |( D) ?, a$ ~
MPU_IIC_Send_Byte((addr<<1)|0); // 发送器件地址+写命令 , ~9 T* {+ Y0 _" _
if(MPU_IIC_Wait_Ack()) // 等待应答' b t' }, y' A, U: H1 h
{
9 `9 `/ n+ P) U; n7 Q
MPU_IIC_Stop(); ! j m6 i$ g2 r( N" X
return 1; ' f2 ^. Q, ]! k7 m+ N
}
& w% I. A5 h/ R) S- e, s7 q$ h1 B
MPU_IIC_Send_Byte(reg); // 写寄存器地址
' l$ ?+ T+ B6 Y0 f5 H. J# L
MPU_IIC_Wait_Ack(); // 等待应答. z! a% l }# v$ r$ Q
for(i=0;i<len;i++)
! y7 \9 j3 w5 |7 E, J
{/ H3 N. a8 c( _$ t- x
MPU_IIC_Send_Byte(buf); // 发送数据
3 \% x* ^9 \* R/ O
if(MPU_IIC_Wait_Ack()) // 等待应答; l( v% l4 \5 }7 r. K3 L
{/ F0 e' h( e* f: x
MPU_IIC_Stop(); 8 r$ D! [* N' Y! t" d
return 1; : d6 I- @$ o# R7 k
} + C5 m( m5 k4 u! v$ A1 p
} 4 ~! S# ]% P5 |! k
MPU_IIC_Stop(); # y' a& E4 _- O) m( J) B
return 0; : P" A* p3 d2 k& k" b
} 6 N1 ]. Q4 {9 P$ r* ~- B
/*********************************************************************8 k: l1 j, Y2 T" u* a3 T
函数名称：u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)( Q3 ^7 X/ K' K
函数功能：从MPU6050连续读数据
2 H0 O: W" {' |. O$ r
入口参数：addr:器件地址；reg:要读取的寄存器地址；len:要读取的长度；buf:读取到的数据存储区$ f# v9 l* c* H: H( Q% H/ }
返回参数：u8 0正常，1错误6 S2 E* m4 |" i8 C$ f8 e% t
修改作者：Aaron: }1 ?0 v8 A _
**********************************************************************/
% h- [ ], k( r( w) d% F4 {
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)0 j3 T( n/ Y& P9 H9 f
{ " f' O- a) W/ V1 L9 U5 Q j
MPU_IIC_Start(); : r$ z# |( N. S0 s: l. D
MPU_IIC_Send_Byte((addr<<1)|0); // 发送器件地址+写命令 ! U5 k3 f) c" _6 a4 C
if(MPU_IIC_Wait_Ack()) // 等待应答+ z0 S: N/ C# [) S) `' x
{& L6 J3 T1 T x! B. J) u
MPU_IIC_Stop(); ' b5 A! W4 t4 N
return 1;
) A0 b4 M% U' H9 [1 K6 b6 u+ z! ]
}
0 k5 q( S5 L" o `" z) m
MPU_IIC_Send_Byte(reg); // 写寄存器地址8 O, d$ @; E3 A) Q& K( m
MPU_IIC_Wait_Ack(); // 等待应答
/ ]6 X8 U! z( }- b
MPU_IIC_Start();6 W" K" s; j/ r2 c4 W. \& J: ]
MPU_IIC_Send_Byte((addr<<1)|1); // 发送器件地址+读命令
$ `' Q# A$ b/ L# T9 c6 [7 V
MPU_IIC_Wait_Ack(); // 等待应答
3 P- k7 C8 b% M) y" z u5 r
while(len)
$ t5 P9 R* ~: i1 z! m7 I
{, C4 y( X# W# }" ~+ \9 R3 w2 P) Z
if(len==1)*buf=MPU_IIC_Read_Byte(0); // 读数据,发送nACK
* j% v, ], N9 t& o X7 R1 B
else *buf=MPU_IIC_Read_Byte(1); // 读数据,发送ACK * i* F$ u* c" \( @4 k9 b# c4 n d
len--;
# M5 @+ H7 z- n9 P1 }: ]
buf++; 8 {$ q' _) b- S' z: _# q" ~
}
: d" l& U# b) E6 ~) T, w
MPU_IIC_Stop(); // 产生一个停止条件 * ]: W2 O' H4 q: K& z* w2 r0 B
return 0;
& f/ _9 R3 x8 w+ l# J* m9 P
}
% V) C: v% e5 f, i2 j( L
/*********************************************************************
! y' _) H9 n- r R! W! Z
函数名称：u8 MPU_Write_Byte(u8 reg,u8 data) 6 X& c& I: N2 e) c0 z/ _
函数功能：向MPU6050写一个字节 " d: ~; t( @2 N; \
入口参数：reg:寄存器地址；data:数据
$ k! [/ P" X1 Q% f8 N/ F+ b0 H( M
返回参数：0，正常；1，失败
$ R( |: @3 Q4 Z, m& c- ^
修改作者：Aaron
7 e7 b9 T) F( o5 i1 [9 s0 f
**********************************************************************/
$ e: l/ O* g! Y& e7 C0 e: N6 f
u8 MPU_Write_Byte(u8 reg,u8 data) : t; ~. R5 a9 A9 V1 P+ s
{
+ Z: M# Q* K& D, K# R" r: C
MPU_IIC_Start();
7 h5 Y+ e; m! J
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0); // 发送器件地址+写命令
( G; g, q: `; M/ S/ d& _* K i
if(MPU_IIC_Wait_Ack()) // 等待应答，0接收应答成功，1接收应答失败6 d" @5 B/ j% p1 M& u* F
{" T/ x9 v0 R& C' u4 M/ \
MPU_IIC_Stop(); // MPU_IIC停止信号( t5 s8 k ?; j& K7 \5 e
return 1; & g! @, b# G( U
}1 }2 F+ x v! k# F: V8 |
MPU_IIC_Send_Byte(reg); // 写寄存器地址% k0 G4 H( O# u" B {8 N
MPU_IIC_Wait_Ack(); // 等待应答
$ b2 c9 {5 Q O* g9 a0 @" s6 n, p
MPU_IIC_Send_Byte(data); // 发送数据
4 A: y8 y; c/ a( w
if(MPU_IIC_Wait_Ack()) // 等待应答6 Z) k- Q# B+ V6 T8 B
{6 u( h) B% t7 |' k& u" F
MPU_IIC_Stop(); 5 k& d [. D& M3 I& k5 ], z9 S
return 1;
$ \1 b) W+ g1 b* d- v9 g! M
} V8 \+ i; M% t) o5 ^0 V
MPU_IIC_Stop();
2 Q, ~5 r/ s/ {, v/ Y3 H
return 0;1 T. t2 \2 V; r
}
; T" x, t1 L [! q0 O
/*********************************************************************4 g' u7 {* ~6 d: o! J6 [6 O7 U, ]
函数名称：u8 MPU_Read_Byte(u8 reg)4 X7 i9 t& C, V
函数功能：从MPU6050读一个字节 % i& e2 _* Y9 ?/ M: X6 l
入口参数：reg:寄存器地址% W) l) c* a; g/ y' j; [
返回参数：u8 读到的数据
1 V O+ ^0 p: ~# X4 ~
修改作者：Aaron
3 U. w4 X) G) x* m2 {5 ~
**********************************************************************/1 }" G7 N+ F$ Q/ O7 T
u8 MPU_Read_Byte(u8 reg)0 ~2 h1 t+ E4 l* `/ f% @
{
! ?; G) k% m0 M! b% R
u8 res;
7 l0 B$ b: o8 E1 H& h1 b, k! l
MPU_IIC_Start(); 7 I! `8 `9 }2 l! D& _
// 开始标志（S）发出后，主设备会传送一个 7 位的 Slave 地址，并且后面跟着一个第 8 位，称为 Read/Write 位。 0写，1读。
0 K1 V; y! Y: O* c. \6 W0 I
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0); // 发送器件地址+写命令4 p6 L0 O- X* Z5 ]- d
MPU_IIC_Wait_Ack(); // 等待应答
; o- E9 O, s% l" O5 M" ~) D; c7 e
MPU_IIC_Send_Byte(reg); // 写入要读取字节的寄存器首地址
" ? d3 Z" h |, Y7 `4 m' {
MPU_IIC_Wait_Ack(); // 等待应答
6 s$ q0 j3 [6 u5 M$ v
MPU_IIC_Start();
3 a V* `6 b" M/ l& n% z" W; f
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1); // 发送器件地址+读命令 , [. C! q7 A4 A8 V
MPU_IIC_Wait_Ack(); // 等待应答 $ \& U$ j9 {1 O! }2 }! N
res=MPU_IIC_Read_Byte(0); // 读取一个字节的数据,发送nACK 6 e0 u! I, e0 ? v: `2 K. b2 {4 l5 I
MPU_IIC_Stop(); // 产生一个停止条件
0 u* A0 B0 t3 Q/ t6 ^& {8 F
return res;
) E$ x; Y/ f' P# d
}2 H- s l; e. `1 ]+ s

复制代码

10.5 main.c主函数

/**
1 `" u# q( L; g! s C& A: |
******************************** STM32F10x *********************************1 P/ W+ R" Q- P# {3 Q$ L
* @文件名称： main.c1 X `0 R* |0 U* G8 @7 g' O
* @修改作者： Aaron
- P) e: w& N$ p1 g1 r6 N
* @库版本号： V3.5.0; |; r$ w% l4 k9 _& y3 p; o7 n
* @工程版本： V1.0.01 S, \; q1 o3 O2 x, v8 j3 q G
* @开发日期： 2020年11月30日
9 o% o! `- H+ `8 l
* @摘要简述：主函数，增加了在LCD显示6轴的原始数据. w& L% H6 d4 s5 s; j+ w Z+ O
******************************************************************************/" M* T; D" \& A# V. B
/*-----------------------------------------------------------------------------
) ?) d/ i* F# V* j* n& Y
* @更新日志：) ?2 ~3 d! @; o8 }! M% M: p" k
* @无
3 O* Q9 v* T! R D, ^% s/ m
* ---------------------------------------------------------------------------*/# y3 }! i! U. v: [3 K0 m
/* 包含的头文件 --------------------------------------------------------------*/
0 K' r0 S9 ]# J' r3 |
#include "led.h"
6 F9 a4 h q8 z0 u3 J
#include "delay.h". h3 W# t7 n# `8 a6 M3 c) {& j
#include "key.h"
: I6 w8 R. I: L+ _4 @
#include "sys.h"# k5 J# X% ?1 |; m( t! G" [
#include "lcd.h"
$ Z; `9 Y. b0 h6 a
#include "serial_communication.h"
, C& x" D, V3 |1 O# m' v& [
#include "mpu6050.h"
3 [0 u3 e- N; F
#include "inv_mpu.h"$ @) M. p4 J( V: f4 ]2 k: F/ B
#include "inv_mpu_dmp_motion_driver.h"
, z( d. t6 x3 F2 S0 q& ]
/********************************************************************* r! z9 O% |; ^* _* L
函数名称：usart1_send_char(u8 c)8 ^5 F0 W, f4 L, e8 O( r" }3 x
函数功能：串口1发送1个字符
# `' G# F) T# l* s& M
入口参数：c:要发送的字符* A8 Z7 S8 }3 a% W/ j' i) f
返回参数：0，成功，1失败5 a4 M" o- R% K7 e" l
修改作者：Aaron
6 }6 C5 P2 e: U
**********************************************************************/9 U) Z) U$ n" k+ x3 A7 g# E
void usart1_send_char(u8 c)
1 _$ v$ |. L: D! U2 l& |
{ 2 _: `2 O# L2 J) i( g R0 [0 p
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); // 循环发送,直到发送完毕
4 ?( N( m+ ]- q4 U6 o* N8 M
USART_SendData(USART1,c);
! }- w5 {5 H2 g, d! ]
} & q1 O% N. d2 j$ L7 ], |) y
/*********************************************************************
6 I8 [( F- p0 L( z! V# x
函数名称：usart1_niming_report(u8 fun,u8*data,u8 len)$ | ]. x$ A' W& ^
函数功能：传送数据给匿名四轴上位机软件
' d: x6 A* w2 H& C4 t7 }
入口参数：fun:功能字.0XA0~0XAF；data:数据缓存区,最多28字节；len:data区有效数据个数
/ D A2 i; Z- B& a0 D( |! U
返回参数：无
/ }0 F( _8 r% G! ?
修改作者：Aaron
3 g. z. z/ O5 }' w8 t5 O
**********************************************************************/
: | Q' ?: B* O. ~6 L
void usart1_niming_report(u8 fun,u8*data,u8 len)4 `+ }; z R. w0 G8 m
{
) ^+ d' q6 T' {. A" f$ i
u8 send_buf[32];
5 x6 Y* e. w, R- m; ~
u8 i;
5 S5 y% h( h H' ^/ U
if(len>28)return; // 最多28字节数据 . E& c2 Q/ ]3 O0 n F
send_buf[len+3]=0; // 校验数置零( a4 P) ] v( t$ u- P/ t6 N, W
send_buf[0]=0X88; // 帧头. L% P7 U# _0 [
send_buf[1]=fun; // 功能字- t& S3 c3 G$ O) w- G+ u1 R3 Y; s
send_buf[2]=len; // 数据长度& l0 [; P4 X5 c" Q i$ H1 y- L
for(i=0;i<len;i++)send_buf[3+i]=data; // 复制数据' W( E3 T! W; G7 s6 m
for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf; // 计算校验和
" h' I# ^( u8 T/ z- y
for(i=0;i<len+4;i++)usart1_send_char(send_buf); // 发送数据到串口1
, ]: X, H! v p, h
}- L8 D) A0 C) B& T$ M
/*********************************************************************
) c) I0 u. _" |2 F8 ^
函数名称：mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
/ H. N" s+ S+ u1 A8 F; ^
函数功能：发送加速度传感器数据和陀螺仪数据
* [5 e- i# W* z; X* l* I
入口参数：aacx,aacy,aacz:x,y,z三个方向上面的加速度值；gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值0 |& S2 {) S* _+ W+ t' x
返回参数：无
+ K! H V1 b4 S$ t- y& P" Y
修改作者：Aaron
' X, d1 ]5 c: M- X: y( [5 X6 @
**********************************************************************/4 _9 j3 p7 D( N& p$ m
void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz) P# g. ~% f, v6 x
{6 N, D4 o( e* n1 D8 G
u8 tbuf[12]; 2 S3 f5 P3 t( ^9 T5 p# p( L
tbuf[0]=(aacx>>8)&0XFF;- b" c% b ]( a$ l" d$ k
tbuf[1]=aacx&0XFF;% M5 W8 k; J6 k6 M
tbuf[2]=(aacy>>8)&0XFF;
4 f. }8 c7 w2 ~* v( X7 b* l
tbuf[3]=aacy&0XFF;
/ D6 a2 l. _, ]' _
tbuf[4]=(aacz>>8)&0XFF;) Y9 N6 ]8 U6 i7 m @; D
tbuf[5]=aacz&0XFF;
2 v: J# h c; D
tbuf[6]=(gyrox>>8)&0XFF;0 O8 O1 _7 H |0 J, {& D
tbuf[7]=gyrox&0XFF;. n* u8 o1 \2 Y/ |& S
tbuf[8]=(gyroy>>8)&0XFF;
5 I" W3 t3 u, b2 k% b7 ~8 y2 h
tbuf[9]=gyroy&0XFF;, c2 J/ _0 c0 V
tbuf[10]=(gyroz>>8)&0XFF;
: P, I0 k) L& |
tbuf[11]=gyroz&0XFF;' s! E) l* z6 k8 M! E0 t3 {. F6 }) i
usart1_niming_report(0XA1,tbuf,12); // 自定义帧,0XA1% A# W$ b1 I. \0 a$ d/ h
} % m& s' n9 ^7 p2 v; l
/*********************************************************************
/ X0 ~6 M/ B# y" X
函数名称：mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
- S5 |. G& H# w6 U
函数功能：通过串口1上报结算后的姿态数据给电脑7 M$ f/ `& A! P1 r, |9 c6 s/ P) }0 \
入口参数：aacx,aacy,aacz:x,y,z三个方向上面的加速度值；gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值；
U2 O% m3 P" }* j U
roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度；8 v) p3 V! ^% m. K& w1 I1 q
pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度8 l* H/ `) g2 d- l; a
yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度4 _& }( D4 h& z- I( H/ D# ^
返回参数：无
7 G) t" |2 |/ F
修改作者：Aaron' w+ L& P5 |# F5 h! K7 i
**********************************************************************/: p9 `" R! K1 S9 d: X
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
( b) `: D6 u& {5 L8 m, \- I; ?
{
9 E7 i* @: p" @3 ~0 P9 |* W
u8 tbuf[28]; : c9 `/ ?6 \# W6 @9 R+ F
u8 i;9 U1 l7 I5 M N- m E
for(i=0;i<28;i++)tbuf=0; // 清06 G9 \/ H' R( s# U& c9 d0 _9 F9 D0 b5 K
tbuf[0]=(aacx>>8)&0XFF;
7 W7 N- c% e4 A* u6 @
tbuf[1]=aacx&0XFF;( `" ]5 j% |3 @9 y
tbuf[2]=(aacy>>8)&0XFF;
- @" H4 R+ ]( K* ?* q1 f( c
tbuf[3]=aacy&0XFF;) H$ O5 ^$ V' I$ c
tbuf[4]=(aacz>>8)&0XFF;8 R7 l1 H6 M5 }8 ?% J7 g
tbuf[5]=aacz&0XFF; & C" r! P& |) q- `0 I7 @+ q; z
tbuf[6]=(gyrox>>8)&0XFF;( r5 c5 o( [6 A- c: \' B7 I
tbuf[7]=gyrox&0XFF;
8 Y% N. O0 D; B" M0 _% K
tbuf[8]=(gyroy>>8)&0XFF;
7 g5 o; U% i4 g4 d: }
tbuf[9]=gyroy&0XFF;
% Z p$ l) J. c* F3 K
tbuf[10]=(gyroz>>8)&0XFF;7 L' |" g5 ]$ H; q) V! W/ L! Y
tbuf[11]=gyroz&0XFF; # k6 ?5 s/ B1 y' h1 o3 H
tbuf[18]=(roll>>8)&0XFF;; M9 Y3 c9 m# y# t
tbuf[19]=roll&0XFF;7 h2 @1 a6 k* V
tbuf[20]=(pitch>>8)&0XFF;3 c& G, V& z0 V6 O9 z0 l7 n [4 D
tbuf[21]=pitch&0XFF;
! p1 {& {- x7 n$ j
tbuf[22]=(yaw>>8)&0XFF;
+ P. i; R5 r# H5 v, n
tbuf[23]=yaw&0XFF;
7 d0 B% Z' p- m) r
usart1_niming_report(0XAF,tbuf,28); // 飞控显示帧,0XAF
, b3 G; @; r% p3 O& l% `0 j
}
3 p* S7 j8 F3 [# o3 m
/*********************************************************************0 D: q k# I7 q" _7 t; M
函数名称：int main(). U i# u- [% v+ K/ r& H2 n" }
函数功能：主函数,增加了在LCD屏上显示6轴原始数据的功能: N$ N( \; Y$ X1 M
入口参数：无
P: t) h% n6 I7 z/ G
返回参数：无. ?$ y8 v- u% F
修改作者：Aaron
\0 R0 @5 ^. s( a3 c, L/ ~. d g
**********************************************************************/- v/ u+ M2 m! t: l0 W$ [
int main(void)+ X$ I, Y1 k! n0 @+ q/ ~/ Z! A
{ 5 @+ z) G4 ?( f( V: N
u8 t=0,report=1; // 默认开启上报，发给匿名四轴上位机软件
6 Y% s; D4 K/ J
u8 key;
, K% d9 i3 s2 Y
float pitch,roll,yaw; // 欧拉角* ~8 [. N& M+ d' d: M- b
short aacx,aacy,aacz; // 加速度传感器原始数据5 K2 K' T. _9 Y/ H; p$ {
short gyrox,gyroy,gyroz; // 陀螺仪原始数据
6 ?; N9 q5 A6 Q2 r
short temp; // 温度 7 M5 I T* T5 R4 U
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 设置NVIC中断分组2:2位抢占优先级，2位响应优先级
! |' F o0 ]$ G1 o. H* `3 i* ~
My_USART1_Init(); // 串口初始化为500000
' ^' |" ^- o* E4 r! [
delay_init(); // 延时初始化
7 i8 G4 S7 z8 N8 j
LED_Init(); // 初始化与LED连接的硬件接口! t0 A5 t- O: O4 L2 {
KEY_Init(); // 初始化按键
0 ?: l. d3 n; T+ \- V4 h
LCD_Init(); // 初始化LCD
$ C+ o% [" E2 Z8 _: V1 f
MPU_Init(); // 初始化MPU60506 e% e( {* y# A8 k' Q: b
POINT_COLOR=RED; // 设置字体为红色 . p- X* M* a7 w
LCD_ShowString(30,70,200,16,16,"MPU6050 TEST"); # }" q$ O, m! v
LCD_ShowString(30,90,200,16,16,"2020/11/30"); 8 b9 Z9 y- j) s) o
while(mpu_dmp_init()): t" X, e& J5 G4 I
{% q0 N/ V& `3 W' H
LCD_ShowString(30,110,200,16,16,"MPU6050 Error");
! U2 z2 t% v+ f* `) a( H: w
delay_ms(200);( S+ {4 @; N+ t Z K( a
LCD_Fill(30,110,239,130+16,WHITE);: g/ \) E8 j! [/ U
delay_ms(200);7 u3 U! [" `7 R2 o% M9 {
} - y4 Y3 c$ U. o9 j
LCD_ShowString(30,110,200,16,16,"MPU6050 OK");
" W, q' x, v( }. B2 t, O
LCD_ShowString(30,130,200,16,16,"KEY0:UPLOAD ON/OFF");3 |. n6 w$ |5 \8 x
POINT_COLOR=BLUE; // 设置字体为蓝色 ' ?; A! v# c# e" K3 }- @8 y
LCD_ShowString(30,170,200,16,16,"UPLOAD ON "); 3 R/ i& Q* ~9 K4 ~8 x
LCD_ShowString(30,200,200,16,16," Temp: . C");
; ~$ ^) g7 `7 q. ?; O
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
@4 u/ R2 I* r7 @
LCD_ShowString(30,240,200,16,16," Roll: . C");
! l: J& U" u: }$ Y, N G- j' D
LCD_ShowString(30,260,200,16,16," Yaw : . C");
! b- b% e* {. _! E
while(1)2 I0 ]' }9 R# ^: e
{
: ?% G# P7 ?: J( |8 y
key=KEY_Scan(0);) o8 W9 C( V# y, S$ U1 ^
if(key==KEY0_PRES)
/ ]; r+ H3 k% r: t9 t+ `
{8 g. t/ s( \1 {
report=!report;) w* w2 `2 A8 M$ @
if(report)
; E3 |- ]7 f9 l
LCD_ShowString(30,170,200,16,16,"UPLOAD ON "); s4 p2 y5 B' \" n- i8 z
else + u" @ N( `# D! B2 B3 u: P
LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");9 n$ p( r0 r) E1 v' J
}
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
2 Q; B0 ?, U% o: Z: F8 g
{ " h% G3 {* s N7 C8 u
temp=MPU_Get_Temperature(); // 得到温度值 ! U, k/ z2 r0 `( H- c" j& s" h
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 得到加速度传感器数据
0 ?4 e' `& p: u3 H7 B* f) m, f1 ~: l
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); // 得到陀螺仪数据. [9 G( g8 E: x
if(report)
1 {" ~+ M( I/ x3 I3 q
mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz); // 用自定义帧发送加速度和陀螺仪原始数据
4 Y+ ~+ q2 _# O( T* t2 e
if(report)
& d9 R% b8 x0 C% |6 d2 w' L5 j
usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));" T0 s/ W3 }( V2 ]* R* K
LCD_ShowString(30,300,200,16,16," aacx: ");
; s6 i2 n' r0 i
if(aacx<0)
. ?* G4 A( Z% w* m5 S' b
{
6 R+ N1 \8 H& p1 u* c( v
LCD_ShowChar(30+48,300,'-',16,0); // 显示负号1 X$ }! k0 Q$ i7 l" v8 p
aacx=-aacx; // 转为正数2 Y# C7 U W, s! E$ L0 P
}
. Y+ w$ c1 W, G I" ?+ x w7 U& H
else 0 v8 R% a2 S' u4 [, t
LCD_ShowChar(30+48,300,' ',16,0); // 去掉负号 ' X* k8 T6 m6 K# q
LCD_ShowNum(30+48+8,300,aacx,10,16); // 显示aacx 0 c8 Z3 \% x0 a" }0 }3 A
LCD_ShowString(30,320,200,16,16," aacy: ");
# t* g9 r1 K: L5 d5 L) z+ l
if(aacy<0)) S% O. ?* s, B4 t H& l
{
2 F% g/ B) Y. L K5 q) T
LCD_ShowChar(30+48,320,'-',16,0); // 显示负号 O" q3 s: v. i; u- p+ T
aacy=-aacy; // 转为正数4 M- g; ^6 ]* d, y: l* _
}7 s7 D7 ~, S' h) h" a
else 1 d" s2 u% A: i1 x1 @
LCD_ShowChar(30+48,320,' ',16,0); // 去掉负号
* j" } z! J' m6 u9 y6 {; }4 Y
LCD_ShowNum(30+48+8,320,aacy,10,16); // 显示aacy ) R9 `, d4 L* s: k
LCD_ShowString(30,340,200,16,16," aacz: ");
: T" Z) Q* L0 n. `, Y& Q
if(aacz<0), Z) |. ~/ [ |$ R. a/ e) h4 i
{8 p0 R- H- A: o, U+ v0 f
LCD_ShowChar(30+48,340,'-',16,0); // 显示负号( {% J1 \$ d0 Y
aacz=-aacz; // 转为正数
$ n1 ~, |6 e3 R: l6 e# Z
}
; R( h# d3 b9 k2 i. @
else " j! h# k( f' L
LCD_ShowChar(30+48,340,' ',16,0); // 去掉负号
+ V' H- P5 G z$ w
LCD_ShowNum(30+48+8,340,aacz,10,16); // 显示aacz
?) s7 u( j0 d4 @: p# \
LCD_ShowString(30,360,200,16,16,"gyrox: "); ; i: q8 H; ~* |& M" [+ \
if(gyrox<0)- b# g5 O/ C* o: P7 e
{ \$ S: @2 i9 n0 A; q, p, i& A( [
LCD_ShowChar(30+48,360,'-',16,0); // 显示负号3 I- _- L, n7 U& ]6 i
gyrox=-gyrox; // 转为正数
2 y/ X _2 q0 s" y3 K
}9 m1 N( _+ M7 l- \
else ( {5 N, H5 \! G1 T1 S9 ]
LCD_ShowChar(30+48,360,' ',16,0); // 去掉负号
! q# _3 N# U7 V0 m8 S, K% J! s
LCD_ShowNum(30+48+8,360,gyrox,10,16); // 显示gyrox
^& f* n) {+ @5 X0 r/ m6 Q
LCD_ShowString(30,380,200,16,16,"gyroy: "); 0 q( X% h' Y) I
if(gyroy<0): _7 O/ \. I6 n5 X
{
% T1 Z6 t H9 E
LCD_ShowChar(30+48,380,'-',16,0); // 显示负号0 ~( y. y H6 d: l1 R# A- c7 v0 f% o* W( f
gyroy=-gyroy; // 转为正数
' x5 K- \5 W3 _. J. W
}6 }- r( k- F! H/ j
else
5 T3 B9 O7 q& p! y/ \
LCD_ShowChar(30+48,380,' ',16,0); // 去掉负号 & R8 k9 P2 q, _# C, c2 i' S
LCD_ShowNum(30+48+8,380,gyroy,10,16); // 显示gyroy ) _9 r1 q2 e+ b( j" U0 C
LCD_ShowString(30,400,200,16,16,"gyroz: ");
$ g/ R' t! _- H: ]! Y! ^! Q
if(gyroz<0)
3 ]0 @" t& F2 ?$ f
{
7 N2 L* y) J0 V& R7 C; ?# X% R8 q2 w
LCD_ShowChar(30+48,400,'-',16,0); // 显示负号: x; A4 v* ^# Y3 J1 p
gyroz=-gyroz; // 转为正数
5 [: l" x8 x4 c ~7 M
}
8 z2 l9 w( j8 J2 X; A& @
else ' j$ C7 X+ L% Z& s* T! R( I
LCD_ShowChar(30+48,400,' ',16,0); // 去掉负号 $ V; g |5 L" `& t( r. r! _
LCD_ShowNum(30+48+8,400,gyroz,10,16); // 显示gyroz
0 p- s1 r; ~5 _, b7 t9 U! l5 m
if((t%10)==0)
# [, _. W. r0 e. k. x+ u/ ~
{
1 o! F! Q. O% f/ w* C4 ~
if(temp<0)
, x" d [: `3 j- i: i5 M `3 X
{5 U) H2 o E; r9 ^, c
LCD_ShowChar(30+48,200,'-',16,0); // 显示负号' U: V4 c- y7 _- F% z, L
temp=-temp; // 转为正数
- k& c4 p) B6 s) _7 X$ z
}
6 |! Q/ T5 f# c {- z, ~2 ]4 m5 U
else
X; c2 K, L* C) R( m
LCD_ShowChar(30+48,200,' ',16,0); // 去掉负号
8 Y7 t8 J3 N9 e/ G" Q" s; `
LCD_ShowNum(30+48+8,200,temp/100,3,16); // 显示整数部分 + g' M% y$ u2 B. k( ~4 `1 }
LCD_ShowNum(30+48+40,200,temp%10,1,16); // 显示小数部分
, p( z# T) k$ Y/ A( A' i0 \' L# q
temp=pitch*10;
: r" p8 D! w% `+ i& h8 ]
if(temp<0)$ M+ ?* j) l8 ]- U, H
{# Q O% e7 _" R; {7 x2 n; R
LCD_ShowChar(30+48,220,'-',16,0); // 显示负号
1 g( l% [% z b" I2 {
temp=-temp; // 转为正数$ N* z7 ^4 Z# N; u
}
( Z8 J+ f/ ^/ l3 B# o0 v
else
0 i/ D; R. N. s/ H! V. [! F: t/ K
LCD_ShowChar(30+48,220,' ',16,0); // 去掉负号 0 N9 h4 g4 `/ Y) p# H7 U
LCD_ShowNum(30+48+8,220,temp/10,3,16); // 显示整数部分 / _( Y# T0 v$ n
LCD_ShowNum(30+48+40,220,temp%10,1,16); // 显示小数部分
# [$ C: \* A' C
temp=roll*10;7 ~9 M4 j6 S2 _; a6 o
if(temp<0): G" R' C1 K' I* a1 U
{- O9 J9 F; j6 A0 d% t4 z: Y
LCD_ShowChar(30+48,240,'-',16,0); // 显示负号
2 d& ?* N. X- V# k- U# f' |
temp=-temp; // 转为正数( k6 a. ? `% Z
}
. Z5 [ E8 [' m1 E/ }# Z( n
else
9 f& Q1 }0 S6 J* x t( b
LCD_ShowChar(30+48,240,' ',16,0); // 去掉负号 ! ~! C* h- `" ~# F# y$ R# ?; Y( H
LCD_ShowNum(30+48+8,240,temp/10,3,16); // 显示整数部分
% R5 p- W% L/ d& f, ?
LCD_ShowNum(30+48+40,240,temp%10,1,16); // 显示小数部分
# F- t$ f, ]. T: t( C& k
temp=yaw*10;$ k3 G/ g! L( j9 g' q! r: T
printf("temp=%d\r\n",temp);: |* h& X! o7 l" }$ \
if(temp<0)
6 r% O% b6 H) q/ v ~" u( H& d
{
, p7 q/ N+ a W' l
LCD_ShowChar(30+48,260,'-',16,0); // 显示负号$ _" p% ~2 t+ H7 Q6 x! n- V
temp=-temp; // 转为正数
* K' W7 k. R& ~; {
}
) i+ b9 w9 R) H: \+ h
else & r, b7 B. m) U; s6 G- g/ x3 I2 y- A
LCD_ShowChar(30+48,260,' ',16,0); // 去掉负号 8 Q5 j& ?) |: f$ \3 }
LCD_ShowNum(30+48+8,260,temp/10,3,16); // 显示整数部分
- z0 W# v+ j! {
LCD_ShowNum(30+48+40,260,temp%10,1,16); // 显示小数部分 ( k0 C& P: c# G7 e6 o
t=0;/ [( K4 x" l) _8 E1 T) ]
DS0=!DS0; // LED闪烁2 ^8 |4 k1 F" @! B n' c
}9 j& _! [# h$ f0 I; l
}
# z* V# c( S/ J1 D- n9 A5 A
t++;
V1 @* u5 Y8 p
}
0 L1 w' p. C% h( w
}
% w/ w& o6 N- p2 G2 F0 Q6 W

复制代码

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

#define i2c_write MPU_Write_Len // 对应mpu6050.c源文件里的MPU_Write_Len8 V! f) \$ C' d7 R7 O; g7 X
#define i2c_read MPU_Read_Len // 对应mpu6050.c源文件里的MPU_Read_Len, ?) y" \; S, R
#define delay_ms delay_ms // 对应delay.c源文件里的delay_ms
7 x/ P, V/ g) V3 ?- X+ u( r$ i
#define get_ms mget_ms // 这是一个空函数

复制代码

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

u8 mpu_dmp_init(void)
: E! g/ Z4 [+ u/ g Y
{: W) q; X+ b {5 N$ W
u8 res=0; r' X5 f3 d% d) i
MPU_IIC_Init(); // 初始化IIC总线
; k0 ^; y, j/ {* Q
if(mpu_init()==0) // 初始化MPU6050/ L: Y* N t5 C
{ ) t2 F- i1 v: ?* ]( `. y# Z
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL); // 设置所需要的传感器& i1 I! ~, S5 F: L. u% f7 Y7 M+ Y
if(res)return 1;
. x# N0 _! D! f1 A+ H% w' I
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL); // 设置FIFO# K& a$ ~/ V3 y* K
if(res)return 2; ; x/ C# c4 |3 u2 U# i
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); // 设置采样率
9 {5 k& S4 ^% H! m* p/ d) n% j
if(res)return 3;
# ~3 W3 o) q8 w5 V3 t! }
res=dmp_load_motion_driver_firmware(); // 加载dmp固件
* }! N/ h* H5 }7 _
if(res)return 4; . |, `% q, \! [7 p p
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪方向
3 e2 ^( C0 K4 C% U# N0 B5 F* m& w
if(res)return 5; 3 w+ B, V2 A. C6 R. c6 M. v' |
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| // 设置dmp功能6 u: v6 f' Z5 {8 H: a% \2 H
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|' ~ G9 L/ i) ?6 c/ Q3 D
DMP_FEATURE_GYRO_CAL);
3 b0 G8 P5 e( @4 K5 T
if(res)return 6;
G; J0 }+ s I1 _+ @
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); // 设置DMP输出速率(最大不超过200Hz)
{/ }7 E. {3 u
if(res)return 7;
" x" M' \0 i4 S+ T
res=run_self_test(); // 自检- I C2 U1 X1 d4 p) w
if(res)return 8;
8 z! K3 A- Z& I# D, ~$ s& |) e
res=mpu_set_dmp_state(1); // 使能DMP
* Y$ h" a. \. `3 {! _. ?
if(res)return 9;
5 n7 ]) }) M2 {" V
}else return 10;7 `9 K* n& B8 n Z
return 0;+ }$ W0 t3 U! V3 E
}

复制代码

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)
) H& [% `% c: h5 g& Z
{6 X. B$ S6 G$ m) L7 q1 {
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; } W+ m( K8 V2 G! P: U" D
unsigned long sensor_timestamp;
6 ^4 A8 P2 W' @: J- o0 R4 g8 f
short gyro[3], accel[3], sensors;
; g& t( ?% j1 s y6 ?$ @! o; Y# m
unsigned char more;
v- q; R1 ~0 |* G! {
long quat[4];
( ]$ _8 h3 T/ o7 L
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))" s$ H4 W6 @9 _, h% L+ j
return 1; ) g" P4 _4 t# D. R# E' {, k
ors&INV_WXYZ_QUAT)
% k3 e8 G2 s4 A' p$ o. T; p
{
! e" w) d( _* T; ^6 F
q0 = quat[0] / q30; // q30格式转换为浮点数# Y& m7 T! Q2 d
q1 = quat[1] / q30;
* p% D+ A7 u, J" s9 c
q2 = quat[2] / q30;
* s" K$ {, x2 F: V2 [
q3 = quat[3] / q30;
: G }% V" o) {) m% g( j; q8 y
// 计算得到俯仰角/横滚角/航向角& o5 X- {4 a# d: ^3 f) j* n* v8 ?
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch' B1 `1 q- x1 Z; T3 U! D
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll8 u0 D' `7 f* V7 Q" @ E; M1 l
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw; F, t: S+ S% f% L. m: S1 m6 H
}
/ i( W4 J4 K. p3 w
else 1 [5 d0 @4 S" s/ h! Q- A+ {' A- [
return 2;
5 T" G0 e7 f8 e: T
return 0;; v1 R, M0 x0 J {$ ?, d
}

复制代码

$ k4 S7 r7 B+ n# V1 ^$ Z- r- Y十一、实验结果
该程序可用匿名四轴上位机软件实时查看6轴的原始数据和姿态数据，即欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。（这里就不放截图了）。
另外在LCD屏上，也可实时查看6轴的原始数据和姿态数据，如下图：