一.MPU6050介绍
1.MPU6050与陀螺仪、加速度计的关系：
6 g" w/ m) Q% OMPU6050是InvenSense公司推出的一款全球首款的整合性9轴运动处理传感器，其最大的特色就是：消除了陀螺仪和加速度计的误差，将陀螺仪和加速度计组合在一起，而且缩小了空间。
8 ^$ _- b! U, h# ~
; X9 R4 r2 U: o/ [. Y6 E0 `2.整体概括
5 k; I7 Z5 v2 d( L6 O* eMPU6050内部整合了三轴MEMS陀螺仪、三轴MEMS加速度计以及一个可扩展的数字运动处理器DMP，而且还可以连接一个第三方数字传感器（比如：磁力计），这样的话，就可以通过IIC接口输出一个9轴信号。
) {5 P  ^4 o( r3 K2 @7 J更加方便的是，有了DMP，可以结合InvenSense公司提供的运动处理资料库，实现姿态解算。通过自带的DMP，可以通过IIC接口输出9轴融合演算的数据，大大降低了运动处理运算对操作系统的负荷，同时也降低了开发难度。

特点：
% W9 k  x# D* M9 K0 _, t① 以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧
, d7 J4 i% A6 ~* p2 U! P拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）
/ \6 [* @0 _) x2 l1 c② 具有 131 LSBs/° /sec 敏感度与全格感测范围为±250、±500、±1000 与±2000° /sec
的 3 轴角速度感测器(陀螺仪)
③ 集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器
④ 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移
⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算
0 G, M8 B6 n4 X: m  D" \' R数据、感测器同步化、姿势感应等的负荷
⑥ 内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求
, i5 d. b' H# ~5 \. P, [1 o⑦ 自带一个数字温度传感器
/ W7 g3 c) A4 ?! d: _& v4 E⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS
9 q; @" }" G& H; ~: j" V! u/ e" m⑨ 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降
) \( y+ C/ V- k# _% q5 }% E中断、 high-G 中断、零动作感应、触击感应、摇动感应功能
4 i# g2 k& O0 Y" f" m⑩ VDD 供电电压为 2.5V±5%、 3.0V±5%、 3.3V±5%； VLOGIC 可低至 1.8V± 5%
⑪ 陀螺仪工作电流： 5mA，陀螺仪待机电流： 5uA； 加速器工作电流： 500uA，加速器省
( T1 f% [1 W; C电模式电流： 40uA@10Hz
⑫ 自带 1024 字节 FIFO，有助于降低系统功耗
⑬ 高达 400Khz 的 IIC 通信接口
⑭ 超小封装尺寸： 4x4x0.9mm（QFN）

其检测轴如图所示：
+ I0 S  k4 I( J( F# P

+ v: H& W) a% u$ |" F

3.引脚说明



9 x/ |1 G" S. U0 `* h, T如图，MPU6050一共有8个引脚，实际上输出六轴数据时，只用了5个：VCC、GND、SCL、SDA、AD0。下面介绍一下引脚：
VCC：供电，3.3V即可
GND：接地
SCL：连接MCU的IIC时钟接口
SAD：连接MCU的IIC数据接口
XCL：连接外部设备的IIC时钟接口
4 r+ M/ Y2 h# Z' wXAD：连接外部设备的IIC数据接口
AD0：地址控制引脚（控制地址的最低位）
% R: h1 i0 y: n; d5 SINT：中断触发接口（不用）
* @  W$ G: B9 W' J  g5 yXCL、XDA只有在连接外部设备（比如磁力计的时候才用），AD0用来控制MPU6050的地址，如果AD0低电平，地址就是0X68；如果AD0高电平，地址就是0X69。
) Z$ m7 [6 s- ~. j
: I) ^7 i  m. p9 y9 r4.基本配置及相关寄存器
& q$ [& J3 n" K! \5 P& F3 r7 ZMCU与MPU6050的通信是建立在IIC通信机制上的，在IIC的基础上，可以实现对MPU6050的寄存器的操作，而MPU6050的运作就是过对寄存器进行读写。所以，了解相关的寄存器和对寄存器的操作是很有必要的。MPU6050的寄存器相关资料都可以在数据手册中查到，下面介绍一下几个重要的寄存器：

电源管理寄存器1
地址：0X68




主要位的功能：
: {+ W% O4 P8 W) R; Q* zDEVICE_RESET：控制复位，1表示复位，复位后会自动清零；
SLEEP：控制MPU6050工作模式，1表示睡眠模式，0表示正常工作模式，复位后改位为1，要手动将改位清零；
' P& T; n1 x0 Z. Y4 lTEMP_DIS：使能温度传感器位，0代表使能；
% ~$ I  `1 _2 f8 M9 z, i/ S4 o, i( Q, ~CLKSEL[2:0]：选择系统时钟源，一般采用PLL_X轴陀螺作为参考，具体的时钟源选择及相应位的值如图：
. \- B/ W2 Z( y  W5 j2 D
- S4 f# x' _4 w/ z8 Y+ q
! v6 e3 J% ~! l) h

/ C% b; L- q2 x% W& {- ?陀螺仪配置寄存器
+ t& ^: s: E1 P* W4 W地址：0X1B
. o: g! y, a& U* R  f0 [5 I  U* K, \
' C) o8 W( Q3 }2 K) G8 y4 k( `  k

主要位的功能：
9 i3 ~+ \+ \+ Q# \* W- V; H8 qFS_SEL[1:0]：设置陀螺仪满量程范围：为0代表±250° /S、为1代表±500° /S、为2代表±1000° /S为3代表±2000° /S；
8 H% N5 i5 p8 K1 Q$ t
陀螺仪的分辨率是16位，所以在最大量程下灵敏度为： 65536/4000=16.4LSB/(° /S)。

加速度计配置寄存器
  t. N) L  a" P2 u1 w地址：0X1C

1 Z' A2 r1 t+ E3 u; W
% a( z$ J; M/ u
: i4 D6 B+ V3 d$ s# _) a9 \( v主要位的功能：
2 c4 ~3 K4 i8 @/ }& m/ |AFS_SEL[1:0]：设置加速度计满量程范围：为 0代表±2g、为1代表±4g、为 2代表±8g、为 3代表±16g；

FIFO使能寄存器
3 k  O; ?! X% n6 L) O地址：0X23
: X7 M" [6 c0 \' N
3 {6 T) n/ x) a$ ~2 ?9 X
' v8 b% I9 r7 w. k! `. K% p
5 }! l/ q2 w6 F# {用来控制FIFO功能，相应位对应着相应的传感器FIFO功能，为0代表禁止，为1代表使能。注意：加速度传感器的三个轴的FIFO功能由一个位ACCEL_FIFO_EN控制。在简单读取传感器数据的情况下可以不使用FIFO。
0 _2 E# ^9 V4 H# `- Z3 W) @9 s
陀螺仪采样率分频寄存器
6 V  N5 {& Z# E& d1 a8 `, j( U地址：0X19
& ?# h/ m% B. Y


, m% f. H% |1 V改寄存器用来设置MPU6050陀螺仪的采样频率，与之相关的是陀螺仪的输出频率，俩者关系是：采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)
- z( A- r0 T, k) ]陀螺仪输出频率与数字低通滤波器（DLPF）有关，DLPF滤波频率一般设置为采样率的一半。
: c( x0 J) U0 i) d1 k/ j3 ]
1 c4 M. P5 z) O8 I+ R温度传感器寄存器
地址：高八位0X41、低八位0X42
直接通过读取寄存器中的值来得到温度数据，温度换算公式为：
. z3 U5 S( f; g. Y/ X% \Temperature = 36.53 + regval/340

二.代码详解
5 C4 M6 F4 h' U8 Y0 P1 X1.框架
( X% I% L; L3 U* F$ E) m- d我是用STM32驱动MPU6050，MPU6050输出原始的六轴数据，经过DMP处理（有库）得到四元数，再由四元数算出欧拉角：yaw、roll、pitch。由串口打印在电脑屏幕上。
5 c$ E3 F6 t# c0 X7 j; z首先，要做底层的IIC驱动，用来和MPU6050建立通信，我在mpu_iic.c中实现了；
: p' a8 }3 W$ t! B+ }; E然后，有了底层的驱动，就要写一些函数来与MPU6050交流了（通过读写寄存器），还可以写入命令、配置MPU6050、读取原始数据等等，这些操作我都写在mpu6050.c中，当然在mpu6050.h头文件中还包含了MPU各寄存器地址和相关指令。
通过mpu6050.c的实现，就可以读出原始六轴数据，下一步就是通过DMP将原始数据转换为四元数，这一步的DMP算法我水平有限，只能移植InvenSense公司提供的例程。关于移植DMP算法，由于DMP算法本质也是对MPU6050的操作，所以我们只需要向移植过来的算法提供：对MPU6050寄存器执行读和写的函数接口即可，最后通过移植过来的函数直接读出四元数！
然后，就是将四元数转换为欧拉角了，这个比较简单，一个函数就可以实现。
; A3 C* O2 {/ P: K7 x最后，打印串口到屏幕。
! l! X, ]9 H  a  e1 H/ x
下面给出各函数文件

# j$ X/ r4 |; _4 r2.mpu_iic.c/mpu_iic.h
* C" d* h5 ~4 y* C8 Empu_iic.h
9 z  M$ _# U9 F1 F4 E8 K主要是宏定义对引脚电平的操作和进行函数声明。
% C8 f. l+ y* v7 G1 G& a; f

1. #ifndef __MPU_IIC_H
   
2. #define __MPU_IIC_H
   
3. 
   1 \  m, e$ s: ^* P& _; @
4. #include "stm32f10x.h"
   2 n+ O& y+ A5 o% E; W& ?& R
5. #include "delay.h"
     \4 Y" L7 O- ]$ I) Y& Q
6. 
   
7. /* 宏定义引脚电平操作函数 */
   
8. #define MPU_SDA_IN()  {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}
   ) r1 Y+ {/ ^9 A0 w
9. #define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;}  
   8 d6 B7 G6 H4 P% s8 U# V7 i
10. 
    
11. 
    9 S- h) y3 |  g
12. #define MPU_IIC_SDA_1() GPIO_SetBits( GPIOB, GPIO_Pin_11 )
    
13. #define MPU_IIC_SDA_0() GPIO_ResetBits( GPIOB, GPIO_Pin_11 )
    7 i. Z3 [4 c! O( K% y# c
14. 
    0 ]% k" h" J, S
15. #define MPU_IIC_SCL_1() GPIO_SetBits( GPIOB, GPIO_Pin_10 )
    / t! B- j: [, b' P) Y# \) X+ q
16. #define MPU_IIC_SCL_0() GPIO_ResetBits( GPIOB, GPIO_Pin_10 )
    ! C% r# ]+ N7 v. k0 P
17. 
    
18. #define MPU_IIC_AD0_1() GPIO_SetBits( GPIOA, GPIO_Pin_15 )
    % ~3 f3 p+ ~& C4 ^1 G
19. #define MPU_IIC_AD0_0() GPIO_ResetBits( GPIOA, GPIO_Pin_15 )
    " p0 c7 j; t9 G
20. 
    
21. #define MPU_IIC_SDA_READ() GPIO_ReadInputDataBit( GPIOB, GPIO_Pin_11 )
    
22. 
    1 Y) a( }. Z0 M' L% I! ^
23. #define MPU_IIC_Delay() delay_us(2)
    
24. 
    6 E- @' m/ M: R: M, A2 l
25. /* 函数声明 */
    . y: N* v4 h8 e! m+ `' p
26. void MPU_IIC_Init( void );
    " g2 i- I- [( {* e* U) ^" A* B/ H7 B" ?
27. void MPU_IIC_Start( void );
    
28. void MPU_IIC_Stop( void );
    / d- D4 I, i4 w) O$ ]- r
29. uint8_t MPU_IIC_Wait_Ack( void );
    
30. void MPU_IIC_Ack( void );
      G4 v+ y/ B3 p
31. void MPU_IIC_NAck( void );
    
32. void MPU_IIC_Send_Byte( uint8_t data );
    % g: v# x  p2 G0 T5 j* k
33. uint8_t MPU_IIC_Read_Byte( uint8_t ack );
    ! z- a% t$ I4 A* n
34. 
    
35. #endif
    
36. 
    8 J$ Z4 r" a; e9 q' O3 g' p2 {$ M

复制代码

  _, y8 k+ f3 Z, zmpu_iic.c
2 p5 @& k/ o: t$ H. ^3 C4 G通过软件模拟IIC的代码，没什么好说的。

1. #include "mpu_iic.h"
   . G# Y& p4 w8 N# W6 z
2. #include "usart.h"
   
3. 
   , m! n' [0 f# i8 e+ u5 h
4. /*
   ' o: z7 d% M0 p) B
5. IIC接口引脚配置
   
6. SDA:PB11
   2 M9 g. i9 T5 m8 r
7. SCL:PB10
   
8. AD0:PB2
   ) T3 c' _0 b0 m; m, k  Q0 g, a
9. */
   . S1 \8 c$ W' U4 U& i
10. void MPU_IIC_Init( void )
    
11. {
    
12.         GPIO_InitTypeDef  GPIO_InitStruct;
    
13.        
    
14.         RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE );
    
15.        
    
16.         GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    0 ^; s9 v5 j1 L) H3 M
17.         GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
    2 D1 Y5 S# O8 t6 z- R' U) ]0 g
18.         GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;
    4 Z  x1 a0 D) t  E' ^5 p0 X
19.         GPIO_Init( GPIOB, &GPIO_InitStruct );
    
20.        
    8 G9 \; p+ q; R9 h# B! [
21.         MPU_IIC_SDA_1();
    
22.         MPU_IIC_SCL_1();
    
23. 
    
24. }
      z& d; O/ Y0 ?2 t% s' t; S
25. 
    
26. 
    
27. void MPU_IIC_Start( void )
    
28. {
    1 J% W  J6 s6 R: {8 m3 n
29.         MPU_SDA_OUT();
    7 @" {+ _" @! K4 e( u9 H# g4 U
30.        
    
31.         MPU_IIC_SDA_1();
    1 t2 n! l' Q! z; M; n
32.         MPU_IIC_SCL_1();
    
33.         delay_us(2);
    
34.         MPU_IIC_SDA_0();
    . x6 o& c: E5 Q8 \" L
35.         delay_us(2);
    
36.         MPU_IIC_SCL_0();
    
37. }
    : A$ d# @# S* l5 _% `9 D: O
38. 
    2 p, w% V+ @9 z8 t( _
39. void MPU_IIC_Stop( void )
    
40. {
    2 L" b0 h" I' {
41.         MPU_SDA_OUT();
    
42.        
    ) p6 @  ~8 i0 ?. s' T; [0 q
43.         MPU_IIC_SDA_0();
    - i- y- k/ L' S; ~
44.         MPU_IIC_SCL_1();
    " d6 j3 b* N) P2 o/ V3 A
45.         delay_us(2);
    
46.         MPU_IIC_SDA_1();
    6 P- r' |: ^) \1 m
47.         MPU_IIC_SCL_1();
    ! k- l; |4 g9 Y& C
48.         delay_us(2);
    
49.        
    
50. }
    
51. 
    
52. /* 由从设备在SCL为高电平的时候拉低SDA作为应答
    ! \9 [5 G; q6 H/ Y; `
53. 返回值：1：未应答
    4 |  j  E2 A/ \* L7 z. P8 b7 T
54.         0：已应答
    
55. */
    
56. uint8_t MPU_IIC_Wait_Ack( void )
    
57. {
    
58.         uint8_t count;
      E. ~' t+ v5 W1 Q- O5 ^- P
59.         MPU_SDA_IN();
    
60.        
    
61.         MPU_IIC_SCL_1();
      C' y1 T) m' k' G/ L& ^6 x
62.         delay_us(2);
    9 ~  ~& e- ]0 H% R
63.         MPU_IIC_SDA_1();
    + w: d+ }3 c% _, [1 u: d: P
64.         delay_us(2);
    / B' n; Q6 g, o  {! I3 O! z, ~
65.        
    
66.         while( MPU_IIC_SDA_READ()==1 )
    8 p# ^, E& m1 Z) K' T
67.         {
    
68.                 count++;
    
69.                 if( count>250 )
    9 o/ `) y# p7 q5 U: H* z/ t( [7 U( F
70.                 {
    
71.                         MPU_IIC_Stop();
    & m* M( |8 p% Q' H
72.                         return 1;
    0 E! p6 L; M1 `% A$ ]# W+ L  A
73.                 }
    3 c  o0 K2 R$ k% `' z9 C* H0 D
74.         }       
    
75.         MPU_IIC_SCL_0();
    6 G, u" _0 L" C/ S8 J
76.         return 0;
    8 [+ v3 w, x) l! k5 N
77. }
    
78. 
    1 R! D1 d! [  f
79. 
    
80. void MPU_IIC_Ack( void )
    
81. {
    2 h) P. z  K, H" z
82.        
    
83.         MPU_IIC_SCL_0();
    
84.         MPU_SDA_OUT();
    5 f4 }- E( l+ f( Y  W4 p
85.         MPU_IIC_SDA_0();
    
86.         delay_us(2);
    ' P9 e+ {/ w( }, W2 D+ z, x+ d
87.         MPU_IIC_SCL_1();
    
88.         delay_us(2);
    
89.         MPU_IIC_SCL_0();
    7 Z$ U; f: a! s3 G; n
90.        
    4 G& k/ @% w( \# e9 z, J
91. }
    
92. 
    * F3 _. X# b* {2 p6 O/ ?
93. 
    & U+ I8 a" L  ?8 b
94. void MPU_IIC_NAck( void )
    ; ~) u* O  ~$ ]: M
95. {
    
96.        
    
97.        
    4 w" P+ ]5 G8 X$ Q: h* [
98.         MPU_IIC_SCL_0();
    * m! A- \2 T& P+ C! y
99.         MPU_SDA_OUT();
    7 @: M( L7 T! F2 C2 ?) ?0 b
100.         MPU_IIC_SDA_1();
     # G$ ~# m' d/ ~/ I- [
101.         delay_us(2);
     . Z+ Q2 N9 O) O" K% a& J/ u
102.         MPU_IIC_SCL_1();
     
103.         delay_us(2);
     
104.         MPU_IIC_SCL_0();
     
105. }
     7 @+ c- o# b$ I2 _' }+ O: ^
106. 
     # I  \9 L# S  m- [+ c0 e
107. 
     8 Q4 G8 l: k. @$ h4 [( Q: l+ G
108. /* 发送一个字节数据，高位先行 */
     ! N1 v2 L; F1 b+ ]) }9 R$ n/ D$ l  u
109. void MPU_IIC_Send_Byte( uint8_t data )
     8 }2 \& V2 }* O
110. {
     
111.         uint8_t t;
     
112.         MPU_SDA_OUT();
     ! X1 h* O3 G7 M3 z) s( j. B
113.        
     
114.         MPU_IIC_SCL_0();
     - M' g8 r6 c  e9 {9 K  S
115.         for( t=0;t<8;t++ )
     1 k8 n: z" H7 e
116.         {
     
117.                 if( ((data&0x80)>>7)==1 )
     
118.                         MPU_IIC_SDA_1();
     
119.                 else
     
120.                         MPU_IIC_SDA_0();
     , Q  `- |) ]7 G! i( o- k$ v
121.                 data<<=1;
     / b1 x% [' r8 `% F. |# f/ ~" |
122.                 MPU_IIC_SCL_1();
     
123.                 delay_us(2);
     $ L) L, x: `- R. s: o" m$ f$ g; n
124.                 MPU_IIC_SCL_0();
     
125.                 delay_us(2);
     * ~) P/ h; \- r3 k  f$ h
126.         }
     
127. }
     6 z# ?9 x$ b" Z1 p( w' E
128. 
     
129. 
     
130. /* 读取一个字节，ack=1时，读取完成后主机发送应答 */
     
131. uint8_t MPU_IIC_Read_Byte( uint8_t ack )
     . C' {- S* K  W- U+ _/ }  T
132. {
     5 @) y6 O# ]" P  y$ a" F8 b4 [
133.         uint8_t t,data=0;
     
134.         MPU_SDA_IN();
     . w9 M$ U. U) P3 n
135.         for( t=0;t<8;t++ )
     
136.         {
     
137.                 MPU_IIC_SCL_0();
     . i$ H& ?$ K& b
138.                 delay_us(2);//等待SDA的变化
     ' B4 H1 }0 h) J9 ?
139.                 MPU_IIC_SCL_1();
     & `# Y/ r# a! A" {. g3 ^: a
140.                
     
141.                 data<<=1;//必须在读取前面，因为之后一位读取后就不再移位
       ~7 {4 m% q0 m7 v8 {$ l  ?
142.                 if( MPU_IIC_SDA_READ()==1 )
     6 k! B) g& i: N& H0 M" A
143.                         data++;
     3 r; W! U$ n  s
144.                
     ( s0 Q6 q8 I% a, {% R- |
145.                 delay_us(2);//等待SDA的变化
     * o- Z& f( A" j6 |
146.                
     + ^  D8 b/ Y/ j# F# {7 `/ T0 g
147.         }
     
148. 
     % S( |4 b4 ^  F8 Z" q
149.         if( !ack )
     5 b4 S; n4 Q% G# l/ V, B: f0 \3 `
150.                 MPU_IIC_NAck();//发送nACK
     3 M9 J+ J0 `* T3 A
151.     else
     ' }! z2 c3 j$ _- O6 V; N6 z
152.         MPU_IIC_Ack(); //发送ACK
     
153.         return data;
     
154. }
     
155. 
     . M( f) s* Q$ s. m% U
156. 
     8 d' R- Y7 A( y2 D4 j, y3 t7 W

复制代码

7 w, Y6 ?$ b& O& ~9 V4 W3.mpu6050.c/mpu6050.h
1 c; j; P' J5 k4 {$ Vmpu6050.h
; C5 f: ?' {5 f2 c; W& }1 c: ?主要是定义MPU相关寄存器的地址，和进行函数声明。
4 B! L6 b2 ^; l7 t! \4 B

1. #ifndef __MPU6050_H
   
2. #define __MPU6050_H
   . ?( _8 o" r2 Q3 C. j4 B9 h# G
3. 
   
4. #include "stm32f10x.h"
   
5. #include "mpu_iic.h"
   0 I! H' ^1 T0 i  _# d  O+ V
6. 
   
7. 
   * @3 I8 L; G6 R- |$ F- ?
8. /* AD0接地，MPU6050的IIC地址为0x68   接3.3V就为0x69*/
   6 z( l) g4 d+ o/ U6 U
9. #define MPU_ADDR                                0X68
   4 k9 P) d3 n0 b+ M1 k- \5 o- g
10. 
    ! X) T3 l# f( h8 G% ?) A7 n# x8 {
11. /************** MPU6050相关寄存器地址 *********************/
    7 }7 S/ F. z8 Y
12. #define MPU_ACCEL_OFFS_REG                0X06        //accel_offs寄存器,可读取版本号,寄存器手册未提到
    
13. #define MPU_PROD_ID_REG                        0X0C        //prod id寄存器,在寄存器手册未提到
    
14. #define MPU_SELF_TESTX_REG                0X0D        //自检寄存器X
    " A  P+ }. _8 G. ^
15. #define MPU_SELF_TESTY_REG                0X0E        //自检寄存器Y
    . F6 u! b6 [( _  z+ b. _0 E. T
16. #define MPU_SELF_TESTZ_REG                0X0F        //自检寄存器Z
    
17. #define MPU_SELF_TESTA_REG                0X10        //自检寄存器A
    0 W$ a" f6 h) M4 i
18. #define MPU_SAMPLE_RATE_REG                0X19        //采样频率分频器
    ; _% z1 D2 F; C! \4 ^2 d3 A: b7 F) Q
19. #define MPU_CFG_REG                                0X1A        //配置寄存器
    
20. #define MPU_GYRO_CFG_REG                0X1B        //陀螺仪配置寄存器
    
21. #define MPU_ACCEL_CFG_REG                0X1C        //加速度计配置寄存器
    # b" A. |" r1 b3 w0 ]- X6 k0 `  l/ H
22. #define MPU_MOTION_DET_REG                0X1F        //运动检测阀值设置寄存器
    
23. #define MPU_FIFO_EN_REG                        0X23        //FIFO使能寄存器
    $ Z! n9 g. e2 G2 H8 r
24. #define MPU_I2CMST_CTRL_REG                0X24        //IIC主机控制寄存器
    3 k2 t2 d+ n5 J2 w/ o: ]( H& }- H; S
25. #define MPU_I2CSLV0_ADDR_REG        0X25        //IIC从机0器件地址寄存器
    3 B% O9 T" y0 D. y3 f6 S
26. #define MPU_I2CSLV0_REG                        0X26        //IIC从机0数据地址寄存器
    
27. #define MPU_I2CSLV0_CTRL_REG        0X27        //IIC从机0控制寄存器
    * k5 C: y8 X' }3 a9 S) R) O8 T! l  n
28. #define MPU_I2CSLV1_ADDR_REG        0X28        //IIC从机1器件地址寄存器
    4 x* g$ u% O4 n' K5 K( S
29. #define MPU_I2CSLV1_REG                        0X29        //IIC从机1数据地址寄存器
    
30. #define MPU_I2CSLV1_CTRL_REG        0X2A        //IIC从机1控制寄存器
    9 q  p4 Q- P" J, o
31. #define MPU_I2CSLV2_ADDR_REG        0X2B        //IIC从机2器件地址寄存器
    
32. #define MPU_I2CSLV2_REG                        0X2C        //IIC从机2数据地址寄存器
    
33. #define MPU_I2CSLV2_CTRL_REG        0X2D        //IIC从机2控制寄存器
    
34. #define MPU_I2CSLV3_ADDR_REG        0X2E        //IIC从机3器件地址寄存器
    
35. #define MPU_I2CSLV3_REG                        0X2F        //IIC从机3数据地址寄存器
    
36. #define MPU_I2CSLV3_CTRL_REG        0X30        //IIC从机3控制寄存器
    3 d- s+ r; Q$ n2 p
37. #define MPU_I2CSLV4_ADDR_REG        0X31        //IIC从机4器件地址寄存器
    
38. #define MPU_I2CSLV4_REG                        0X32        //IIC从机4数据地址寄存器
    
39. #define MPU_I2CSLV4_DO_REG                0X33        //IIC从机4写数据寄存器
    
40. #define MPU_I2CSLV4_CTRL_REG        0X34        //IIC从机4控制寄存器
    3 |+ U! ], p* m! r& g( W* _3 g% @
41. #define MPU_I2CSLV4_DI_REG                0X35        //IIC从机4读数据寄存器
    5 ?6 n& D0 L, s" ~- @
42. 
    ! k/ @. l- m+ t
43. #define MPU_I2CMST_STA_REG                0X36        //IIC主机状态寄存器
    
44. #define MPU_INTBP_CFG_REG                0X37        //中断/旁路设置寄存器
    
45. #define MPU_INT_EN_REG                        0X38        //中断使能寄存器
    0 N5 Q) u+ t: X2 g- U. Z
46. #define MPU_INT_STA_REG                        0X3A        //中断状态寄存器
    . y  x: |# h1 @* j3 J0 w/ n/ U
47. 
    6 s  z1 C  [0 }4 B, u
48. #define MPU_ACCEL_XOUTH_REG                0X3B        //加速度值,X轴高8位寄存器
    
49. #define MPU_ACCEL_XOUTL_REG                0X3C        //加速度值,X轴低8位寄存器
    
50. #define MPU_ACCEL_YOUTH_REG                0X3D        //加速度值,Y轴高8位寄存器
    
51. #define MPU_ACCEL_YOUTL_REG                0X3E        //加速度值,Y轴低8位寄存器
      E/ Q, M  s+ l
52. #define MPU_ACCEL_ZOUTH_REG                0X3F        //加速度值,Z轴高8位寄存器
    
53. #define MPU_ACCEL_ZOUTL_REG                0X40        //加速度值,Z轴低8位寄存器
    / G8 l0 o% E  H, [4 j7 i% p
54. 
    7 W' [$ p! u0 q# M2 G5 e
55. #define MPU_TEMP_OUTH_REG                0X41        //温度值高八位寄存器
    & C& }. u) M8 b# O
56. #define MPU_TEMP_OUTL_REG                0X42        //温度值低8位寄存器
    
57. 
    ( @$ w3 n% o+ {& m! q) I
58. #define MPU_GYRO_XOUTH_REG                0X43        //陀螺仪值,X轴高8位寄存器
    ( b* l3 e' N/ j) A9 e3 A
59. #define MPU_GYRO_XOUTL_REG                0X44        //陀螺仪值,X轴低8位寄存器
    
60. #define MPU_GYRO_YOUTH_REG                0X45        //陀螺仪值,Y轴高8位寄存器
    
61. #define MPU_GYRO_YOUTL_REG                0X46        //陀螺仪值,Y轴低8位寄存器
    
62. #define MPU_GYRO_ZOUTH_REG                0X47        //陀螺仪值,Z轴高8位寄存器
    
63. #define MPU_GYRO_ZOUTL_REG                0X48        //陀螺仪值,Z轴低8位寄存器
    
64. 
    9 @" a% M5 B7 o, z( h) ^7 J
65. #define MPU_I2CSLV0_DO_REG                0X63        //IIC从机0数据寄存器
    
66. #define MPU_I2CSLV1_DO_REG                0X64        //IIC从机1数据寄存器
    ' X7 _* N- a+ Y" D9 z0 B
67. #define MPU_I2CSLV2_DO_REG                0X65        //IIC从机2数据寄存器
    
68. #define MPU_I2CSLV3_DO_REG                0X66        //IIC从机3数据寄存器
    
69. 
    
70. #define MPU_I2CMST_DELAY_REG        0X67        //IIC主机延时管理寄存器
    - l0 P) J. S' [% O6 G  x: Q/ b* ~
71. #define MPU_SIGPATH_RST_REG                0X68        //信号通道复位寄存器
    ; v" T- I, _8 H8 j5 t# y1 m) `  K
72. #define MPU_MDETECT_CTRL_REG        0X69        //运动检测控制寄存器
    
73. #define MPU_USER_CTRL_REG                0X6A        //用户控制寄存器
    % U* F6 z" T% p) P# d
74. #define MPU_PWR_MGMT1_REG                0X6B        //电源管理寄存器1
    5 a4 j2 D' U5 t$ F0 g. G  q
75. #define MPU_PWR_MGMT2_REG                0X6C        //电源管理寄存器2
    
76. #define MPU_FIFO_CNTH_REG                0X72        //FIFO计数寄存器高八位
    4 J4 D& b& J& [4 b/ c" l
77. #define MPU_FIFO_CNTL_REG                0X73        //FIFO计数寄存器低八位
    
78. #define MPU_FIFO_RW_REG                        0X74        //FIFO读写寄存器
    
79. #define MPU_DEVICE_ID_REG                0X75        //器件ID寄存器
    
80. 
    $ Y* u' h- w# R. H2 S
81. 
    ! c' v) Y. c! c6 a  q+ J
82. 
    1 q$ ?4 d! |1 c6 K" o- a/ a
83. 
    0 J1 c* s) t' C% f. S! a
84. /* 函数声明 */
    
85. uint8_t MPU_Read_Byte( uint8_t reg );
    
86. uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data );
    0 [7 v8 v7 x% f! o
87. uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf );
    
88. uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf );
    7 g* p, e5 ]" ?. |% f
89. 
    2 R% j* p* l# ~0 Q, a
90. uint8_t MPU_Init( void );
    ) [- R( o; }4 P, y  h  g7 C$ V
91. 
    
92. 
    
93. uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr );
    
94. uint8_t MPU_Set_Accel_Fsr( uint8_t fsr );
    : f; K( U3 I2 D0 h
95. uint8_t MPU_Set_LPF( uint16_t lpf );
    % A+ j" ~% h  y( x2 w9 N
96. uint8_t MPU_Set_Rate( uint16_t rate );
    6 _. \- E5 O) i1 X( n$ N
97. short MPU_Get_Temperature( void );
    
98. uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz );
    6 z# r. G5 S0 N) s# p
99. uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az );
    
100. 
     
101. #endif
     ) P. R1 @6 G! S3 E6 Z% E
102. 
     

复制代码

' d( f' C8 i7 T5 Z! r  [mpu6050.c
0 Z+ \) m, M% T, m最为重要的一部分代码，包括了对MPU6050的一系列基本配置和读取原始数据的操作，代码都注解的很详细了。
AD0引脚用了PA15，所以要GPIO_Remap_SWJ_JTAGDisable，换个引脚也可以。
: q5 z: i; x* e* S  ~6 L

1. #ifndef __MPU6050_H
   1 i& E& O' q/ y! }7 }- K2 j
2. #define __MPU6050_H
   . F9 I& e1 f" |, q" k
3. 
   ; M0 P, I3 L" K: K4 J
4. #include "stm32f10x.h"
   
5. #include "mpu_iic.h"
   # G# F' s4 c: m' h* D) q6 {
6. 
   
7. 
   3 B+ ?) b2 J* V3 }! U# Y' x
8. /* AD0接地，MPU6050的IIC地址为0x68   接3.3V就为0x69*/
   
9. #define MPU_ADDR                                0X68
   
10. 
    % c! T, K- ~9 T1 P8 P
11. /************** MPU6050相关寄存器地址 *********************/
    * x/ C6 E: C  E9 s  q9 L
12. #define MPU_ACCEL_OFFS_REG                0X06        //accel_offs寄存器,可读取版本号,寄存器手册未提到
    / F9 J! B/ i6 w3 k/ \' {3 M
13. #define MPU_PROD_ID_REG                        0X0C        //prod id寄存器,在寄存器手册未提到
    % P  T5 R, C/ e) Y1 Z1 x
14. #define MPU_SELF_TESTX_REG                0X0D        //自检寄存器X
    
15. #define MPU_SELF_TESTY_REG                0X0E        //自检寄存器Y
    
16. #define MPU_SELF_TESTZ_REG                0X0F        //自检寄存器Z
    0 R3 j9 h9 t3 _5 b- q6 V6 U
17. #define MPU_SELF_TESTA_REG                0X10        //自检寄存器A
    
18. #define MPU_SAMPLE_RATE_REG                0X19        //采样频率分频器
    
19. #define MPU_CFG_REG                                0X1A        //配置寄存器
    
20. #define MPU_GYRO_CFG_REG                0X1B        //陀螺仪配置寄存器
    0 ]( K% _' w  M7 v6 h& w  [
21. #define MPU_ACCEL_CFG_REG                0X1C        //加速度计配置寄存器
    
22. #define MPU_MOTION_DET_REG                0X1F        //运动检测阀值设置寄存器
    
23. #define MPU_FIFO_EN_REG                        0X23        //FIFO使能寄存器
    
24. #define MPU_I2CMST_CTRL_REG                0X24        //IIC主机控制寄存器
    + v$ |6 f0 @+ u0 N4 R
25. #define MPU_I2CSLV0_ADDR_REG        0X25        //IIC从机0器件地址寄存器
    ! j, z- T- g; l* Z# d9 ?* L
26. #define MPU_I2CSLV0_REG                        0X26        //IIC从机0数据地址寄存器
    : Z  U' R# j" K1 h# G8 y3 d
27. #define MPU_I2CSLV0_CTRL_REG        0X27        //IIC从机0控制寄存器
    2 X# l2 D! s+ U
28. #define MPU_I2CSLV1_ADDR_REG        0X28        //IIC从机1器件地址寄存器
    
29. #define MPU_I2CSLV1_REG                        0X29        //IIC从机1数据地址寄存器
    
30. #define MPU_I2CSLV1_CTRL_REG        0X2A        //IIC从机1控制寄存器
    
31. #define MPU_I2CSLV2_ADDR_REG        0X2B        //IIC从机2器件地址寄存器
    
32. #define MPU_I2CSLV2_REG                        0X2C        //IIC从机2数据地址寄存器
    2 _. _& U7 X+ y& M+ e9 n( x
33. #define MPU_I2CSLV2_CTRL_REG        0X2D        //IIC从机2控制寄存器
    1 U$ ?+ e# V# a7 l! }& I8 q
34. #define MPU_I2CSLV3_ADDR_REG        0X2E        //IIC从机3器件地址寄存器
    
35. #define MPU_I2CSLV3_REG                        0X2F        //IIC从机3数据地址寄存器
    
36. #define MPU_I2CSLV3_CTRL_REG        0X30        //IIC从机3控制寄存器
    9 m  d! S) f. Y( Z
37. #define MPU_I2CSLV4_ADDR_REG        0X31        //IIC从机4器件地址寄存器
    
38. #define MPU_I2CSLV4_REG                        0X32        //IIC从机4数据地址寄存器
    
39. #define MPU_I2CSLV4_DO_REG                0X33        //IIC从机4写数据寄存器
    
40. #define MPU_I2CSLV4_CTRL_REG        0X34        //IIC从机4控制寄存器
    
41. #define MPU_I2CSLV4_DI_REG                0X35        //IIC从机4读数据寄存器
    . X  f8 M2 f" _( f! ]) w- m
42. 
    
43. #define MPU_I2CMST_STA_REG                0X36        //IIC主机状态寄存器
    
44. #define MPU_INTBP_CFG_REG                0X37        //中断/旁路设置寄存器
    
45. #define MPU_INT_EN_REG                        0X38        //中断使能寄存器
    
46. #define MPU_INT_STA_REG                        0X3A        //中断状态寄存器
    3 B8 c, m6 f7 `2 Z
47. 
    . O) q# E5 U, R$ M! v  Z
48. #define MPU_ACCEL_XOUTH_REG                0X3B        //加速度值,X轴高8位寄存器
    
49. #define MPU_ACCEL_XOUTL_REG                0X3C        //加速度值,X轴低8位寄存器
    % p' v) P2 O7 I0 A& d
50. #define MPU_ACCEL_YOUTH_REG                0X3D        //加速度值,Y轴高8位寄存器
    5 g) `4 r& G2 t' b' ^. C  d. p
51. #define MPU_ACCEL_YOUTL_REG                0X3E        //加速度值,Y轴低8位寄存器
    
52. #define MPU_ACCEL_ZOUTH_REG                0X3F        //加速度值,Z轴高8位寄存器
    2 h' n2 ?1 N" Y$ P7 L
53. #define MPU_ACCEL_ZOUTL_REG                0X40        //加速度值,Z轴低8位寄存器
    ) t! [- o# {" y
54. 
    : V) \9 _/ n6 p+ x( O9 p
55. #define MPU_TEMP_OUTH_REG                0X41        //温度值高八位寄存器
    
56. #define MPU_TEMP_OUTL_REG                0X42        //温度值低8位寄存器
    
57. 
    - Y9 r- r: M' ]1 X  `7 o0 d+ B
58. #define MPU_GYRO_XOUTH_REG                0X43        //陀螺仪值,X轴高8位寄存器
    
59. #define MPU_GYRO_XOUTL_REG                0X44        //陀螺仪值,X轴低8位寄存器
    3 e3 j6 }1 j+ _
60. #define MPU_GYRO_YOUTH_REG                0X45        //陀螺仪值,Y轴高8位寄存器
    
61. #define MPU_GYRO_YOUTL_REG                0X46        //陀螺仪值,Y轴低8位寄存器
    - }( f  J7 Y2 s( N5 j2 U4 W2 Q' q
62. #define MPU_GYRO_ZOUTH_REG                0X47        //陀螺仪值,Z轴高8位寄存器
    0 h5 t  n% M# @) m; m7 G
63. #define MPU_GYRO_ZOUTL_REG                0X48        //陀螺仪值,Z轴低8位寄存器
    1 M# J# U2 ^! X) C( `2 {
64. 
    % v) p. i: M: A& g1 Y1 N; j
65. #define MPU_I2CSLV0_DO_REG                0X63        //IIC从机0数据寄存器
    , x  c# @7 i, c* N, {. q
66. #define MPU_I2CSLV1_DO_REG                0X64        //IIC从机1数据寄存器
    
67. #define MPU_I2CSLV2_DO_REG                0X65        //IIC从机2数据寄存器
    + N3 c" a+ j" y( ]5 e4 ^$ q6 ?( [
68. #define MPU_I2CSLV3_DO_REG                0X66        //IIC从机3数据寄存器
    
69. 
    : u# l7 v4 V+ p/ s
70. #define MPU_I2CMST_DELAY_REG        0X67        //IIC主机延时管理寄存器
    
71. #define MPU_SIGPATH_RST_REG                0X68        //信号通道复位寄存器
    : K" Y4 L, [# m/ n
72. #define MPU_MDETECT_CTRL_REG        0X69        //运动检测控制寄存器
    
73. #define MPU_USER_CTRL_REG                0X6A        //用户控制寄存器
    
74. #define MPU_PWR_MGMT1_REG                0X6B        //电源管理寄存器1
    5 v& a2 W9 {$ v+ m7 w7 p, w
75. #define MPU_PWR_MGMT2_REG                0X6C        //电源管理寄存器2
    1 R& V4 b) e% `; Q$ c& S
76. #define MPU_FIFO_CNTH_REG                0X72        //FIFO计数寄存器高八位
    # i% o" d; S/ U$ f7 g. w% K4 @( P, H
77. #define MPU_FIFO_CNTL_REG                0X73        //FIFO计数寄存器低八位
    & ^0 J; `$ T: D* P+ G7 v
78. #define MPU_FIFO_RW_REG                        0X74        //FIFO读写寄存器
    
79. #define MPU_DEVICE_ID_REG                0X75        //器件ID寄存器
    " g, T# ?( Z7 d1 U3 ?
80. 
    
81. 
    
82. 
    
83. 
    * O& m" v/ i' j! J8 B' u8 l
84. /* 函数声明 */
    
85. uint8_t MPU_Read_Byte( uint8_t reg );
    
86. uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data );
    2 }/ ?6 u5 Q% l+ q2 d
87. uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf );
    
88. uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf );
    6 z3 u4 `8 q5 W7 b7 {6 L( q
89. 
    
90. uint8_t MPU_Init( void );
    ' A% i9 v% Q( p7 u+ a+ @( c
91. 
    
92. 
    ( ]* n. a" s$ s, j1 Q) G7 l
93. uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr );
    8 W+ ?) z/ _: H* f7 d
94. uint8_t MPU_Set_Accel_Fsr( uint8_t fsr );
    # l1 f; M' C% R+ ^  F3 j3 J
95. uint8_t MPU_Set_LPF( uint16_t lpf );
    8 A3 c' \1 G, b* G* x, |( x
96. uint8_t MPU_Set_Rate( uint16_t rate );
    $ ~* H7 ?  d4 C7 U$ v7 D3 X. y/ @
97. short MPU_Get_Temperature( void );
    3 H5 t  I" F! ?% b5 X) s/ n
98. uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz );
    9 O. ?( ^$ {) ^6 O
99. uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az );
      Y/ [6 y/ W5 i: m+ ?. F
100. 
     
101. #endif
     ( f3 X6 ?0 J% A2 n  U7 w
102. 
     

复制代码

! C+ _8 @8 I, J0 M/ c3 @, u4.DMP相关代码
要想使用DMP求欧拉角的代码，包含下面这几个文件即可，下面列出接口函数，到时候使用时直接使用接口函数即可。

5 b0 {+ y1 q( Q' U- I: X6 B

; _, g' f8 N: \0 g: B6 c) s向DMP算法提供的接口宏定义
6 t4 H7 Q( q6 r) A% m) o只需要提供：对MPU6050的读写操作函数和延时函数即可

1. #define i2c_write   MPU_Write_Continue
   5 m/ }2 U5 K; A# h5 [9 v
2. #define i2c_read    MPU_Read_Continue
   
3. #define delay_ms    delay_ms
   - G$ v$ w3 f. u: Z! v+ d. j, {

复制代码

, k" U0 B6 h) {+ d9 c; ADMP初始化
: Z& V# \" T) |$ T/ Y. Y( J

1. //mpu6050,dmp初始化
   
2. //返回值:0,正常
   0 U9 E( F% E& ]0 n+ S
3. //    其他,失败
   
4. uint8_t mpu_dmp_init(void)
   
5. {
   : \/ ]. G$ G' C: m  N/ N' J
6.         uint8_t res=0;
   ; t1 j# q. `8 d( H
7.         MPU_IIC_Init();         //初始化IIC总线
   
8.         if(mpu_init()==0)        //初始化MPU6050
   
9.         {         
   
10.                 res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
    
11.                 if(res)return 1;
    6 P  ~9 g% p/ a! l7 _
12.                 res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
    
13.                 if(res)return 2;
    & Z: r$ C9 `+ R0 U$ Z% V
14.                 res=mpu_set_sample_rate(DEFAULT_MPU_HZ);        //设置采样率
    
15.                 if(res)return 3;
    
16.                 res=dmp_load_motion_driver_firmware();                //加载dmp固件
    ) ~5 e+ T* U% S, T
17.                 if(res)return 4;
    
18.                 res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
    2 x, Z% z7 }8 e( m. C
19.                 if(res)return 5;
    8 K# n5 n8 _1 z% W) m, e& }
20.                 res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|        //设置dmp功能
    
21.                     DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
    % T/ W* N" ]$ f# ]2 i
22.                     DMP_FEATURE_GYRO_CAL);
    3 Y- ]* K1 x& T" K  p9 j5 L
23.                 if(res)return 6;
    
24.                 res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);        //设置DMP输出速率(最大不超过200Hz)
    $ c; \, ?6 A' `* f  ~6 {- W8 _8 t
25.                 if(res)return 7;   
    
26.                 res=run_self_test();                //自检
    8 F5 a8 e" Z+ r4 w' f: L# V6 t9 \# H3 i
27.                 if(res)return 8;   
    
28.                 res=mpu_set_dmp_state(1);        //使能DMP
    8 S, O8 [3 m3 _/ Y* `* f3 i& E  D; u* M
29.                 if(res)return 9;     
    % x+ h1 C! ?! P# b- w$ n2 G7 h+ p
30.         }else return 10;
    
31.         return 0;
    & a; S5 l! H/ S9 g7 E: q6 a
32. }
    

复制代码

获取欧拉角
* u) M8 K# v/ f8 G7 g- ^) k# w

1. //得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
   
2. //pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
   
3. //roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
   
4. //yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
   
5. //返回值:0,正常
   
6. //    其他,失败
   
7. uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
   
8. {
   
9.         float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
   
10.         unsigned long sensor_timestamp;
    
11.         short gyro[3], accel[3], sensors;
    
12.         unsigned char more;
    
13.         long quat[4];
    
14.         if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;         
    0 Y7 `8 R0 J! q/ _1 Q( L
15.         /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
    
16.          * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
    9 Y' X1 l* o  M- L6 r; y$ L
17.         **/
    & X* @& M# {  C/ ]/ ]! R
18.         /*if (sensors & INV_XYZ_GYRO )
    
19.         send_packet(PACKET_TYPE_GYRO, gyro);
    
20.         if (sensors & INV_XYZ_ACCEL)
    
21.         send_packet(PACKET_TYPE_ACCEL, accel); */
    
22.         /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
    : n( N$ O* B3 ^5 ~; k3 d
23.          * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
    
24.         **/
    ; N! g3 B% `4 a. ?% {+ v
25.         if(sensors&INV_WXYZ_QUAT)
    
26.         {
    6 \' d( e) X$ o* \5 r3 e
27.                 q0 = quat[0] / q30;        //q30格式转换为浮点数
    8 f% D- o7 [4 \! }, g  c- M# [" N' D
28.                 q1 = quat[1] / q30;
    
29.                 q2 = quat[2] / q30;
    + ]+ U1 ^9 ^/ k% M& O) @
30.                 q3 = quat[3] / q30;
    $ d. }: O, D5 W  O
31.                 //计算得到俯仰角/横滚角/航向角
    2 S" V4 d" Y- a, u9 H, |
32.                 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;        // pitch
    9 Q+ f) M# r2 n
33.                 *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;        // roll
    , [! b1 g: C0 n& F
34.                 *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    / r% ]5 R: N4 T$ [. a1 E0 s$ `! ~
35.         }else return 2;
    
36.         return 0;
    
37. }
    . L( a* k$ R  M" g6 D/ E
38. 
    

复制代码

0 P! \" t$ _) r# p5.mian()函数
主函数中对MPU6050进行初始化后，对DMP也进行初始化，然后就可以直接使用mpu_dmp_get_data()获取欧拉角，还可以获取温度值。
: |* ]! P4 N2 v  E' O3 |  k" Y4 Q

1. #include "stm32f10x.h"   
   
2. #include "usart.h"
   % w# g# i& n1 [: ^, H( H
3. #include "delay.h"
     R% `7 Y. ~' U' w# N( [* a8 r
4. #include "mpu6050.h"
   8 v: T2 y2 j( p( g- W- i  y" Q/ Z
5. #include "inv_mpu.h"
   
6. #include "inv_mpu_dmp_motion_driver.h"
   4 T/ H; W5 J- l6 O' o# t
7. 
   6 o- z! @: I! P4 j: k# B
8. int main(void)
   4 P2 e# T7 @% a1 D; z7 L% [
9. {
   # H! `- E' G, i- w9 A; _8 B- Q% f
10.         uint8_t x=0;
    * r+ c# Y8 a* ~) R$ M
11.         float pitch,roll,yaw;                 //欧拉角
    . o6 Z5 i+ q1 A) i7 C) g
12.         short aacx,aacy,aacz;                //加速度传感器原始数据
    " l3 f5 u8 _, ?5 M. e) I
13.         short gyrox,gyroy,gyroz;        //陀螺仪原始数据
    
14.         short temp;                                        //温度
    
15.        
    / x; b/ f. M" ]
16.         NVIC_PriorityGroupConfig( 2 );
    7 Q; o. \, X% _$ b
17.         delay_init();
    & }- Q9 S( l- g' X( Y4 L! M
18.         USART1_Init(115200);       
    
19.         printf("程序开始\n");
    : E6 ]% W* ]4 P' B& j
20.        
    
21.         if( MPU_Init()!=0 )
    + J4 l3 w  j: z; p: t# v, j
22.         {
    " e; w0 L' w5 Z* r# J, O- E$ U) |
23.                 printf("MPU6050初始化错误！\n");
    + @6 ^0 H( b0 a5 q/ _9 {
24.                 return 0;
    
25.         }
    ) |5 ?: g9 e) K
26.                
    4 @- i. c. I$ n& u; ]+ e8 U! {
27.         if( mpu_dmp_init() )
    6 @& P1 b# Z8 t# a  T8 r& C
28.         {
    , G- Y; {: |3 t9 J, C9 I
29.                 printf("DMP初始化错误！\n");
    
30.                 return 0;
    ( `/ L! C+ L' I3 K% O. G& @
31.         }
    
32.         while(1)
    * g/ e6 L# m3 Q6 A, k8 B
33.         {
    
34.                 if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
    ( h5 `, v# Y  A
35.                 {
    5 F/ n; x& D! F0 A2 ^- m* p
36.                         temp=MPU_Get_Temperature();        //得到温度值
    " y2 I1 o& v* ^( y; t
37.                         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度传感器数据
    
38.                         MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        //得到陀螺仪数据
    0 P% c' Q7 E. X+ |
39.                 }
    
40.                 delay_ms(100);
    
41.                 printf("pitch:%02f  roll:%02f  yaw:%02f\n",pitch,roll,yaw);
    & D1 Z  P0 }$ Q) T. s! i
42.         }
      q: U5 a$ E1 c) z
43. 
    * M; l0 O- B. i* Y2 p6 o
44. }
    1 ^: q! l. f  M

复制代码

这就是我理解的MPU6050，后续做平衡小车的时候要读取欧拉角，先总结一下
7 ]1 ]' W$ w! Q4 |' {% T  B. f" H————————————————
# X& x" J) z3 F8 ]& ?版权声明：Aspirant-GQ
如有侵权请联系删除


, B  J# _6 M. s. D5 u