你的浏览器版本过低,可能导致网站不能正常访问!
为了你能正常使用网站功能,请使用这些浏览器。

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

[复制链接]
STMCU小助手 发布时间:2022-11-18 19:00
主要内容:
) C8 v6 B6 a7 U* V# Z+ a9 Y; m1)MPU6050简介;7 g# T% I- A- |# f7 q* ]7 I2 V* @
2)MPU6050相关寄存器介绍;
6 \! V. M3 ?: ~" y4 a5 o* L3)相关实验代码解读(除了案例可实现的功能外,主函数增加了在LCD屏上显示6轴的原始数据(含正负号)的功能)。
1 K6 o3 P2 r) E. u3 w
% }3 A2 D+ W  b  u一、什么是MPU6050?3 Y6 v& h/ u* o9 G3 {4 b
MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件,内带3轴陀螺仪和3轴加速度传感器,并且含有一个第二IIC接口,可用于连接外部磁力传感器,利用自带数字运动处理器(DMP: Digital Motion Processor)硬件加速引擎,通过主IIC接口,可以向应用端输出完整的9轴姿态融合演算数据。
) y) ~1 X* B: _$ s% e
) ^; U5 }  R9 R: x/ g5 V- i二、MPU6050特点
6 k+ ?7 j. F6 K1)自带数字运动处理(DMP: Digital Motion Processing),输出6轴或9轴(需外接磁传感器)姿态解算数据;5 y0 J0 k. h% |( ?+ i
2)集成可程序控制,测量范围为±250、±500、±1000与±2000°/sec 的3轴角速度感测器(陀螺仪);2 W  B4 v( j; ^1 w
3)集成可程序控制,范围为±2g、±4g、±8g和±16g的3轴加速度传感器;
  a1 [4 y9 R" A( }+ \' @4)自带数字温度传感器;' h( F; r( ]: J5 S
5)可输出中断(interrupt),支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G中断、零动作感应、触击感应、摇动感应功能;
4 u$ d7 g4 W$ J$ x6)自带1024字节FIFO,有助于降低系统功耗;
1 B1 V% j6 _" ?) @+ T" H7)高达400Khz的IIC通信接口;
) E; P0 u$ [* K0 N$ g# s8)超小封装尺寸:4x4x0.9mm(QFN)。
6 d+ S" K2 H1 a/ `7 S0 h( b% z% \" q2 T+ p  U6 u( `$ h
三、MPU6050框图' l( H2 g% }: y% D; p
20201202115316729.png & g! i& i; B/ ~$ U- p
AD0=0对应器件物理地址=0X68# k& r6 B: j6 I4 h1 E
AD0=1对应器件物理地址=0X69/ n" g. Z( {1 v# y# T

6 M5 j+ G- c. O6 G四、MPU6050初始化9 @+ O; F  K; G# q: W
1)初始化IIC接口;
3 }3 [( H! x7 x2)复位MPU6050,由电源管理寄存器1(0X6B)控制;
% @& _# v  d3 }2 x/ `3)设置角速度和加速度传感器的满量程范围,由陀螺仪配置寄存器(0X1B)和加速度传感器配置寄存器(0X1C)设置 ;) K9 M7 ]$ E6 s5 X- R# r
4)设置其他参数:
0 v( A/ E) k' d6 H4 q  ~5 i% F# `设置中断,由中断使能寄存器(0X38)控制;$ y% e: P9 K2 @  l  l6 N: c; O3 f% |
设置AUX IIC接口,由户控制寄存器(0X6A)控制;
& N$ p7 W0 C$ y2 ]设置FIFO,由FIFO使能寄存器(0X23)控制;& G5 M: N& T& \9 x7 ~1 o9 g
设置陀螺仪采样率 ,由采样率分频寄存器(0X19)控制;8 B4 ]. m4 d: d+ o) l0 ~7 s9 _
设置数字低通滤波器,由配置寄存器(0X1A)控制;  l% ^' v8 ^) ^% p# n& H- M' s; Z
5)设置系统时钟。由电源管理寄存器1(0X6B)控制。一般选择x轴陀螺PLL作为时钟源,以获得更高精度的时钟;
6 I7 B; R) C* A1 ?4 T$ W4 o6)使能角速度传感器(陀螺仪)和加速度传感器。由电源管理寄存器2(0X6C)控制。8 [( X7 g, R6 v* D
初始化完成后,即可读取陀螺仪、加速度传感器和温度传感器的数据了!!
6 }3 i0 @  Z3 Q1 [+ r' M" t
3 ~' @3 n9 V3 f. \, G五、主要的寄存器介绍
4 {+ u; m0 K6 x: w( ?9 j8 u4 s  h5.1)电源管理寄存器1(0X6B)
0 n$ u8 b+ L, T  g( A
! F) _. r( X: Y: x; } 20201202124650455.png
1 H" w, ~  E! _/ j/ Q" G6 E2 D, L- ?# g7 p, H3 p' g5 U5 B
DEVICE_RESE=1,复位MPU6050,复位完成后,自动清零;
) |/ f" K1 d2 ZSLEEP=1,进入睡眠模式;SLEEP=0,正常工作模式;
: b% }! G! D  M  X+ H, _, Q; k+ XTEMP_DIS,用于设置是否使能温度传感器,设置为0,则使能;
8 L: n2 t1 I) |$ F7 a% ?CLKSEL[2:0],用于选择系统时钟源,如下表所示:
# k9 C& |( i% E0 x( P' [# z  s0 _. O( N6 e& y, U; x
20201202124734929.png
) V8 h. Z9 \+ t6 l/ w' s& ~% s# c7 U; J1 G8 S; x; Q$ g% Z" W
5.2)陀螺仪配置寄存器(0X1B): L; Z) [' J+ H
+ r9 b1 O  Z3 e# k
20201202124826207.png + K  J( e  S  \
/ e4 O2 |% _+ i9 `4 d$ K- g: W6 a8 M4 ?
主要关注FS_SEL[1:0]这两个位,用于设置陀螺仪的满量程范围:* S) `/ B4 O: H1 |# G* _4 G
0,±250°/S;1,±500°/S;2,±1000°/S;3,±2000°/S;# H% `* _$ t$ b5 h: _: s$ z
一般设置为3,即±2000°/S,因为陀螺仪的ADC为16位分辨率,所以灵敏度为:65536/4000=16.4LSB/(°/S)。
9 E' s, J+ y5 D
8 ^, U$ P) C3 v5 S% W6 G( ^5.3)加速度传感器配置寄存器(0X1C)% w- A* |+ L& X" j
  U4 }0 K, R! B: v, r# _
20201202124906190.png 4 ]/ J& u3 z, H% O# c- ^7 q  i) Z
; O' W7 @7 ]$ Q3 V, m+ t
主要关注AFS_SEL[1:0]这两个位,用于设置加速度传感器的满量程范围:
; N" e+ F' K2 Y0,±2g;1,±4g;2,±8g;3,±16g;
: |) W: M; Q) g0 W5 B! [5 `& |6 l一般设置为0,即±2g,因为加速度传感器的ADC是16位,所以灵敏度为:65536/4=16384LSB/g。
6 x) v/ ?8 R' p
* f+ D7 M6 f! e5.4)FIFO使能寄存器(0X23)8 `" a' F2 ~9 J. }  Y' Y* H/ p

# f. c7 K' e8 V, g+ e3 M# g4 x 20201202124951209.png
& L0 p8 [; J1 r8 |% F5 I  ]; u
* I9 {7 A/ \& m该寄存器用于控制FIFO使能,在简单读取传感器数据的时候,可以不用FIFO,设置对应位为:0,即可禁止FIFO,设置为1,则使能FIFO。
5 U$ F( \) t' N# t! E: }# s注意:加速度传感器的3个轴,全由1个位(ACCEL_FIFO_EN)控制,只要该位置1,则加速度传感器的三个通道都开启FIFO了。, U# B4 r- o, C% R, ]

6 N# h+ t& H6 y" p0 J- u* x5.5)陀螺仪采样率分频寄存器(0X19)
% Q# R( J& d: ~# n: _+ t* F7 J: x. e! e  Z" y
2020120212502194.png
$ Z; L6 }8 ^# v$ X2 ^+ d/ S- L% c1 {, O5 p' U  q
该寄存器用于设置MPU6050的陀螺仪采样频率,计算公式为:
- z2 k& ?6 J) D采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)
1 o- @% I* X  A4 O0 J% h( F) ~这里陀螺仪的输出频率,是1Khz或者8Khz,与数字低通滤波器(DLPF)的设置有关,当DLPF_CFG=0或7的时候,频率为8Khz,其他情况是1Khz。而且DLPF滤波频率一般设置为采样率的一半。采样率,我们假定设置为50Hz,那么:SMPLRT_DIV=1000/50-1=19。
, J0 O7 }% p% G( t4 `7 r
* T* j( l9 E3 {8 v0 i3 e5.6)配置寄存器(0X1A)! r4 m" X* G  w  b0 c% Z8 v

2 m! t$ k5 t! V5 O( d 20201202125111514.png
# \/ M  I  [& V& I( j$ u3 w
8 s) T$ ~! B2 U. s重点看数字低通滤波器(DLPF)的设置位:DLPF_CFG[2:0],加速度计和陀螺仪,都是根据这三个位的配置进行过滤的,如下表所示:; H( _. N& K5 K1 Z" K

- K  M! X0 H" x* r( m* {! J 20201202125128497.png ( |9 t! P" {$ @' l% A' b6 r. b' k

3 ]' o# Z& |! X/ D! @备注:带宽=1/2采样率' {& O/ O6 B8 R$ m4 [/ `& q9 @

1 M6 B2 b6 X! ]8 F5.7)电源管理寄存器2(0X6C)9 G4 x, @% {. \3 x3 \% [* ?+ o
8 o+ P; l- E2 U3 W
20201202125210534.png * R3 N$ b0 ?9 ]2 O* \8 x$ S
5 e" q0 C8 M6 g3 c- l8 N% x2 P
该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率;& Q2 n1 y# v7 q
剩下的6位,分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式;
. O3 q0 E6 Q' h4 f本例程全部设置为:0 ,即可。
9 C6 o( s/ o9 [; C
( E6 A6 R# H3 y! ~; Q5.8)加速度传感器数据输出寄存器(0X3B~0X40)
/ }* {) E9 F4 x- T0 H2 L# s
3 o0 C. I' Z) r6 l. k* D; O 20201202125232456.png   Q* G' j* q4 c; a7 g+ q- N
9 Y9 Y- j5 w* x# h. v; a& Q* e+ {
总共由6个寄存器组成,输出X/Y/Z三个轴的加速度传感器值,高字节在前,低字节在后。$ H+ H0 s8 K2 s6 _; M; I) N  p; O

9 ]$ {7 p% Q+ m6 J5.9)陀螺仪数据输出寄存器(0X43~0X48)6 G0 T1 ]- x( y6 S% s$ u- _6 ?# V

2 c9 X4 Z* c( j" w% H 20201202125315716.png
9 @4 a* m% t3 A: h
  l+ j' _( s. z1 Z4 C+ `/ p, [* s总共由6个寄存器组成,输出X/Y/Z三个轴的陀螺仪传感器数据,高字节在前,低字节在后。- f/ v( h' Z7 B
* G4 J1 K. a8 J# E  o
5.10)温度传感器数据输出寄存器(0X41~0X42), L7 F/ Q& k( D+ C3 j5 s

) T( X! W1 E- w5 v* }  v1 K9 d! C 20201202125336901.png 2 |, d: u* _9 z

. J5 F6 }- V) p通过读取0X41(高8位)和0X42(低8位)寄存器得到,温度换算公式为:
6 E4 u, r1 z$ E) i8 ^* s( w( P% ?. cTemperature = 36.53 +regval/340
  |* [4 H/ D0 G; b! l- d1 ?其中,Temperature为计算得到的温度值,单位为℃,regval为从0X41和0X42读到的温度传感器值。' ]3 C3 {( ~$ G9 D* L* Q6 t4 v
% d+ t+ b. G  c" h: ^* e2 [
六、DMP使用介绍
+ R, B; @+ M9 S! g- Q. t" a! E实际使用时(比如做四轴),我们更希望得到姿态数据,即欧拉角:航向角(yaw)、横滚角(roll)和俯仰角(pitch)。
9 b% `* Y# ?: |要利用原始数据,需要进行姿态融合解算。MPU6050自带数字运动处理器,即DMP,并且InvenSense提供了一个MPU6050的嵌入式运动驱动库,结合MPU6050的DMP,可以基于原始数据直接转换成四元数输出,而得到四元数之后,便可很方便的计算出欧拉角,从而得到yaw、roll和pitch。' m  y4 s: c" z2 K8 X3 U
备注:InvenSense提供的MPU6050运动驱动库是基于MSP430的,需要将其移植后才可以用到STM32上面。+ U# V! Z. h) }2 _6 ]+ H$ \, {; D

$ H; t9 G4 r0 j, D移植细节:
8 C; @% Q5 a) h官方DMP驱动库移植,主要是实现这4个函数:i2c_write、i2c_read、delay_ms和get_ms。3 B* F' g& ]: l, D
移植后的驱动代码如下图:8 |# W1 b& I! z  S

$ h0 W' ?' j! \0 O; ` 20201202125526501.png $ z' R2 C8 l0 D5 t/ V
( K" T5 l- |5 T& U& O+ l4 I
MPU6050 DMP输出的是姿态解算后的四元数,采用q30格式,即2的30次方,要得到欧拉角,需要做一个转换,代码如下:
% `) W: D# H* Z  ~, h
2 r' y4 D7 x! h# y4 j% p
  1. q0=quat[0] / q30;   // q30格式转换为浮点数% ~  [5 N$ X1 u8 k7 l/ E' Q9 C
  2. q1=quat[1] / q30;   // q30格式转换为浮点数
    ' y- \. E3 Q# d% z* ?
  3. q2=quat[2] / q30;   // q30格式转换为浮点数
    * F( c; j2 Y, \
  4. q3=quat[3] / q30;   // q30格式转换为浮点数
    . N! N% h3 L0 O; x+ y" u0 A# q
  5. // 计算得到俯仰角/横滚角/航向角2 {) n$ |+ U& A# `/ o
  6. pitch=asin(-2 * q1 * q3 +2 * q0* q2)* 57.3;      // 俯仰角
    ' ]+ m: B& Y8 c$ g9 I% g3 T
  7. roll=atan2(2 * q2 * q3 +2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // 横滚角yaw=atan2(2*(q1*q2 +- y3 v3 t/ W. Z$ C3 }. g3 K
  8. q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;          // 航向角
复制代码
  L4 V. {. }- p0 a
上述代码中:quat[0]~quat[3]:是MPU6050的DMP解算后的四元数,q30格式; q30:是一个常量:1073741824,即2的30次方; 57.3:是弧度转换为角度,即180/π,这样结果就是以度(°)为单位的。
/ Y+ y9 C+ m( r! g, j# X* f1 ?2 K* e  Z6 b+ v( _* Q
七、ATK-MPU6050模块
3 D# z9 V) U4 j  ~# H; @2 A6 W, c7 bATK-MPU6050模块主要由MPU-6050芯片和RT9193-33芯片组成,原理图如下图所示:
. d& K7 D7 Z6 {( n& [' Q9 f5 y" Y  r; b* p  n3 z
20201202125803667.jpg 1 u0 J" i, I! }

5 t; P3 p0 J6 k% n2 I! G3 ~# p7 x* o开发板通过ATK-MODULE接口外接MPU6050模块,硬件连接原理图如下:
( x" _- x! g; P) c0 P
6 D8 U& K  A5 k2 P 20201202125834118.jpg
6 X  I, h4 s/ M
! }2 y: x0 L) f+ O4 a4 \8 [9 ?八、重点关注的相关代码4 N7 M, \0 V* \8 b
8.1)MPU-6050驱动代码
, n& M$ A2 D; ?: g. T1,MPU6050 IIC接口驱动代码
6 x; Y# p$ D" w# b2,MPU_Init函数0 q4 h  ?" ~0 ~# z# i2 d" g
3,MPU_Get_Gyroscope函数+ h5 h8 I$ \7 D* }0 h
4,MPU_Get_Accelerometer函数4 T( G6 P8 ^# q' B+ Y4 ?# k
5,MPU_Get_Temperature函数
  s: F5 |' S1 ^5 m' W) V! n+ y  ]8.2)DMP驱动代码
# A& \9 H/ @' z. x) V* E' E1,DMP移植相关代码2 S7 `( a' C9 z3 a
i2c_write、i2c_read、delay_ms和get_ms.! y" ~1 q/ `: ?' d6 k
2, mpu_dmp_init函数. V* a- j/ N5 \
3, mpu_dmp_get_data函数
, W; |* D& u7 x" k6 E- c& S% @/ {8 A$ j+ K9 ^) c
九、MPU6050读写时序9 Q% z$ j& G( ?2 b
9.1)单字节写入时序
* T6 t! R4 a6 q! }( ^! R( Z, U; C
20201202130030646.png 6 d7 L3 r# \5 ^8 }, @- d' {& q
4 o6 E5 n. C# d8 ~
9.2)连续写入时序0 |2 b6 ~3 [+ a( o- o1 X0 V1 n

; I" I3 J6 j- S' u+ i2 ?0 R+ l9 u# M 2020120213005560.png 0 l6 r* B& Y$ B8 S2 i2 V+ ~" U

6 j9 ~# x. b; F. m9.3)单字节读出时序7 ~: T' Y0 @8 L) k5 g
" C7 o! u3 [0 d8 Q2 O* Z; t
20201202130110852.png - H! L" N- B8 G8 V- x, J

: J) k5 x3 d0 N/ X) t4 n9.4)连续读出时序
; \% I& r2 q5 }( U% ~; e' y/ V: R* J- ^: a# f" Y. q, ]
20201202130123583.png ; O9 S6 U7 _: |5 h0 z9 Y# B
! c+ \. c( i- b4 b$ d- x6 ]
十、部分实验代码解读  Y2 Y8 F) a. c: P6 t
10.1 mpuiic.h头文件函数9 ~4 L- ]  S0 h) C  `

, j& M$ v  z0 m3 T( j7 P" i
  1. /**$ G1 \/ D$ c& ^# l
  2. ********************************  STM32F10x  *********************************
    ; \5 v- v. E/ L9 w) f  c
  3. * @文件名称: mpuiic.h
    ; d# q: |) q6 M( l: U# T) M, I4 `1 n
  4. * @修改作者: Aaron! G4 _1 X. p" L+ f
  5. * @库版本号: V3.5.0
    $ g, t& G1 D# K" T1 b7 A9 k& p* W) l
  6. * @工程版本: V1.0.0
    . w3 r, I2 B+ l; {
  7. * @开发日期: 2020年11月30日
    ! p( _) r$ F, u: `' [) n( E
  8. * @摘要简述: mpuiic头文件,主要跟MPU-6050模块进行IIC通讯时用
    ; u' t) ]6 _( M; ~' I4 I- X' I5 t
  9. ******************************************************************************/& e$ i1 D0 L; e) d% M- n
  10. /*-----------------------------------------------------------------------------
    % e# l+ h* {9 s5 G
  11. * @更新日志:) G. O/ i# \0 E: ?4 w
  12. * @无
      w! Z& @  c" F$ e5 X
  13. * ---------------------------------------------------------------------------*/
    ' p* J  R) \, R/ b
  14. #ifndef __MPUIIC_H: G- V$ J' W" v+ [* D' c
  15. #define __MPUIIC_H5 ^+ a5 e4 }1 T" v6 [/ p4 K
  16. /* 包含的头文件 --------------------------------------------------------------*/* B! A+ R1 |' g8 r$ t7 T
  17. #include "sys.h"
    ) ^6 j" q8 h- R4 k- Z5 C8 L5 G
  18. #include "stm32f10x.h"
    / y% M/ @9 a! V/ R& U
  19. /* 寄存器方法控制GPIO管脚输入输出模式 ----------------------------------------*/4 ?- ~; m# [. h- E& Z
  20. #define MPU_SDA_IN()  {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}   // PB11 上拉输入模式
    7 ~0 d- `) M( p2 x
  21. #define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;}   // PB11 通用推挽输出模式,50MHz4 [0 p; P6 V/ |3 `" ~  M: W: j
  22. /* 位操作方法定义 ------------------------------------------------------------*/
    ; |' h" Q8 ^; E- M
  23. #define MPU_IIC_SCL    PBout(10)   // PB10对应SCL6 a% T4 g8 ^, V6 a  E' _
  24. #define MPU_IIC_SDA    PBout(11)   // PB11对应SDA,输出SDA  8 A7 u: E6 P# P: F' C' E' g" Q
  25. #define MPU_READ_SDA   PBin(11)    // PB11对应SDA,输入SDA
    ! c% B; b8 p2 P4 z, s
  26. /* 定义IIC底层驱动程序 -------------------------------------------------------*/
    2 m; z9 M. C0 F6 ]" [
  27. void MPU_IIC_Delay(void);                         // MPU IIC延时函数,延时 2 us
    ! q. T4 _# g! C) s4 }) d1 v5 r
  28. void MPU_IIC_Init(void);                   // 初始化IIC的IO口
    , j3 U* \, H" A3 W$ V, x! I# g! {- A
  29. void MPU_IIC_Start(void);                  // 发送IIC开始信号6 k! e4 \3 D2 I, A
  30. void MPU_IIC_Stop(void);                              // 发送IIC停止信号
    ' j$ x4 Y3 J/ p; g3 w$ @( ?% }
  31. u8 MPU_IIC_Wait_Ack(void);                 // IIC等待ACK信号1 [) `# t4 h1 P- z
  32. void MPU_IIC_Ack(void);                    // IIC发送ACK信号   # l  a; s' x. E) X6 q
  33. void MPU_IIC_NAck(void);                   // IIC不发送ACK信号
    % Q7 q( _# K) q; c
  34. void MPU_IIC_Send_Byte(u8 txd);            // IIC发送一个字节
    5 {3 ^1 L+ i) D8 A- ?* x
  35. u8 MPU_IIC_Read_Byte(unsigned char ack);   // IIC读取一个字节8 `* n5 ^% K6 g6 W6 ]' l2 Q4 e
  36. #endif /* __MPUIIC_H */$ x/ c9 ^* @8 a( a! I6 z9 v
  37. /****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
复制代码

; V: h1 j4 D. f! q' J10.2 mpuiic.c源文件函数' j9 \5 H% A5 l' e, W) A
  b, N3 x* R4 p3 Y
  1. /**! x1 `: c8 n- K% t3 \5 O6 Z7 c
  2. ********************************  STM32F10x  *********************************
    / H4 q4 R0 `. B) V6 u8 q
  3. * @文件名称: mpuiic.c! M+ x# K- j' T" r  A0 ^" ^
  4. * @修改作者: Aaron
      u  |% q. |, k$ f, A( d
  5. * @库版本号: V3.5.0
    1 Q% Q- V7 b6 J
  6. * @工程版本: V1.0.06 f8 F4 [: B& o! y+ q8 R4 n% D
  7. * @开发日期: 2020年11月30日
    $ ]" _: R3 w4 X7 p& q. Z/ t
  8. * @摘要简述: mpuiic源文件,主要包含与MPU-6050进行IIC通讯的IIC底层驱动程序6 }4 |" E" L9 m6 i0 o
  9. ******************************************************************************/' p  P& X8 _$ B  C# r
  10. /*-----------------------------------------------------------------------------
    ' f( L+ z, M$ @' t% i- t$ N
  11. * @更新日志:; h8 _/ v  }2 A
  12. * @无
    3 W1 r5 E8 r; m3 N) H- w
  13. * ---------------------------------------------------------------------------*/
    5 f# j# t& p5 o" y" i3 e. m! h
  14. /* 包含的头文件 --------------------------------------------------------------*/7 Y2 P5 h* H# Y. d9 R- B3 ^) }
  15. #include "mpuiic.h"  @0 |9 p2 r3 K, m) {6 f. c- C
  16. #include "delay.h") n; c( ~  v: H
  17. /*********************************************************************
    5 o5 u$ m0 \1 O( _
  18. 函数名称:MPU_IIC_Delay(). k1 Q8 V; U& u
  19. 函数功能:延时函数,延时2微秒(us)" w9 S  T) k* e# A/ P
  20. 入口参数:无1 F% T' v) E( `% E; Z% l9 L5 J
  21. 返回参数:无1 I$ Y8 r& E- n; E: Y
  22. 修改开发:Aaron, j( Q& d/ I6 g8 H) l
  23. **********************************************************************/; p1 f$ T9 i) \5 D
  24. void MPU_IIC_Delay(void)
    5 Q) Y; S2 M# T& i4 d, p! F
  25. {
    . H4 B& W" ~4 i# I" I) }
  26. delay_us(2);& `9 Z* _" N: c8 V/ {
  27. }
    6 c( [, P1 b8 \6 m9 ?) N
  28. /*********************************************************************
    4 ^0 k/ z* o; k# G8 m
  29. 函数名称:MPU_IIC_Init(); v" h2 h( ~% b4 H; @, r
  30. 函数功能:MPU_IIC管脚初始化配置( D, `. c4 K7 t, [' C( v, a/ B
  31. 入口参数:无/ O; q/ a9 w& `' K* n
  32. 返回参数:无
    ) A$ x. X% p; w8 V  q9 g
  33. 修改作者:Aaron' D+ l1 h4 }/ \  b+ |" I
  34. **********************************************************************/+ F9 u' ^% M1 M2 V
  35. void MPU_IIC_Init(void)
    * e6 F7 z- z& r1 d7 z* g
  36. {          $ W# f" M1 d5 L
  37.   GPIO_InitTypeDef  GPIO_InitStructure;
    1 t' `8 @: I+ S: a. h
  38.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);     // 使能外设IO PORTB时钟   
    % g8 ?* O9 t! f( w) P
  39.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;   // 端口配置,管脚10和管脚116 ^# R0 v; X( [( r- n% z8 [  F# J
  40.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;         // 推挽输出
    : @, k* @/ [0 q. X$ f( i; w5 l
  41.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;        // IO口速度为50MHz
      g8 R% y1 `/ W
  42.   GPIO_Init(GPIOB, &GPIO_InitStructure);            % d" ~8 ~) ^3 J+ j
  43.   GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11);             // PB10(SCL),PB11(SDA) 输出高 8 ~2 n8 G# Y7 f
  44. }5 m) m# B3 p. G# w6 Q/ ^/ u( P3 N
  45. /*********************************************************************8 I3 U# @! p! k' Q5 r6 d- _
  46. 函数名称:MPU_IIC_Start()
    : t* W* s& t6 |1 H% W  A& ~/ m
  47. 函数功能:MPU_IIC起始信号1 d. O- i0 @% L0 Q/ A* G
  48. 入口参数:无
    # T+ D4 N- T# ]0 f# `9 O
  49. 返回参数:无
    # Y* n$ r: _( v. M  @
  50. 修改作者:Aaron& f1 A: \' \- S
  51. **********************************************************************/
    2 W7 h3 y% i8 C% d
  52. void MPU_IIC_Start(void)& W- e8 m+ V' y4 c3 C( b# b8 F8 k
  53. {/ [8 K$ u" d3 Y9 q* Z, x0 g
  54. MPU_SDA_OUT();      // SDA线输出模式; n" ?6 _1 r( }
  55. MPU_IIC_SDA=1;      // SDA输出高
    , U1 o; L  v9 X
  56. MPU_IIC_SCL=1;      // SCL输出高,此时为空闲状态
    5 V1 B7 I7 @# C
  57. MPU_IIC_Delay();    // 延迟 2 us
    3 W# t4 r* t/ x; l& b9 C0 a1 j, }5 }
  58. MPU_IIC_SDA=0;      // 在SCL输出高的状态下,SDA由高至低跳变,IIC开始& l. t4 u3 |; B
  59. MPU_IIC_Delay();    // 延迟 2 us
    $ U  |# O3 h* }, a& U# }# [0 M
  60. MPU_IIC_SCL=0;      // 钳住I2C总线,准备发送或接收数据
    . p- S" Q0 V* t/ w5 S% z
  61. }  
    4 M: J/ p, E; F$ m
  62. /*********************************************************************# i( _: Q  L$ w$ G- ]6 E( l% H
  63. 函数名称:MPU_IIC_Stop()
    8 f6 N' |" t- G3 T/ F5 p
  64. 函数功能:MPU_IIC停止信号
    % c! p: E- F( e8 {) A. x% k
  65. 入口参数:无
    / y0 C6 ?7 L6 D3 X
  66. 返回参数:无
    1 ^" \9 ^" l7 l2 C
  67. 修改作者:Aaron; S( m$ }$ o) P' A
  68. **********************************************************************/2 S( `( F* h' m# c! L
  69. void MPU_IIC_Stop(void)2 I+ ]4 a& H0 i8 E# h4 f" |* Z
  70. {9 S2 z- N. e, d- Q6 i* M, z# f' A
  71. MPU_SDA_OUT();      // SDA线输出模式: k) g- R3 A8 x3 n7 J  W
  72. MPU_IIC_SCL=0;      // SCL输出低
    7 P* Y8 \8 T' E' F- u: t
  73. MPU_IIC_SDA=0;      // SDA输出低
    # X5 k" Y$ ^; T/ `. F( v' v9 a( ]
  74. MPU_IIC_Delay();    // 延迟 2 us' n" H1 @7 Z# C
  75. MPU_IIC_SCL=1;      // 先SCL输出高,在SCL输出高的状态下,SDA一直是低电平
    $ o' I$ {- b, \' J8 s! H* R0 H9 x
  76. MPU_IIC_SDA=1;      // SDA输出高,发送I2C总线结束信号
    1 M% t8 O5 f# P& }
  77. MPU_IIC_Delay();    // 延迟 2 us        . ~5 a6 E) B8 a5 K3 ?& h5 S! I( k
  78. }
    9 _8 l& e. I3 \4 j: h% V' F5 ^
  79. /*********************************************************************
    & r* z* o# H. \* @
  80. 函数名称:u8 MPU_IIC_Wait_Ack()+ \  ]9 @9 \. S% i1 E% a
  81. 函数功能:等待应答信号到来1 x3 J9 Z' n7 q0 D; D, q9 b7 _" o
  82. 入口参数:无5 W) @- T$ V# f4 z# v9 L4 d8 x
  83. 返回参数:u8,1,接收应答失败,0,接收应答成功8 E( U/ H+ |. @
  84. 修改作者:Aaron
    ) {% u3 k, K4 F0 o" U3 K
  85. **********************************************************************/
    5 W; M3 s2 K  d7 P( z
  86. u8 MPU_IIC_Wait_Ack(void)
    ; A: K, K1 m# T5 u% @
  87. {" H0 E9 s& w" J" ~
  88. u8 ucErrTime=0;4 B* N% g2 W0 i! n
  89. MPU_SDA_IN();         // SDA设置为输入  0 o2 ~" x1 S( _# E* P" P( N2 L
  90. MPU_IIC_SDA=1;        // SDA设高电平# n7 T; p  F! t4 w# A% `
  91. MPU_IIC_Delay();      // 延迟 2 us
      @* |$ h6 w# l! i. M
  92. MPU_IIC_SCL=1;        // SCL设高电平
    * E1 E# {1 r, D+ d
  93. MPU_IIC_Delay();      // 延迟 2 us
    ) `; i: ~* W* w
  94. while(MPU_READ_SDA)   // 读取PB11的值,一直读PB11=1,高电平,则不断循环,直到溢出
    ; e. M# z  V' u5 e
  95. {. M3 k; N% n4 S% X; r; v
  96.   ucErrTime++;
    # `$ _1 P0 K0 W2 g
  97.   if(ucErrTime>250)  P- [  C: e3 Z* X" e* Y
  98.   {& g0 }' R7 o' f3 p
  99.    MPU_IIC_Stop();
      q6 I% d; C9 [- J7 p2 n, M
  100.    return 1;# j2 L7 k% S  |) s8 ~' r+ M% h
  101.   }
    7 l! m( h: T* ^; t
  102. }
    ! F! I# n, Z) q
  103. MPU_IIC_SCL=0;        // SCL设低电平     8 h, _6 V6 v: `" I- Q" Q, c
  104. return 0;  5 u5 d* q. H9 J% F( Z- j) m0 |
  105. }
    ' v. h" f& \0 a  ^* [$ ^
  106. /*********************************************************************8 I7 p/ P5 G4 X9 |  h
  107. 函数名称:MPU_IIC_Ack()
    % p/ {8 b) f! w) c% S$ U
  108. 函数功能:产生ACK应答% F4 Y& {0 I0 v& Z7 p0 ]
  109. 入口参数:无/ P8 J1 l4 t* L3 [& m3 O/ R
  110. 返回参数:无
    3 v) K& c1 N  ~# a# S: ^! u( s6 d6 A
  111. 修改作者:Aaron) a3 M' t; a% h0 _- `4 z
  112. **********************************************************************/' i+ x8 a! O. M$ V0 u
  113. void MPU_IIC_Ack(void)! X- V& u0 X" G( [" ^, V2 q
  114. {
    8 ?  l9 `6 x* W$ c8 u! C
  115. MPU_IIC_SCL=0;       // SCL设低电平4 p  h- d1 F; F+ U4 V
  116. MPU_SDA_OUT();       // SDA输出模式% |& a% @+ W/ W) u3 o! H: V
  117. MPU_IIC_SDA=0;       // SDA设低电平- r" W% B' g9 U3 d3 C0 P" I
  118. MPU_IIC_Delay();     // 延时 2 us
    8 n; v9 t9 j4 i, x
  119. MPU_IIC_SCL=1;       // SCL设高电平" w8 s, D# L4 \) L
  120. MPU_IIC_Delay();     // 延时 2 us* Z6 _6 `$ J) h0 [
  121. MPU_IIC_SCL=0;       // SCL设第电平,在SCL高电平期间,SDA为低电平,则说明有效应答
    # w9 c+ j; M9 U3 a* t
  122. }
    * O* a: {, Z! c: y% Y9 U, Z
  123. /*********************************************************************# a# B+ k, X& t0 k8 B# |+ i
  124. 函数名称:MPU_IIC_NAck()
    ; _% V" x7 \# H2 g
  125. 函数功能:不产生ACK应答
    6 W# [( n5 V7 `! `: U$ `
  126. 入口参数:无
    * N( v% U) M1 U/ b  r) C6 k% Q: n
  127. 返回参数:无8 j, F7 i3 p5 L# b9 S. X
  128. 修改作者:Aaron
    " c& P! Z: y" Z" P7 Y
  129. **********************************************************************/$ |+ ^# C  Y4 V- d2 O* C
  130. void MPU_IIC_NAck(void)) _' _, ~" i2 d0 L, g. X
  131. {
    6 X0 [& ^( L5 c2 ~
  132. MPU_IIC_SCL=0;       // SCL设低电平0 k$ L7 u! E: ?& J" B" v! ?
  133. MPU_SDA_OUT();       // SDA输出模式
    3 a# D  z7 x# S; c( s
  134. MPU_IIC_SDA=1;       // SDA设高电平0 D6 B  R6 t, A5 m0 l
  135. MPU_IIC_Delay();     // 延时 2 us3 X* k& h' T6 i$ I( b
  136. MPU_IIC_SCL=1;       // SCL设高电平
    * \# s" q6 Y" |( c$ g, Z
  137. MPU_IIC_Delay();     // 延时 2 us
      L+ u  p7 M! d1 L1 E
  138. MPU_IIC_SCL=0;       // SCL设低电平,在SCL高电平期间,SDA为高电平,则说明无效应答% ]  q# H6 Z) E9 D4 j+ I! J$ z
  139. }
    9 H+ f* b" c* [  ^- D4 p
  140. /*********************************************************************
    : L4 ]$ y; `/ C: s0 P
  141. 函数名称:MPU_IIC_Send_Byte(u8 txd)
    ( |+ m9 g: \& T3 T+ t; l
  142. 函数功能:IIC发送一个字节
    ! o* x/ E' ~7 I, O6 V. a" z, y
  143. 入口参数:u8 txd
    8 I; t2 d! n- M  q5 p+ p6 L
  144. 返回参数:无
    $ R* N5 D4 U2 o3 T/ ^: Y! X  h4 i
  145. 修改作者:Aaron$ y) p6 D4 e& I5 q% ~; Y. P
  146. **********************************************************************/
    . Y' r) P: X6 _+ D1 L& F; |
  147. void MPU_IIC_Send_Byte(u8 txd)
    ! z( r" h7 O- q" K& }! x( F
  148. {                        
    - L# I+ U4 \3 H  e
  149. u8 t;   4 J# G  R' J8 e) s2 W
  150. MPU_SDA_OUT();       // SDA输出模式
    ' @. \  ~3 x: i, f
  151. MPU_IIC_SCL=0;       // SCL设低电平,此时可改变SDA,以实现数据的有效传输
    # [5 q8 P5 U" b4 L. ~
  152. for(t=0;t<8;t++)     // 发送一个字节,即8个bit
    4 B" M* I$ }% O, ~7 {/ a  _
  153. {              
    & d- J7 Z/ M' d5 V8 |; _
  154.   MPU_IIC_SDA=(txd&0x80)>>7;  // 获取u8 txd的最高位(0或1),并右移7位,将最高位值给MPU_IIC_SDA: y$ S9 l( ]) j- I4 T7 l( e
  155.   txd<<=1;   
    ! G1 [3 b7 F* T9 l8 s& J& E( W
  156.   MPU_IIC_SCL=1;              // SCL设高电平
      D- E0 G: z' Z8 C0 b
  157.   MPU_IIC_Delay();            // 延时 2 us% i; O5 L8 x; F2 Q
  158.   MPU_IIC_SCL=0;              // SCL设低电平,传输SDA完成* |0 w4 v+ R8 t' I: o; D
  159.   MPU_IIC_Delay();            // 延时 2us
    ( e  M- B" Y) E- K. m
  160. }  
    6 f) @9 [( W# {3 E( r
  161. } ; @' x* q, q$ z/ C" l8 i) W5 o9 x
  162. /*********************************************************************" ^* m4 I- A; [  U+ ?& J: V8 i
  163. 函数名称:u8 MPU_IIC_Read_Byte(unsigned char ack)
    & o& ?9 Z+ f! F$ b6 O1 j0 v$ M
  164. 函数功能:IIC读取一个字节,ack=1时,发送ACK,ack=0,发送nACK : k# A8 K! Z% l  w
  165. 入口参数:ack& `9 W3 E5 j' V9 [8 p9 t6 I* Z
  166. 返回参数:u8," e% I8 N1 F: T- F1 m: c
  167. 修改作者:Aaron$ N4 t* X  b0 ^! f- h
  168. **********************************************************************/
    + |& L/ t! ]% @! H4 f
  169. u8 MPU_IIC_Read_Byte(unsigned char ack)
    # k1 d2 l/ X6 C, K. R
  170. {
      `, H/ J+ S8 O) v
  171. unsigned char i,receive=0;* F$ h/ ^( |+ Z9 }# i
  172. MPU_SDA_IN();                 // SDA设置为输入模式% `% R! _3 m3 l3 o4 b# ~
  173. for(i=0;i<8;i++ )             // 读取一个字节,即8个bit" ~" u: y. A! _  \0 p$ j
  174. {
    . {2 e' z) W/ |3 b% p2 p: _% E5 F
  175.   MPU_IIC_SCL=0;               // SCL设为低电平7 a# p6 ^4 ?1 Y, ]: k% x
  176.   MPU_IIC_Delay();             // 延时 2us
    5 s* R( s0 _% S. S2 I
  177.   MPU_IIC_SCL=1;               // SCL设为高电平* _1 s0 x! F# Y) E& b
  178.   receive<<=1;
      H( k4 T8 R3 v7 Q% ~# E
  179.   if(MPU_READ_SDA)             // 如果读取的SDA为1,则receive最低值+1,若读取的SDA为0,则receive最低值不变,即0
    6 R, D; Q& M" @1 \, ~2 Z
  180.    receive++;   % m$ o/ v& W( S0 V9 I1 q
  181.   MPU_IIC_Delay();             // 延时 2us# O7 B( q" x7 [8 m) X) B# K
  182. }      2 Z) h* t- F9 T* b8 B* F
  183. if (!ack)
    4 j" S1 W3 ^% O  c' a: S
  184.   MPU_IIC_NAck();              // 发送nACK
    % e1 v0 E1 E; Z! |
  185. else% F3 k5 a# P3 i
  186.   MPU_IIC_Ack();               // 发送ACK   
    3 p! Z) X0 j6 l4 P3 m) B) g0 C( s% u
  187. return receive;
    8 G# }8 f/ t1 Q6 ~) l7 l! p
  188. }3 @; Y* r  W8 \& M4 U
  189. /****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
复制代码

1 Q* D: P' v0 \- q1 W# j  V5 q10.3 mp6050.h头文件函数: b( C3 A- Q% ^6 M0 J: l* q7 X% @
代码中标记“初学”的说明是前面寄存器介绍时讲到的。' [0 y) V8 ~* H9 d: {! C; y- ^
% d  j; f& i+ l! |1 V) E! ~
  1. /**5 `( G2 Q# D2 v5 x
  2. ********************************  STM32F10x  *********************************
    9 m/ \: i& c; m3 d9 `
  3. * @文件名称: mpu6050.h
    0 D7 \& Y3 j1 }
  4. * @修改作者: Aaron
    # P; D' {1 s% s$ f1 n
  5. * @库版本号: V3.5.0
    $ T% W; z% R3 A5 d8 i! K
  6. * @工程版本: V1.0.0
    . A7 |3 e3 o; t6 ^6 i, s
  7. * @开发日期: 2020年11月30日1 r4 Y! @* ?) V0 d$ D! a- f% K
  8. * @摘要简述: mpu6050头文件- o* O6 o" w0 `/ o9 |, W
  9. ******************************************************************************/: ^$ y0 g; V5 F4 ^
  10. /*-----------------------------------------------------------------------------
    0 |/ x3 q. P" X- ?+ P! n$ C
  11. * @更新日志:
    4 o" X2 W% K0 j! v1 I6 B
  12. * @无$ S% C2 H$ X* U' @; P* P
  13. * ---------------------------------------------------------------------------*/
    1 L8 J; d/ D5 |7 i" G. s2 p! q
  14. #ifndef __MPU6050_H
    ; V) l0 {% r# u6 B' s' j/ t
  15. #define __MPU6050_H
    ' g/ E2 c2 B: q' Z
  16. /* 包含的头文件 --------------------------------------------------------------*/
    - E3 \3 C6 h( I6 k  e
  17. #include "mpuiic.h"   
    $ Z+ H2 ], s: X4 y( A" @; W
  18. /* 位操作控制PA15,对应MPU6050 AD0的高低电平 ----------------------------------*/2 U1 H7 J# t3 D' }( v" ]
  19. #define MPU_AD0_CTRL   PAout(15) // 控制AD0电平,从而控制MPU-6050地址
    & s5 H3 Z2 r9 {& ~' ?1 r, N
  20. /* MPU6050 寄存器ID定义 ------------------------------------------------------*/
    ; {1 }% _8 z$ R; h9 ]1 b/ }
  21. // #define MPU_ACCEL_OFFS_REG 0X06 // accel_offs寄存器,可读取版本号,寄存器手册未提到* w2 V6 d4 e% j* l* x
  22. // #define MPU_PROD_ID_REG    0X0C // prod id寄存器,在寄存器手册未提到( c& t9 \2 d; v: @
  23. #define MPU_SELF_TESTX_REG    0X0D // 自检寄存器X ACC和G/ o# G! L/ Q( R; L/ U- [
  24. #define MPU_SELF_TESTY_REG    0X0E // 自检寄存器Y ACC和G4 B  A/ u3 u" \
  25. #define MPU_SELF_TESTZ_REG    0X0F // 自检寄存器Z ACC和G+ F8 M& A" Q* v% ^$ F  T
  26. #define MPU_SELF_TESTA_REG    0X10 // 自检寄存器X,Y,Z only ACC
    , t! q& w+ f0 i2 `/ C# D0 L
  27. #define MPU_SAMPLE_RATE_REG   0X19 // 陀螺仪采样频率分频器    **初学**6 Z6 W' W: w. H: O4 M
  28. #define MPU_CFG_REG           0X1A // 配置寄存器              **初学**
    8 m/ S- P; \: d2 B* r  N' a1 h
  29. #define MPU_GYRO_CFG_REG      0X1B // 陀螺仪配置寄存器        **初学**. ?+ ~3 m7 v" Z8 r0 v1 `1 b# [1 z' x
  30. #define MPU_ACCEL_CFG_REG     0X1C // 加速度计配置寄存器      **初学**& P: i. [/ x3 Q" ]4 g
  31. #define MPU_MOTION_DET_REG    0X1F // 运动检测阀值设置寄存器$ x' j) N; Q, ~. H! k& R7 k1 G3 H
  32. #define MPU_FIFO_EN_REG       0X23 // FIFO使能寄存器          **初学**( U) L* D5 N; @7 y. d
  33. #define MPU_I2CMST_CTRL_REG   0X24 // IIC主机控制寄存器% b- W) ~: W* Z9 T  V
  34. #define MPU_I2CSLV0_ADDR_REG  0X25 // IIC从机0器件地址寄存器
    % r+ ]6 O6 o/ \. Q8 k
  35. #define MPU_I2CSLV0_REG       0X26 // IIC从机0数据地址寄存器) \; g2 Z$ l* u& P, q! z
  36. #define MPU_I2CSLV0_CTRL_REG  0X27 // IIC从机0控制寄存器$ d; q/ T. ^+ \. R7 m
  37. #define MPU_I2CSLV1_ADDR_REG  0X28 // IIC从机1器件地址寄存器' x5 |; n$ Q) s2 t
  38. #define MPU_I2CSLV1_REG       0X29 // IIC从机1数据地址寄存器
    * t$ |  v& s+ Z! ?3 X; X/ h( k
  39. #define MPU_I2CSLV1_CTRL_REG  0X2A // IIC从机1控制寄存器
    / p* R( e/ N! I$ C+ `6 |# l5 f
  40. #define MPU_I2CSLV2_ADDR_REG  0X2B // IIC从机2器件地址寄存器
    / {; F4 }' q2 |* o" @
  41. #define MPU_I2CSLV2_REG       0X2C // IIC从机2数据地址寄存器1 K/ U# O, s3 f6 W' N. I
  42. #define MPU_I2CSLV2_CTRL_REG  0X2D // IIC从机2控制寄存器
    + N. e7 d2 [& C
  43. #define MPU_I2CSLV3_ADDR_REG  0X2E // IIC从机3器件地址寄存器
    , i- s. {( u6 R, y
  44. #define MPU_I2CSLV3_REG       0X2F // IIC从机3数据地址寄存器
    : `: t3 Q& x1 x$ P, X
  45. #define MPU_I2CSLV3_CTRL_REG  0X30 // IIC从机3控制寄存器
    - ^" z; V$ {$ C3 W! ^
  46. #define MPU_I2CSLV4_ADDR_REG  0X31 // IIC从机4器件地址寄存器& ^4 {; l; i% C1 ]  ]0 w
  47. #define MPU_I2CSLV4_REG       0X32 // IIC从机4数据地址寄存器
    & @5 N& `. N* l9 T1 _; E0 m4 u
  48. #define MPU_I2CSLV4_DO_REG    0X33 // IIC从机4写数据寄存器
    ) o: x7 C# l+ y  u% t- @7 [) o
  49. #define MPU_I2CSLV4_CTRL_REG  0X34 // IIC从机4控制寄存器
    & Y1 i# L5 ~" y. `
  50. #define MPU_I2CSLV4_DI_REG    0X35 // IIC从机4读数据寄存器! n; h  U. F# G  |, h
  51. #define MPU_I2CMST_STA_REG    0X36 // IIC主机状态寄存器
      Y" {8 u' W/ r- w& z+ l
  52. #define MPU_INTBP_CFG_REG     0X37 // 中断/旁路设置寄存器+ n- l& F0 s8 @& H6 \' D
  53. #define MPU_INT_EN_REG        0X38 // 中断使能寄存器! e. v" L( `5 n  l8 H9 P8 b
  54. #define MPU_INT_STA_REG       0X3A // 中断状态寄存器
    1 S1 p7 Y# H* K" W, {
  55. #define MPU_ACCEL_XOUTH_REG   0X3B // 加速度值,X轴高8位寄存器  **初学**2 \% ]* }6 G9 r0 h# R. a  A- f+ g
  56. #define MPU_ACCEL_XOUTL_REG   0X3C // 加速度值,X轴低8位寄存器  **初学**
    # ^1 o+ E6 z8 ?6 w3 _, D
  57. #define MPU_ACCEL_YOUTH_REG   0X3D // 加速度值,Y轴高8位寄存器  **初学**2 x* J8 g! {. \. Q+ H/ Y
  58. #define MPU_ACCEL_YOUTL_REG   0X3E // 加速度值,Y轴低8位寄存器  **初学**% j. x: s5 z- h9 D2 I; d% T, @3 t
  59. #define MPU_ACCEL_ZOUTH_REG   0X3F // 加速度值,Z轴高8位寄存器  **初学**+ |( d$ j  i; ?/ X6 N$ Y. e( ?
  60. #define MPU_ACCEL_ZOUTL_REG   0X40 // 加速度值,Z轴低8位寄存器  **初学**
    6 @! c# o. H, L( x1 n( f! `8 @
  61. #define MPU_TEMP_OUTH_REG     0X41 // 温度值高八位寄存器    **初学**
    0 s+ M% @! ?9 c
  62. #define MPU_TEMP_OUTL_REG     0X42 // 温度值低八位寄存器    **初学**! j; \7 V9 _6 t5 }0 c2 Q) ]
  63. #define MPU_GYRO_XOUTH_REG    0X43 // 陀螺仪值,X轴高8位寄存器 **初学**8 e. j8 p0 Z7 I. q# D) q* K% n9 o* u
  64. #define MPU_GYRO_XOUTL_REG    0X44 // 陀螺仪值,X轴低8位寄存器 **初学**
    * L9 \3 N  k* d) `1 \
  65. #define MPU_GYRO_YOUTH_REG    0X45 // 陀螺仪值,Y轴高8位寄存器 **初学**
    2 i+ V6 l2 [3 Q& {" q
  66. #define MPU_GYRO_YOUTL_REG    0X46 // 陀螺仪值,Y轴低8位寄存器 **初学**
    8 d6 b: X' R( c) ]
  67. #define MPU_GYRO_ZOUTH_REG    0X47 // 陀螺仪值,Z轴高8位寄存器 **初学**
    # K' E8 z* f: e& d
  68. #define MPU_GYRO_ZOUTL_REG    0X48 // 陀螺仪值,Z轴低8位寄存器 **初学**
    3 _* y% ^' v2 M* r% H* w+ E& N
  69. #define MPU_I2CSLV0_DO_REG    0X63 // IIC从机0数据寄存器
    6 X- T* f, d! j& T. O+ T7 T) W
  70. #define MPU_I2CSLV1_DO_REG    0X64 // IIC从机1数据寄存器" ?) n! l9 L+ m4 P1 E0 @- B1 M$ ?
  71. #define MPU_I2CSLV2_DO_REG    0X65 // IIC从机2数据寄存器6 c( {& g& Q1 M# ]6 i
  72. #define MPU_I2CSLV3_DO_REG    0X66 // IIC从机3数据寄存器
    ' ^- x( y3 j- L" b- e' F/ H; S
  73. #define MPU_I2CMST_DELAY_REG  0X67 // IIC主机延时管理寄存器% x: |, O, {! d' }: ?9 U
  74. #define MPU_SIGPATH_RST_REG   0X68 // 信号通道复位寄存器) S- a/ K+ Z6 g5 I
  75. #define MPU_MDETECT_CTRL_REG  0X69 // 运动检测控制寄存器, |% ^4 A) u3 r0 C) W
  76. #define MPU_USER_CTRL_REG     0X6A // 用户控制寄存器9 s! r" v2 r" P
  77. #define MPU_PWR_MGMT1_REG     0X6B // 电源管理寄存器1      **初学**! T6 S6 S8 Q* E# J$ y
  78. #define MPU_PWR_MGMT2_REG     0X6C // 电源管理寄存器2      **初学**
    ' d6 m4 I. y5 F
  79. #define MPU_FIFO_CNTH_REG     0X72 // FIFO计数寄存器高八位
    ' ?$ p. l: e) T' ~$ g
  80. #define MPU_FIFO_CNTL_REG     0X73 // FIFO计数寄存器低八位; V, m4 F- w5 E( o. f2 o
  81. #define MPU_FIFO_RW_REG       0X74 // FIFO读写寄存器
    " S. i0 C+ j/ z5 r) @* t
  82. #define MPU_DEVICE_ID_REG     0X75 // 器件ID寄存器
    , }7 o+ X, K; l' ], m8 @
  83. // 备注:如果AD0脚(9脚)接地,0,IIC地址为0X68(不包含最低位);& Z% `" r" m6 u* x. C
  84. // 如果接V3.3,则IIC地址为0X69(不包含最低位),
    0 B$ G! K' s3 d8 A' o) ?  Q) J& f
  85. // 本开发板由PA15管脚控制,另PA15为低电平
    4 t! Y& i3 n/ r% \( Y
  86. #define MPU_ADDR       0X68
    ; u3 T3 _+ G+ P; P  }: P9 p* _1 M4 L
  87. /* 函数申明 -----------------------------------------------------*/
    3 t. m! k# v( t9 y
  88. u8 MPU_Init(void);                                 // 初始化MPU6050
    9 }+ U1 Q, _# \1 R
  89. u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);   // IIC连续写
    * @- e) h2 w7 k) q3 W
  90. u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf);    // IIC连续读 4 z8 d2 w/ a( c, d0 k& A' q! i; S
  91. u8 MPU_Write_Byte(u8 reg,u8 data);                 // IIC写一个字节9 F1 E/ `0 M& f( ^+ Q1 V/ H
  92. u8 MPU_Read_Byte(u8 reg);                          // IIC读一个字节# \% q; u6 M( r# _2 U
  93. u8 MPU_Set_Gyro_Fsr(u8 fsr);7 C' l, I* V  E
  94. u8 MPU_Set_Accel_Fsr(u8 fsr);
    % @( E$ p& E* n1 ~+ T
  95. u8 MPU_Set_LPF(u16 lpf);3 m) y/ ?  j7 v# F3 c
  96. u8 MPU_Set_Rate(u16 rate);
    & U6 Y* P( g* n/ s
  97. u8 MPU_Set_Fifo(u8 sens);2 u9 A  \9 N; I1 Q: t3 S+ T
  98. short MPU_Get_Temperature(void);
    / j3 b  Z$ \  v' w
  99. u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);5 F  v' E7 U1 [  y9 m
  100. u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
    / X( G. E6 h: M' Q& L
  101. #endif /* __MPU6050_H */* v" d7 R2 h+ i* F2 J% ?
  102. /****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
复制代码
$ h/ Y3 F- L1 s/ S5 W8 Y
10.4 mpu6050.c源文件函数
  B) `# Z. I# {4 X9 G/ c( [0 a) v7 _- J
  1. /**
    " g" e* p) X3 g
  2. ********************************  STM32F10x  *********************************# s" a2 r) b1 Q* M; ?: c
  3. * @文件名称: mpu6050.c3 r: G9 c; K7 [" ~
  4. * @修改作者: Aaron
    ! L! D/ t% Y& [* z& m
  5. * @库版本号: V3.5.0
    5 u. K' z+ \! z' L
  6. * @工程版本: V1.0.0
    ! M- V9 o! I4 u) J2 U$ f3 X
  7. * @开发日期: 2020年11月30日3 Q' J& g5 t) u; ~2 G5 C
  8. * @摘要简述: mpu6050源文件* A# ^+ g* B$ \, h: {6 {5 M
  9. ******************************************************************************/
    ! ^6 q% t: g+ N  e; Z4 ]* L" X. `
  10. /*-----------------------------------------------------------------------------
      r8 ~! r$ A% D7 G) v3 d
  11. * @更新日志:
    / }' t0 J& x( O" Z
  12. * @无
    4 ~0 l+ f! v! j  `
  13. * ---------------------------------------------------------------------------*/# ?; K5 @# R" D2 G% t2 s! A
  14. /* 包含的头文件 --------------------------------------------------------------*/$ L9 r6 N. }0 P: n3 I! Y
  15. #include "mpu6050.h"
    ) {% b8 ?* {0 O/ u. f
  16. #include "delay.h"
    ; m- P- {8 |$ n% f3 t
  17. /*********************************************************************- e1 j/ N5 [5 J/ _2 L. f
  18. 函数名称:u8 MPU_Init()
    + i" B9 ^, H$ k$ N# R  P
  19. 函数功能:初始化MPU6050
    , P# |! t% c( {7 `( b* {
  20. 入口参数:无) l  }6 B+ x) b# X- D5 |
  21. 返回参数:0,成功,1失败
    ) @9 `; |  x7 U# W1 t$ R
  22. 修改作者:Aaron
    5 h3 K4 b/ T4 f) v$ m" G5 F
  23. **********************************************************************/
    ! T+ F1 C8 k& ^5 ?
  24. u8 MPU_Init(void)6 s0 s, E- T5 P; o9 J
  25. { ! v! Y9 R" ^7 e" m
  26. u8 res;
    " X1 D3 h4 d1 ^# ]
  27. GPIO_InitTypeDef  GPIO_InitStructure;
    : n1 f( P' |7 A; _2 |
  28. RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);      // 使能AFIO时钟 ( l+ n4 Z1 P' a" M1 t' Q9 i1 U
  29. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);     // 使能外设IO PORTA时钟  1 _! m& S% F* d3 i$ W
  30. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;               // 管脚15" x' M7 C+ e3 B$ j! P$ \: p# {
  31. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;         // 推挽输出
    ; [* F! ?' c# M  L5 i% ^4 L+ M* Q
  32. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;        // IO口速度为50MHz  x+ L9 ]2 T) h" j' u; |2 u/ v
  33. GPIO_Init(GPIOA, &GPIO_InitStructure);                   // 根据设定参数初始化PA15,对应MPU-6050芯片的AD0地址管脚
    # i0 R; _+ Q: r4 E
  34. GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);  // 禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO!!!1 ]5 w/ W$ }8 Q2 c$ [9 k
  35. MPU_AD0_CTRL=0;                                          // 控制MPU6050的AD0脚为低电平,则从机地址为:0X689 G6 C% V* K. p: k- k& V* f
  36. MPU_IIC_Init();                                          // 初始化IIC总线/ Q9 Q0 U- o+ s; ~1 X
  37. MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);                  // 对电源管理寄存器1输入0x80,复位MPU6050; J; O! T$ |1 p! t
  38. delay_ms(100);) s0 A1 a( Y$ F4 {
  39. MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);                  // 对电源管理寄存器1输入0x00,主要令[6]位设0,正常工作模式,即唤醒MPU6050
    $ |3 Z( @1 A4 R  E2 G
  40. MPU_Set_Gyro_Fsr(3);                                     // 设置陀螺仪传感器满量程范围,±2000dps( v* z  D. `( p% p- _  q! H
  41. MPU_Set_Accel_Fsr(0);                                    // 设置加速度传感器满量程范围,±2g
    ( I$ m* q4 T. v: u1 ^2 z6 i
  42. MPU_Set_Rate(50);                                        // 设置采样率50Hz
    ) i+ G. E: l" S) }" f2 H
  43. MPU_Write_Byte(MPU_INT_EN_REG,0X00);                     // 对中断使能寄存器输入0x00,关闭所有中断1 t0 p) E3 N2 t! ~0 B2 t% J
  44. MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);                  // 对用户控制寄存器输入0x00,I2C主模式关闭( F( W1 f) L5 Y2 F. @3 T2 I4 u2 J% `
  45. MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);                    // 对FIFO使能寄存器输入0x00,关闭FIFO
    $ S  K  v8 H7 z, b
  46. MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);                  // 中断/旁路设置寄存器输入0x80,INT引脚低电平有效
    $ Y+ p% U7 j6 k, `1 O2 j5 _8 Q
  47. res=MPU_Read_Byte(MPU_DEVICE_ID_REG);                    // 读取器件ID寄存器值,传至res里/ ]8 e% p/ D" A  S' H- z1 G
  48. if(res==MPU_ADDR)                                        // 如果res=0x68,则器件ID正确7 x( K  q1 X1 }# ?! Y* @( V
  49. {
    2 j* H& F7 V& R# u1 _" R
  50.   MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);                 // 对电源管理寄存器1输入0x01,设置CLKSEL,001,PLL X轴为参考
    , B9 k2 e, K! e  Y6 x# w
  51.   MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);                 // 对电源管理寄存器2输入0x00,加速度与陀螺仪都工作(都不待机)
    ' r) l, }$ b9 o% Q
  52.   MPU_Set_Rate(50);                                       // 设置采样率为50Hz
    - E& l$ o& i) h- R( m- J$ b
  53.   }' N9 V2 ~8 T' T& @% W7 e
  54. else - Z7 m7 a* \. u* s
  55.   return 1;7 ?8 \; X9 i: p4 X. h/ ]
  56. return 0;) D7 Y9 T8 t$ c3 a+ g, Z
  57. }
    6 ~7 N: x' E1 S. R% J
  58. /*********************************************************************
    1 W% B3 H" |. E  F
  59. 函数名称:u8 MPU_Set_Gyro_Fsr(u8 fsr)4 r$ o* X$ W% ?, w3 V, ?
  60. 函数功能:设置MPU6050陀螺仪传感器满量程范围
    ) A/ t0 ^& O1 M1 u' f
  61. 入口参数:u8 fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
    ( |. {, }% y1 E6 F6 d2 T$ a
  62. 返回参数:0,成功,1失败0 N- R: s8 W7 _# s) _& N
  63. 修改作者:Aaron
    % M8 ?$ x) u$ j4 ^( P1 n- b
  64. **********************************************************************/
    / v: f+ t% U% s+ `' `# N- R3 p
  65. u8 MPU_Set_Gyro_Fsr(u8 fsr)
    7 v1 X  k. [# \& Q' X4 O
  66. {
    / d, E  C3 [9 n. m$ t
  67. return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);  // 设置陀螺仪满量程范围为±2000dps  
    4 b! N8 F4 H* R8 Y$ S6 W0 ~: t4 e" G
  68. }/ Z+ {" L5 D* B* T
  69. /*********************************************************************+ c7 n4 h. F2 A5 H/ `- Q/ _$ m1 R
  70. 函数名称:u8 MPU_Set_Accel_Fsr(u8 fsr)
    1 ~4 ~! ~& ]6 v
  71. 函数功能:设置MPU6050加速度传感器满量程范围0 [  }8 _4 K$ b/ d# l8 ~
  72. 入口参数:fsr:0,±2g;1,±4g;2,±8g;3,±16g$ @: }' m8 c1 N5 ?
  73. 返回参数:0,成功,1失败
    1 ]5 b$ ~9 U: T  `& N
  74. 修改作者:Aaron& N: q! K- z+ M6 a5 g6 I
  75. **********************************************************************/
    2 }! y8 d- z* A' z( S. _
  76. u8 MPU_Set_Accel_Fsr(u8 fsr)7 Y8 a+ A( B1 {! x, }" R# o
  77. {
    0 D# o5 R$ ^- r7 ~/ k$ z$ f, d( r
  78. return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3); // 设置加速度传感器满量程范围为±2g  
      J/ Q, j6 r% g& p
  79. }, R" T# @% d$ w3 ~# j
  80. /*********************************************************************2 n! K, f0 C/ P4 \
  81. 函数名称:u8 MPU_Set_LPF(u16 lpf)6 O9 G5 x& i/ g1 ^$ n
  82. 函数功能:设置MPU6050的数字低通滤波器; t% {& Z; O$ d- X% C
  83. 入口参数:lpf:数字低通滤波频率(Hz)
      f$ v5 g% G3 i% g- H
  84. 返回参数:0,成功,1失败
    7 ~; f, |3 M) q2 ?- n; R
  85. 修改作者:Aaron
    . @/ j9 ]' L! n% C
  86. **********************************************************************/
      Z% ]/ m2 j# S8 i9 n
  87. u8 MPU_Set_LPF(u16 lpf), g, s) F4 i8 w9 ~9 d8 y' Z
  88. {
    8 ~1 p6 S8 E  E7 T2 o- R2 f
  89. u8 data=0;
    : ~3 @& q0 N* n# t* D5 b
  90. if(lpf>=188)                                // 如果lpf≥188Hz,则data=1# W  t' K  \! i- M; E
  91.   data=1;# i5 p2 A" \* A9 y; s
  92. else if(lpf>=98)
    4 I; J$ |% p6 ~0 l4 Z5 D8 o
  93.   data=2;
    ( n% N" a6 J1 ]
  94. else if(lpf>=42)
    - G, c6 T8 w# n/ B8 O  w% u
  95.   data=3;! J2 h3 s5 v! d- e: `2 R! D; k8 i* a
  96. else if(lpf>=20)
    0 F% J& Z& q- E8 S' m1 I5 |1 v3 X
  97.   data=4;2 p4 m2 W' J; G* b- P1 _
  98. else if(lpf>=10)
    3 Q5 \% }( `* ]  c. v
  99.   data=5;; P' b% ^2 v) i
  100. else
    & E9 }% v7 f7 w% x
  101.   data=6; 0 z1 z, T; G6 H, E1 o
  102. return MPU_Write_Byte(MPU_CFG_REG,data);    // 设置数字低通滤波器  : ~! X! Y6 s" {9 c
  103. }$ Y; ^4 u2 Q- y. ^2 N5 C2 o% S
  104. 4 w7 c, a3 x( B' m3 M3 ^  t
  105. /*********************************************************************
    / r+ ]3 t7 u0 X5 k9 [
  106. 函数名称:u8 MPU_Set_Rate(u16 rate)8 {3 r* e, C% A" r9 ^/ l
  107. 函数功能:设置MPU6050的陀螺仪采样率(只在陀螺仪输出频率Fs=1KHz时成立)% Q1 O* `5 e& U/ R3 w! u
  108. 入口参数:rate:4~1000(Hz),陀螺仪采样频率& J5 _9 f9 u* I% _; @- k
  109. 返回参数:0,成功,1失败/ M/ g/ Z, V$ H) c  T' T) A
  110. 修改作者:Aaron
    8 d+ T! f3 t  X0 }
  111. **********************************************************************/
    6 p% ]5 [' Y) E" g
  112. u8 MPU_Set_Rate(u16 rate)0 l; d+ H$ v* d' K; G
  113. {
    , {7 i1 T* j8 }7 d! G7 l
  114. u8 data;
    / t& k6 C2 _2 y6 T' l
  115. if(rate>1000): Z- u. Z4 N, m4 ~/ w
  116.   rate=1000;7 Z1 E: _; a7 w# `- j
  117. if(rate<4)
    ( w5 T; C0 `, y) ~; y: R
  118.   rate=4;
    ( B, A9 t# j( P. i3 U6 P/ x: u
  119. data=1000/rate-1;6 C" f" o$ n( h; y" h9 d6 K9 M
  120. data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);
    , i" O' x9 l2 H( ~$ K2 X+ W! r
  121.   return MPU_Set_LPF(rate/2);                // 自动设置LPF为采样率的一半
    : ~7 b. g9 p7 Q0 w( G7 Q1 j* c
  122. }
    9 T" {) Z' [& N
  123. /*********************************************************************1 {3 Y  ]6 `* Y3 s
  124. 函数名称:short MPU_Get_Temperature()
    % j- G- {2 k  _# K8 u' ~
  125. 函数功能:得到温度值& z) A; E6 w, v6 e0 L5 P
  126. 入口参数:rate:4~1000(Hz),陀螺仪采样频率; c# K! \' q# B
  127. 返回参数:温度值(扩大了100倍)
    9 ]; L7 K1 k* c) x/ e- e
  128. 修改作者:Aaron
      V# k. K2 W4 X0 R5 `/ v1 F
  129. **********************************************************************/
    + ^. j* P  m  }" N) E9 `
  130. short MPU_Get_Temperature(void)) E, _' Z  b, Q$ C/ }2 _3 t
  131. {/ n( T0 N  @  x, x; _# H: }
  132. u8 buf[2];
    ! K1 ?# N1 w  K& i4 ]0 Q
  133. short raw;; t1 D' G: L2 N( ?6 ?. }
  134. float temp;3 n5 o  K: D1 a5 ~* Q; P5 T
  135. MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); // addr:器件地址;reg:寄存器地址;len:写入长度;buf:数据区
    ! }7 u8 M3 @% b$ w! W
  136. raw=((u16)buf[0]<<8)|buf[1];  
    / J$ E& u0 H3 O& P- F) y
  137. temp=36.53+((double)raw)/340;  + T! P  e1 y! T1 {' _
  138. return
    & t8 i8 ^( R/ C* p" ^. N7 F- Y; N& G
  139.   temp*100;;
    4 \! ]1 c" _, y1 F! ?7 C
  140. }
    ; v* Q) H2 i$ C2 @: m) Y
  141. /*********************************************************************5 @: T, v4 w! n
  142. 函数名称:u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
    : W9 W. F+ n" m7 I5 ?- w
  143. 函数功能:得到陀螺仪值(原始值)
    6 p% b3 [/ m) r) p8 I  ~- W* c
  144. 入口参数:gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
    7 I1 c& }# u- |/ O! O* o
  145. 返回参数:u8 0成功,1失败: R. I+ h7 K. q
  146. 修改作者:Aaron: N7 a; J) a4 C+ P
  147. **********************************************************************/' h& }! G. M" w/ `$ b" K
  148. u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
    4 ]4 E' @( R' P7 m2 [  ?
  149. {
    7 i; B+ A6 e7 o4 v1 o1 r
  150. u8 buf[6],res;  
    : c# q" W8 y9 e8 o$ J5 W
  151. res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);) x8 y. G( K5 V; N
  152. if(res==0)
    : j% B( R/ C) ]' z: e
  153. {
    " D0 p- n# v# w# t) j
  154.   *gx=((u16)buf[0]<<8)|buf[1];  
    ) B' z6 r! d# [* s$ m- M# b  ~! X
  155.   *gy=((u16)buf[2]<<8)|buf[3];  ( d) |. q! R; q7 T% s3 Q4 A
  156.   *gz=((u16)buf[4]<<8)|buf[5];# }* j$ ~: A( b' u5 a* c
  157. }  
    . J) I$ W1 Y) k9 H* S) a5 h7 K
  158. return res;;
    & Q+ ~; ]" N+ q5 N1 }1 ]# h# E- \7 Z
  159. }
    & l5 y1 Z/ l8 x
  160. /********************************************************************** ?. R2 i" {. N1 v6 Q
  161. 函数名称:u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
    ' }' G6 ^) Z5 Y4 V: L; @9 }( l
  162. 函数功能:得到加速度值(原始值)
    ' f$ D6 o0 n5 f- ~6 t
  163. 入口参数:gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)7 t0 e) T, W( D2 G3 m
  164. 返回参数:u8 0成功,1失败5 `$ m7 j. v! {" L" {, `8 y/ [+ D1 e9 u
  165. 修改作者:Aaron
    % h. o* |2 j+ o: t( n8 N5 M
  166. **********************************************************************/
    1 }6 B4 G* E. d; ~$ \- J& x
  167. u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)8 J  s+ s! ?2 z; s2 D
  168. {
    . s( K8 X' U( W, }
  169. u8 buf[6],res;  
    1 g8 G; B7 d+ F! y  g* u
  170. res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);" n0 v2 }; n. l; C- d
  171. if(res==0)1 A$ J; Z, H2 I5 n6 p4 b% g
  172. {" s1 Q7 p% `" s+ H  i
  173.   *ax=((u16)buf[0]<<8)|buf[1];  3 }/ e' ?8 p3 t& E
  174.   *ay=((u16)buf[2]<<8)|buf[3];  
    / W: Y- C  g# f( J1 J0 k) z
  175.   *az=((u16)buf[4]<<8)|buf[5];8 \# k# V  T" \9 X) Q, {$ b
  176. }  
    , ]5 E- {7 A' h* a. ?. v/ g' s  l
  177. return res;;- Y4 H0 U# o. `- Z
  178. }
    + x6 E. p, r1 R9 y0 i6 t0 i" I/ _, D3 W
  179. /*********************************************************************( M4 F6 J8 @& Q2 @
  180. 函数名称:u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)' m# D. n8 T+ e4 S- O6 Y5 x; i
  181. 函数功能:向MPU6050连续写入数据
    / M2 M# }( L; }6 p. w- z
  182. 入口参数:addr:器件地址;reg:寄存器地址;len:写入长度;buf:数据区) T9 [5 e7 C+ V8 q
  183. 返回参数:u8 0成功,1失败
    3 g% f( w2 P& a7 X
  184. 修改作者:Aaron
    : Q* r2 m$ b' ~6 S
  185. **********************************************************************/, A4 u8 O: ~2 M6 t# `( N" X% B! ^
  186. u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)) l9 U; {8 G2 Y* I) O
  187. {
    5 F4 \3 U! Z' f, `0 N
  188. u8 i;
    1 C2 j4 V* U6 A4 O* W' d- Y7 X+ {
  189. MPU_IIC_Start();
    9 k- B" A9 L* y
  190. MPU_IIC_Send_Byte((addr<<1)|0);   // 发送器件地址+写命令
    7 G$ H: R# M8 B
  191. if(MPU_IIC_Wait_Ack())               // 等待应答
    . e% Q, c( \- b( d8 {- Y
  192. {
    ' a, ]$ N: {! h
  193.   MPU_IIC_Stop();   3 B8 L- t# h* @/ p
  194.   return 1;  
    1 s4 t& l0 R* F
  195. }% W$ K1 R6 o, K+ L5 Q7 C2 O# r0 x- @
  196. MPU_IIC_Send_Byte(reg);              // 写寄存器地址9 B9 Z1 E' W( A- O( h9 Z9 o0 W
  197. MPU_IIC_Wait_Ack();              // 等待应答
    % o8 G; v7 [# j
  198. for(i=0;i<len;i++)
    ' d/ a4 n4 a0 x( |: B/ m
  199. {! d: a$ q  Z' U4 Y0 w2 P
  200.   MPU_IIC_Send_Byte(buf);      // 发送数据9 a6 _# M- E+ u: h, W
  201.   if(MPU_IIC_Wait_Ack())          // 等待应答
    . K# d; S+ N0 c) o+ C7 H
  202.   {
    + A# Y, O/ ^6 ?, K5 @8 `
  203.    MPU_IIC_Stop();  ) d. ~7 x5 j6 z3 H6 H- x( d  [+ R
  204.    return 1;   # Y) s- L0 n# f/ T  Y
  205.   }  
    # E' |( W6 g7 a8 {
  206. }   
    6 d0 ^  h& n, O* x3 X# D% T- n
  207. MPU_IIC_Stop();  
    : Y9 D1 ]0 ~6 y  P" E, j: q
  208. return 0; $ \5 [& w- s; Z
  209. } 6 E! B3 ^' D, k# _0 O/ {  D
  210. /*********************************************************************
    $ z# {, a5 W; f3 I
  211. 函数名称:u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)& n$ E3 T' y! J/ t8 V" {
  212. 函数功能:从MPU6050连续读数据
    . v" X- K, s% R3 G6 Q5 ]6 M( V
  213. 入口参数:addr:器件地址;reg:要读取的寄存器地址;len:要读取的长度;buf:读取到的数据存储区
    4 Y; I4 {7 W3 ~4 [
  214. 返回参数:u8 0正常,1错误- D5 J$ E5 c$ A% O8 F2 _
  215. 修改作者:Aaron
    / r# f% j2 L" ~) j" @& ?
  216. **********************************************************************/
    . e# p0 Q% s2 }/ H5 m( c
  217. u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)- s: m! k. {9 R
  218. {
    . R0 ?- C0 e" W1 U" D, C" q- f
  219. MPU_IIC_Start(); 1 m7 |! m) C0 W9 L& [5 C' h' p  O
  220. MPU_IIC_Send_Byte((addr<<1)|0);    // 发送器件地址+写命令
    - ~$ x9 b+ y- N- s! @5 D4 E
  221. if(MPU_IIC_Wait_Ack())             // 等待应答
    ) G. e# z: _' f/ y$ z. F  q
  222. {5 g* y' b5 O0 i
  223.   MPU_IIC_Stop();   ) u1 L. F1 ~! x9 [
  224.   return 1;  
    6 y* g) d' [$ p" A9 h& ]- M
  225. }
    0 r( d* ?6 p- C4 m/ f0 i
  226. MPU_IIC_Send_Byte(reg);            // 写寄存器地址
    * T( ^+ O8 m7 R/ j8 ?; d/ _
  227. MPU_IIC_Wait_Ack();                // 等待应答- P( [2 f% L6 l3 |4 _  K
  228. MPU_IIC_Start();1 W. \) w6 W; p( j( t
  229. MPU_IIC_Send_Byte((addr<<1)|1);    // 发送器件地址+读命令
    : ?6 q* h* N/ ]. T, n
  230. MPU_IIC_Wait_Ack();                // 等待应答
    8 }2 p  e. S0 y& Z2 D5 D$ _4 `
  231. while(len)
    $ b. |8 v* A+ S% m* [2 m2 P3 @
  232. {5 q& \* T# B" y2 A1 R8 Y
  233.   if(len==1)*buf=MPU_IIC_Read_Byte(0); // 读数据,发送nACK
    7 c/ Y3 k# }9 O7 x% Z
  234.   else *buf=MPU_IIC_Read_Byte(1);      // 读数据,发送ACK  
    " ^: |% h$ z; i% G/ c
  235.   len--;+ {9 |% h7 h# B4 p. p$ n; L- i
  236.   buf++;
    0 T# J' z( x& s* a% Z
  237. }   
    + x+ v& u0 W% ]% ~3 X8 ?6 j
  238.     MPU_IIC_Stop();  // 产生一个停止条件 , m' `; {3 y7 Y8 V
  239. return 0; 4 Y3 w1 _- _) l* J$ L
  240. }
    6 `" u% c5 K2 z' R$ U$ t
  241. /*********************************************************************4 G: [! l' g7 r) S: G
  242. 函数名称:u8 MPU_Write_Byte(u8 reg,u8 data) ; X5 M! Q( }+ y% h4 E! m5 q
  243. 函数功能:向MPU6050写一个字节 . e6 m/ G+ f& W% T4 o, ~
  244. 入口参数:reg:寄存器地址;data:数据
    & F. k1 ?; h/ Z3 Z) Q7 T
  245. 返回参数:0,正常;1,失败
    ! t! @  k- Q& i
  246. 修改作者:Aaron
    ; m3 N! |( R& i/ B
  247. **********************************************************************/$ @$ F# y1 ]- J8 |2 s5 g1 U& a
  248. u8 MPU_Write_Byte(u8 reg,u8 data)      
    / N5 |. t5 A2 N! n6 ]% M
  249. {
    4 i& {7 w- x5 B  N
  250. MPU_IIC_Start();
    0 I1 Q$ M6 U# r) e5 H1 D
  251. MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);  // 发送器件地址+写命令
    / T9 k3 E" p& m# |. \7 w
  252. if(MPU_IIC_Wait_Ack())                     // 等待应答,0接收应答成功,1接收应答失败
    : d8 T7 v" H1 L& M$ z6 K5 X
  253. {1 ~( L8 ]5 {) y2 e  f
  254.   MPU_IIC_Stop();                             // MPU_IIC停止信号  P7 I- l4 u3 L7 e2 b6 |
  255.   return 1;  
    ! B- H# ^! e4 X. s
  256. }
    * B2 U( u. ?2 Q/ ]
  257. MPU_IIC_Send_Byte(reg);              // 写寄存器地址! }9 R. {% @8 l! U
  258. MPU_IIC_Wait_Ack();                  // 等待应答 6 r- g+ }+ b. d' W! W
  259. MPU_IIC_Send_Byte(data);             // 发送数据5 y$ |- M1 A, e& Q; f8 ?! e
  260. if(MPU_IIC_Wait_Ack())               // 等待应答
    2 i( i+ x) ^  a' H. a! [- ]: r' M
  261. {: N6 O' A5 I% }" z+ j
  262.   MPU_IIC_Stop();  
    6 F3 i0 q; b5 ~" ^
  263.   return 1;   
    5 W9 D; Y3 Y$ T/ U
  264. }   & p, Z. @9 j# ?
  265.     MPU_IIC_Stop();  2 ?& S; I$ e* Z" Y
  266. return 0;2 s3 |' O& k. P4 U
  267. }5 K" M# o3 R$ o, I' ~2 i# @
  268. /*********************************************************************
    . H( H5 x/ e4 ^% P" K
  269. 函数名称:u8 MPU_Read_Byte(u8 reg)' T, K3 j  e  H' a; A1 B
  270. 函数功能:从MPU6050读一个字节 , L" w" R% y! @
  271. 入口参数:reg:寄存器地址
    5 v4 ~6 N( F3 h$ b  w
  272. 返回参数:u8 读到的数据
    5 k- T; T; J3 p6 @
  273. 修改作者:Aaron
    / R* d) u) ]' Q4 i, \/ W! _# {
  274. **********************************************************************/1 @$ d4 B2 p, J9 h6 k9 b* b6 D
  275. u8 MPU_Read_Byte(u8 reg)+ W- d. H% t3 t8 A
  276. {
    4 {! c7 A9 P7 t9 o0 o5 R) Z
  277. u8 res;
    0 o6 n) ^0 c* a$ R1 s4 |
  278. MPU_IIC_Start(); 2 Y9 R: c! r, h4 g; F8 p; `
  279. // 开始标志(S)发出后,主设备会传送一个 7 位的 Slave 地址,并且后面跟着一个第 8 位,称为 Read/Write 位。 0写,1读。
    & T2 z' G7 Q7 j. w/ ~
  280. MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);   // 发送器件地址+写命令
    " e$ o& m* {+ M, |1 c2 B8 O3 n, V
  281. MPU_IIC_Wait_Ack();                           // 等待应答
    9 e/ ]) M1 J) {; s- C6 k( g
  282. MPU_IIC_Send_Byte(reg);                         // 写入要读取字节的寄存器首地址* G) y3 u/ c7 s  A0 s
  283. MPU_IIC_Wait_Ack();                          // 等待应答# l: b7 A+ t% H
  284. MPU_IIC_Start();  b) q" L$ J* g' S
  285. MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);   // 发送器件地址+读命令 ) d, S$ L: v' \8 U+ U, C/ _
  286. MPU_IIC_Wait_Ack();                           // 等待应答 $ I& D  ^9 u) E* t
  287. res=MPU_IIC_Read_Byte(0);                        // 读取一个字节的数据,发送nACK 1 I" j8 g8 C" Z. W9 n
  288. MPU_IIC_Stop();                                    // 产生一个停止条件
    * m, H6 H: ^+ S  q3 r" `4 j
  289. return res;  
    : i& g* f" ]  R8 F  V" i' S( o
  290. }
    0 c) u& c$ Z$ _! ^9 H7 X  g2 J2 p
  291. /****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
复制代码

( Z4 Q! a& Z' t' ~$ b10.5 main.c主函数+ e( @7 H+ w" c# I" h1 v4 s
; D3 ~8 {: W. O6 ]- v  C* o& F' V
  1. /**# I& S5 t: |& h5 O
  2. ********************************  STM32F10x  *********************************
    ( B$ O4 M  A& }; @! c; `
  3. * @文件名称: main.c5 y9 K: T3 |, ]) G4 A
  4. * @修改作者: Aaron  ^; T, s* P! }- F
  5. * @库版本号: V3.5.0
    3 l* A9 r  ~0 z% i5 S; N/ S
  6. * @工程版本: V1.0.0
    ( R: Q" S$ _7 R( S8 e: F
  7. * @开发日期: 2020年11月30日* B; c+ {4 B5 Q6 K
  8. * @摘要简述: 主函数,增加了在LCD显示6轴的原始数据; X9 J/ v+ N) R  M* A/ ~  l
  9. ******************************************************************************/% c5 y2 v& G8 }. R8 t# d6 L
  10. /*-----------------------------------------------------------------------------
    2 q2 f5 m+ ]! I6 k) [7 u0 Z
  11. * @更新日志:' M  w! o3 }- Z+ x& t- V8 _) Z
  12. * @无
    4 I+ c$ z$ b3 t% e# m- r
  13. * ---------------------------------------------------------------------------*/& Q. i; ?, ~' e' \9 x
  14. /* 包含的头文件 --------------------------------------------------------------*/3 ]6 W5 @% Q+ ?; C! b( Q. L5 M
  15. #include "led.h"
    - ~0 d8 g' T/ @, V
  16. #include "delay.h"8 n" Y) D( X- g4 G6 T$ |* v
  17. #include "key.h"$ y% h( \% }: P% I* d5 s4 ]* _
  18. #include "sys.h"  ]7 q, Y) K/ G" @" U7 f8 f- L" J
  19. #include "lcd.h"5 G- G! z& p$ w6 w
  20. #include "serial_communication.h"7 }5 Y. R/ z, T8 U5 U
  21. #include "mpu6050.h"
    ' ~0 b4 C: t' h7 Z; p; l6 m; g" t
  22. #include "inv_mpu.h"/ e; b# T5 k; B6 C
  23. #include "inv_mpu_dmp_motion_driver.h" - \1 U$ _4 x+ o( d
  24. /*********************************************************************
    % H8 z1 A5 e+ h5 e0 Z
  25. 函数名称:usart1_send_char(u8 c)1 T; ~: ~6 ~* b& n: q% ~
  26. 函数功能:串口1发送1个字符& X: o0 w4 W% _% C
  27. 入口参数:c:要发送的字符# S8 ^$ U2 P* A7 q
  28. 返回参数:0,成功,1失败
    1 S( x3 U8 D. [7 ]+ p! ]& S! i
  29. 修改作者:Aaron
      E2 J$ z7 M; f  H; T' @4 p$ f
  30. **********************************************************************/6 p' D  [) `9 g$ U
  31. void usart1_send_char(u8 c)8 n6 I" `  v# o
  32. {           # E' |5 _2 l5 z2 L' h. |+ N
  33.         while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); // 循环发送,直到发送完毕   
    8 K+ ?8 a6 {3 A& p3 ~( A
  34.         USART_SendData(USART1,c);  
    2 Y. n* Y& u( G) z% T
  35. }
    3 X1 f" ~% g+ u; j: ?0 i+ p2 ~
  36. /*********************************************************************0 I: H: R, y8 M+ J" m( W* p/ q
  37. 函数名称:usart1_niming_report(u8 fun,u8*data,u8 len)' P% Q3 d! Q* X6 s
  38. 函数功能:传送数据给匿名四轴上位机软件- j) k( i4 F1 U' P" L( @; _
  39. 入口参数:fun:功能字.0XA0~0XAF;data:数据缓存区,最多28字节;len:data区有效数据个数
    # D3 l) Y$ `5 ]1 ^
  40. 返回参数:无
      ~: y" v/ ?8 r& m- K) F
  41. 修改作者:Aaron, m0 x) \( w; a; ^- i  y+ T2 L$ n
  42. **********************************************************************/' \( P) g9 l# n* _9 \  b1 D
  43. void usart1_niming_report(u8 fun,u8*data,u8 len)
    & M+ a5 `/ _  C, h) Y1 W& [' d
  44. {
    ' h& r6 ]: o  \, f. s
  45.         u8 send_buf[32];- \# k! W' V, ~7 m( o$ [( w! {7 l
  46.         u8 i;
    " \) ^: l6 B) O5 o7 z1 t, u( e
  47.         if(len>28)return;                // 最多28字节数据
    5 K9 q5 c8 d9 Y& x( l
  48.         send_buf[len+3]=0;                // 校验数置零" ?1 j9 E; b, a% l% y( `: [( f1 y& p
  49.         send_buf[0]=0X88;                // 帧头
    # |# }& \! s3 {* u4 f3 H+ h
  50.         send_buf[1]=fun;                // 功能字
    5 M5 x1 a& C2 R
  51.         send_buf[2]=len;                // 数据长度0 K: m; o5 ^; Z7 u1 l
  52.         for(i=0;i<len;i++)send_buf[3+i]=data;                                                // 复制数据9 Y8 w: W5 M0 D& P
  53.         for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf;                                // 计算校验和        0 S" u9 t9 J. b, y3 }5 W  f6 M
  54.         for(i=0;i<len+4;i++)usart1_send_char(send_buf);                                // 发送数据到串口1
    ) s; [% v' U  q$ Y6 X+ a
  55. }
    * z* K% q- h2 U& K. [
  56. /*********************************************************************2 X# D/ q* }2 W: j: y* Z
  57. 函数名称:mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)! g" J# [8 O1 N
  58. 函数功能:发送加速度传感器数据和陀螺仪数据8 ~8 [+ R3 q6 Q: H3 S, q3 k
  59. 入口参数:aacx,aacy,aacz:x,y,z三个方向上面的加速度值;gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值% U) ]( F2 o- ^& U$ N
  60. 返回参数:无/ `7 n, G5 {6 x* Q9 o1 |
  61. 修改作者:Aaron
    & z" w( U3 B0 y  X& u
  62. **********************************************************************/
    7 }/ r" J( A, n6 s1 G  S. `
  63. void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)0 Y$ N( ~. o  g0 a( J; n
  64. {
    $ y5 `' I: F! W! I% f: {
  65.         u8 tbuf[12];
    & e8 G9 B: \. T3 T" X9 S- o
  66.         tbuf[0]=(aacx>>8)&0XFF;
    & t) j( ?& D# z, l
  67.         tbuf[1]=aacx&0XFF;
    & z2 m5 _1 f9 V, @; L4 V$ p
  68.         tbuf[2]=(aacy>>8)&0XFF;! ]8 z) g1 I; n! r
  69.         tbuf[3]=aacy&0XFF;5 [. J2 q' R$ g1 V
  70.         tbuf[4]=(aacz>>8)&0XFF;
    % l# M' x' ^! n* t' ^4 i
  71.         tbuf[5]=aacz&0XFF; ( K! y. s- ^0 L: S
  72.         tbuf[6]=(gyrox>>8)&0XFF;- ~& R* i0 k' R1 W+ c& p+ t+ i+ [
  73.         tbuf[7]=gyrox&0XFF;' ]8 t; a& M1 T" M6 B
  74.         tbuf[8]=(gyroy>>8)&0XFF;
    % k- b0 @5 v* c. x# \7 K
  75.         tbuf[9]=gyroy&0XFF;! u% K1 D# _# o: ?& E
  76.         tbuf[10]=(gyroz>>8)&0XFF;7 P! W  a9 _, b) K9 q7 C! H7 {
  77.         tbuf[11]=gyroz&0XFF;/ U. r/ p& c  g3 t" G
  78.         usart1_niming_report(0XA1,tbuf,12);                // 自定义帧,0XA1
    . }* _- I' ?8 H
  79. }        
    8 n) `4 o; j. O7 x( ~
  80. /*********************************************************************% U$ z4 W' d% m5 d% O
  81. 函数名称:mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz); n& B8 h% w# i2 r) Z; `: L$ h* b
  82. 函数功能:通过串口1上报结算后的姿态数据给电脑
    & }) h* |1 G9 b- J4 y! |
  83. 入口参数:aacx,aacy,aacz:x,y,z三个方向上面的加速度值;gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值;# d. B9 @. t# {5 j/ Q. q
  84.                                         roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00  ->  180.00度;
    . S6 A; Z( _/ N7 X+ h4 D% C% _
  85.                                         pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度
    - G3 ?7 I1 M, i5 x
  86.                                         yaw:航向角.单位为0.1度 0 -> 3600  对应 0 -> 360.0度9 \! L# G+ q) ?7 d6 {- O0 d( B
  87. 返回参数:无' U0 H/ C5 ]* q  v% }# O; i8 S
  88. 修改作者:Aaron
    0 Z: K  ?# M8 F
  89. **********************************************************************/
    9 B7 B0 u. `( s# q
  90. void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)2 G$ R# y5 H( m  ^
  91. {* Q( B6 G! z& k' x# I& s, |
  92.         u8 tbuf[28]; / Y. z9 _5 q) d0 W: H+ H' c
  93.         u8 i;
    4 f: }5 X( b4 O8 f
  94.         for(i=0;i<28;i++)tbuf=0;                                        // 清0
    & y. _6 Z6 O1 o
  95.         tbuf[0]=(aacx>>8)&0XFF;
    $ w( _4 _- |1 C# t
  96.         tbuf[1]=aacx&0XFF;4 R4 G2 H0 C! x* O4 t
  97.         tbuf[2]=(aacy>>8)&0XFF;
    - q$ ~1 W7 |3 Q1 L: A* i$ E& }
  98.         tbuf[3]=aacy&0XFF;, x6 _7 L  X- Y6 b, h& p% W& ?% X
  99.         tbuf[4]=(aacz>>8)&0XFF;
    & a# _$ V8 S" |' D1 F, B
  100.         tbuf[5]=aacz&0XFF;
    - U- G* X. K0 v6 o% Q
  101.         tbuf[6]=(gyrox>>8)&0XFF;
      ~/ K: X. }+ z1 y/ G2 b
  102.         tbuf[7]=gyrox&0XFF;9 W6 W) X- K$ u# d3 f. `& Z
  103.         tbuf[8]=(gyroy>>8)&0XFF;
    , s5 e7 R/ T# ]
  104.         tbuf[9]=gyroy&0XFF;
    ( B' o1 X6 T4 f+ L
  105.         tbuf[10]=(gyroz>>8)&0XFF;, ]& C/ A, j. l& T4 I
  106.         tbuf[11]=gyroz&0XFF;        
    : I% R8 h3 i+ I1 |1 s
  107.         tbuf[18]=(roll>>8)&0XFF;
    ( Y6 D* q1 v6 `  O5 P/ p
  108.         tbuf[19]=roll&0XFF;' j" H' [  M5 y) Z0 v- Q! n8 w
  109.         tbuf[20]=(pitch>>8)&0XFF;2 T( }" c7 H9 L9 I$ L8 `9 [4 C3 R
  110.         tbuf[21]=pitch&0XFF;4 s% m/ z6 b0 \+ S# @
  111.         tbuf[22]=(yaw>>8)&0XFF;
    ! N: y; X: k# n/ Y
  112.         tbuf[23]=yaw&0XFF;
    . Z" g2 R0 ]/ w* W
  113.         usart1_niming_report(0XAF,tbuf,28); // 飞控显示帧,0XAF
    / a8 J# J- q$ x5 U! b
  114. }  
    9 l# ?1 p  F) X* d
  115. /*********************************************************************6 S! c% J; R( j/ s4 }) Q4 h, V
  116. 函数名称:int main(), m6 I# w9 }" D+ I
  117. 函数功能:主函数,增加了在LCD屏上显示6轴原始数据的功能, v; ~* j/ U% m
  118. 入口参数:无
    % E7 u& P: W1 F- X$ ?7 G% ~
  119. 返回参数:无
    5 e5 z5 N$ W. K; W; e( N
  120. 修改作者:Aaron/ j3 }1 j  S' k! p& b
  121. **********************************************************************/( N6 ?$ r$ G- K0 Q
  122. int main(void)
      B* h  @" k) g# ^- B
  123. {         ( S7 w1 j/ o8 H% G2 T7 k
  124.         u8 t=0,report=1;                                                // 默认开启上报,发给匿名四轴上位机软件
    8 O3 z$ @: n3 `5 q
  125.         u8 key;! @$ |5 Y( @0 ]. S$ |
  126.         float pitch,roll,yaw;                                         // 欧拉角
    3 G. ^& B4 M5 R7 q6 v* ^; n
  127.         short aacx,aacy,aacz;                                        // 加速度传感器原始数据
    9 [$ K( Q* K& \( y, P: c! b& I
  128.         short gyrox,gyroy,gyroz;                                // 陀螺仪原始数据7 x# Y: {: A, E+ t% _. z. n
  129.         short temp;                                                                // 温度         
    6 g9 S  X$ d- L; H) r( G1 A& z
  130.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);                         // 设置NVIC中断分组2:2位抢占优先级,2位响应优先级  _4 |! {9 Q, ?2 C/ P" X% q% q$ v
  131.         My_USART1_Init();                                                                                  // 串口初始化为500000
    2 [) J! j$ P( Z+ ?1 u  g/ Z
  132.         delay_init();                                                                                         // 延时初始化 % @1 w+ N- B' J1 t- V
  133.         LED_Init();                                                                                                   // 初始化与LED连接的硬件接口9 Y5 L: N: _+ T! |) _
  134.         KEY_Init();                                                                                                 // 初始化按键
    % B0 v0 t5 p. i( H
  135.         LCD_Init();                                                                                                    // 初始化LCD  
    . H& ~  m( K5 W( a( M, S8 l
  136.         MPU_Init();                                                                                                 // 初始化MPU6050
    0 x  W0 K+ V8 _$ g! U; k$ {( j) I- B$ B
  137.          POINT_COLOR=RED;                                                                                 // 设置字体为红色         
    7 R9 ]$ b% ~% }; u& G
  138.         LCD_ShowString(30,70,200,16,16,"MPU6050 TEST");        8 Q3 |# Z0 [" U4 w5 j+ i
  139.         LCD_ShowString(30,90,200,16,16,"2020/11/30");
    , P3 \4 _: C4 E/ X1 S5 U) I
  140.         while(mpu_dmp_init())
    2 i* U, \5 g4 |! r$ T
  141.          {) B. k) d3 Q& B  ]- ~  U6 u% ?3 M, `& @
  142.                 LCD_ShowString(30,110,200,16,16,"MPU6050 Error");
    5 h+ I! v* T. N- Z; q7 P
  143.                 delay_ms(200);
    ! F% J" V6 u( s. g: c3 M: r
  144.                 LCD_Fill(30,110,239,130+16,WHITE);
    0 G8 U% Q) l4 e9 n& H
  145.                  delay_ms(200);
    . \5 Y! f" I9 `, I1 ~0 d+ j
  146.         }  8 I0 t  o- N  n6 U2 {0 `+ |
  147.         LCD_ShowString(30,110,200,16,16,"MPU6050 OK");
    . x4 y! C, E$ |% Y
  148.         LCD_ShowString(30,130,200,16,16,"KEY0:UPLOAD ON/OFF");
    : [1 ?4 `4 F0 @% T; ]5 x
  149.         POINT_COLOR=BLUE;                                                                                                                                 // 设置字体为蓝色 6 V. @; F0 }' z+ X
  150.          LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");         
    $ ^- S. F, Y, l. K0 w# Y8 f
  151.          LCD_ShowString(30,200,200,16,16," Temp:    . C");        
    1 Y! J) J- n: G+ T! g: o
  152.          LCD_ShowString(30,220,200,16,16,"Pitch:    . C");        - f/ t7 n) n. q8 |; h4 u8 ?+ t
  153.          LCD_ShowString(30,240,200,16,16," Roll:    . C");         4 i+ b7 _' r1 C4 A: I0 a! v/ Q
  154.          LCD_ShowString(30,260,200,16,16," Yaw :    . C");         
    / T0 {2 u* {' t' @7 o' ]
  155.          while(1); b: O8 K+ @) E2 b, j3 ^5 D8 N
  156.         {! x: E+ J& g$ x/ a) `9 {" h* p8 x
  157.                 key=KEY_Scan(0);
    ! X2 j: Q; T" f0 w* r+ S
  158.                 if(key==KEY0_PRES)
    - h6 _* g, W+ w7 d7 W3 x- I5 |* M
  159.                 {
    : `) ~9 J. R8 x+ I( C
  160.                         report=!report;( A4 p0 w5 b4 j
  161.                         if(report)& J- S9 \+ u1 R( C: V8 `" w
  162.                                 LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");' t: r# h' |+ @1 S: N. t, T
  163.                         else
    . G' _  s2 I. Z, b/ J8 t( ^
  164.                                 LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
      y$ X  U: c! x3 W4 {) J, F$ y9 h
  165.                 }
    6 G( @7 R# U- k9 f8 }
  166.                 if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)! e; r1 _8 T. d
  167.                 {
    ) `. a5 }, h8 L- i( N$ D
  168.                         temp=MPU_Get_Temperature();                                                                        // 得到温度值                 ) t! s) \) f8 K) \# z- Z
  169.                         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);                                        // 得到加速度传感器数据
    / X% a6 M5 Y4 t
  170.                         MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);                                        // 得到陀螺仪数据
    4 K+ h# b7 S) r  d* s" F
  171.                         if(report)8 i1 B5 ?& I( `' J. x4 c- G! P
  172.                                 mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);        // 用自定义帧发送加速度和陀螺仪原始数据
    / [( n. A# y! V5 l- f/ x% c
  173.                         if(report); m/ `& w# ~9 J; \
  174.                                 usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
    * o+ H$ Z) m9 d/ O  @$ l' b# z3 y3 t4 c1 ^
  175.                         LCD_ShowString(30,300,200,16,16," aacx:    ");                                                                  
    8 a/ m6 G0 P, o) L. X2 z$ w: p5 l
  176.                         if(aacx<0)6 c: c" ?/ w( u& f) [
  177.                         {
    * H) p- B% M- ~8 X# b
  178.                                 LCD_ShowChar(30+48,300,'-',16,0);                         // 显示负号
    3 b; a. b: I" W7 A) L% L9 e  h2 @6 Y
  179.                                 aacx=-aacx;                                                                        // 转为正数
    ; R9 ]9 A; }8 f5 T' O9 k
  180.                         }: K# V# f( h) O
  181.                         else % I" _: J$ T) r1 I3 _
  182.                                 LCD_ShowChar(30+48,300,' ',16,0);                        // 去掉负号 - g) C' F& U* m# Y" c1 ~: v
  183.                         LCD_ShowNum(30+48+8,300,aacx,10,16);                        // 显示aacx                            + r1 C; O+ T/ t9 n- Z4 m! a4 u2 y7 ]
  184.                         LCD_ShowString(30,320,200,16,16," aacy:    ");        
    3 y! S$ N' C- }* r+ ]
  185.                         if(aacy<0)
    ( Q" [2 I8 B' _5 E3 J
  186.                         {
    2 }) ]; g% p' P# B: h5 M
  187.                                 LCD_ShowChar(30+48,320,'-',16,0);                          // 显示负号
    " `( y. {' z+ L& r8 S7 K
  188.                                 aacy=-aacy;                                                                        // 转为正数; j+ C! z- d9 U2 M
  189.                         }5 s0 O. k- I9 J, Y" }4 s  I4 f( u% v# @
  190.                         else
    9 V- {1 c1 b4 S
  191.                                 LCD_ShowChar(30+48,320,' ',16,0);                        // 去掉负号 9 N0 i- x7 H$ Y6 ^
  192.                         LCD_ShowNum(30+48+8,320,aacy,10,16);                        // 显示aacy            
    : F) @. p" _$ i
  193.                         LCD_ShowString(30,340,200,16,16," aacz:    ");         ( j9 g6 e* v! A9 D; d
  194.                         if(aacz<0): C0 n- P3 b( k* p
  195.                         {7 {7 D# b3 A1 j* {
  196.                                 LCD_ShowChar(30+48,340,'-',16,0);                          // 显示负号/ p5 b& q( C) M6 }
  197.                                 aacz=-aacz;                                                                        // 转为正数6 A+ t. p( v7 b$ |3 g- r6 j$ q
  198.                         }
    , S. |( ?6 a# n4 X
  199.                         else 9 j3 P3 [. c( |4 h
  200.                                 LCD_ShowChar(30+48,340,' ',16,0);                        // 去掉负号 - j; h* x0 r1 ?
  201.                         LCD_ShowNum(30+48+8,340,aacz,10,16);                        // 显示aacz           ; ?% v+ ?* r: [! p; [# f
  202.                         LCD_ShowString(30,360,200,16,16,"gyrox:    ");        
    + P! y4 c7 W% h4 O1 B
  203.                         if(gyrox<0)
    2 E5 e5 v2 f! M1 f- i7 [$ M, S* N
  204.                         {
    5 |1 b8 R1 s8 v8 Z% c; k: z
  205.                                 LCD_ShowChar(30+48,360,'-',16,0);                          // 显示负号
    0 R, s' _. n7 m1 ]( D+ ?8 d* {
  206.                                 gyrox=-gyrox;                                                                // 转为正数
      L2 w0 @0 {# n, [5 w
  207.                         }
    & K' }$ s# G! k, J  K0 |  f6 T0 ^
  208.                         else
    $ O# ]( c* [5 l; H0 M/ K7 C* t& G+ k- p
  209.                                 LCD_ShowChar(30+48,360,' ',16,0);                        // 去掉负号 9 _, U7 _' h2 a1 ?6 S. Q& o& w& B
  210.                         LCD_ShowNum(30+48+8,360,gyrox,10,16);                        // 显示gyrox   
    7 N( ^2 k" f/ `! X$ D9 v; J7 I
  211.                         LCD_ShowString(30,380,200,16,16,"gyroy:    ");        
    7 p6 S  I5 x. q" n/ E$ _% J
  212.                         if(gyroy<0)8 z" m7 E3 _; k. N) }, }* l
  213.                         {
    8 @- [  Q8 n. f- J
  214.                                 LCD_ShowChar(30+48,380,'-',16,0);                          // 显示负号; F$ q6 \4 Z/ R5 X' e- ?; }- o
  215.                                 gyroy=-gyroy;                                                                // 转为正数; T4 h. a* ~3 _6 Q/ ?, d
  216.                         }
    + y; s5 u, @" p# Q( ], J
  217.                         else 2 B1 H% i- X1 _5 Z+ k% {& J$ l
  218.                                 LCD_ShowChar(30+48,380,' ',16,0);                        // 去掉负号 ! V5 p  y: c( J' g3 D- e  x5 [+ b
  219.                         LCD_ShowNum(30+48+8,380,gyroy,10,16);                        // 显示gyroy                  
    6 `" v# F! g3 \2 ?0 Y( @8 e0 ~' t9 O1 X
  220.                         LCD_ShowString(30,400,200,16,16,"gyroz:    ");# o, Q- l$ y# r; C( \2 U
  221.                         if(gyroz<0): Z3 \$ u. c: N3 a
  222.                         {- U6 l4 x9 M7 ~
  223.                                 LCD_ShowChar(30+48,400,'-',16,0);                          // 显示负号
    * G5 H6 h, R" J! {* m
  224.                                 gyroz=-gyroz;                                                                // 转为正数1 W0 N: z" U5 w0 X
  225.                         }
    - ]; F8 E% K! B5 I) }! v* r% H) a
  226.                         else
    $ Z+ J) O( V; t& J7 K- K
  227.                                 LCD_ShowChar(30+48,400,' ',16,0);                        // 去掉负号 # `7 \3 E' r6 {- Y+ I$ W2 i
  228.                         LCD_ShowNum(30+48+8,400,gyroz,10,16);                        // 显示gyroz                           
    9 r9 G+ N7 g7 j4 ^  _& F
  229.                         if((t%10)==0)0 s9 W4 q2 c9 ?5 s$ A
  230.                         { 1 a& q) o0 c# Z* P/ d! O
  231.                                 if(temp<0)
    3 a$ d) r! C6 E& g
  232.                                 {
    + l- i, M5 n! g' ~( b% o3 V
  233.                                         LCD_ShowChar(30+48,200,'-',16,0);                // 显示负号
    " C5 [6 v% i* p. @
  234.                                         temp=-temp;                                                                // 转为正数
    & l) r+ x3 s, C' J# i" c2 o
  235.                                 }
    ) p% W$ \9 l4 ?2 f) k: L" o
  236.                                 else
    " m, v3 A; ~0 e! w+ I3 I0 ]
  237.                                         LCD_ShowChar(30+48,200,' ',16,0);                // 去掉负号 ) }" J/ M0 _( G6 p4 k! w. L& E
  238.                                 LCD_ShowNum(30+48+8,200,temp/100,3,16);                // 显示整数部分            , e2 l' g: F4 E' a& j
  239.                                 LCD_ShowNum(30+48+40,200,temp%10,1,16);                // 显示小数部分 . ?+ a8 J5 Q1 v) Q$ a4 r( @! _  I" J
  240.                                 temp=pitch*10;& [+ R, z/ ]# ]- }( ^0 _7 w# d
  241.                                 if(temp<0)
    9 |: S+ @/ n+ M
  242.                                 {- E5 q, i: X" F4 l3 K" h4 e  I6 U
  243.                                         LCD_ShowChar(30+48,220,'-',16,0);                // 显示负号3 c! [: l; G! o& W# l1 f; [
  244.                                         temp=-temp;                                                                // 转为正数0 `+ S' W' S! m
  245.                                 }
    7 f  R8 }7 Y8 w$ }4 _# E' S* G. Y
  246.                                 else * I( }/ X3 ^5 C* o" L$ O9 O
  247.                                         LCD_ShowChar(30+48,220,' ',16,0);                // 去掉负号 1 C, c+ Y8 k* [8 I, z) Q% t
  248.                                 LCD_ShowNum(30+48+8,220,temp/10,3,16);                // 显示整数部分            
    ; R, w/ H- u9 T  s- m
  249.                                 LCD_ShowNum(30+48+40,220,temp%10,1,16);                // 显示小数部分
    ( z6 W, X9 ]2 c% {
  250.                                 temp=roll*10;3 Q- l/ c& `" j- A8 o
  251.                                 if(temp<0)
    " r: C: K, g9 }5 W4 k# J
  252.                                 {6 _  r9 Q+ _# X& K8 T" R
  253.                                         LCD_ShowChar(30+48,240,'-',16,0);                // 显示负号
    3 A0 G9 C1 N- c- B! F
  254.                                         temp=-temp;                                                                // 转为正数1 U. q& ~" e/ \/ k# V
  255.                                 }
    ! P4 M! t) ?/ j& p0 E' A9 P9 k
  256.                                 else   T3 w' h+ T: d( j: l9 B
  257.                                         LCD_ShowChar(30+48,240,' ',16,0);                // 去掉负号
    $ z4 p( q# Q! k! i0 @; S4 v
  258.                                 LCD_ShowNum(30+48+8,240,temp/10,3,16);                // 显示整数部分            5 g) ?3 r# j6 k$ Q
  259.                                 LCD_ShowNum(30+48+40,240,temp%10,1,16);                // 显示小数部分 . L# ]* a) P0 ~' \
  260.                                 temp=yaw*10;
    ! O3 s* m9 T: {' ?1 g
  261.                                 printf("temp=%d\r\n",temp);
    ; p6 V: Z( o. q/ |5 G8 X
  262.                                 if(temp<0)
    . Z1 o$ d  v. a0 B% I4 \6 N3 I6 Q
  263.                                 {
    * V+ y9 z# [( o# Q  \2 O. G6 w( `! h( q. `
  264.                                         LCD_ShowChar(30+48,260,'-',16,0);                // 显示负号% S6 {0 v3 S. U  C. N- j( }
  265.                                         temp=-temp;                                                                // 转为正数
    / M' |. C& `1 `$ f' C
  266.                                 }" ?9 d. o& H3 S) M3 b
  267.                                 else
    8 n% t) O6 o! u2 `6 {3 z
  268.                                         LCD_ShowChar(30+48,260,' ',16,0);                // 去掉负号 5 _8 W- [( M' }& N+ c
  269.                                 LCD_ShowNum(30+48+8,260,temp/10,3,16);                // 显示整数部分            " B3 e" v# n9 P, m: h8 e
  270.                                 LCD_ShowNum(30+48+40,260,temp%10,1,16);                // 显示小数部分  
    5 Y) o- g' |+ I
  271.                                 t=0;% h7 s) F" `/ I- p, g3 }; O5 S
  272.                                 DS0=!DS0;                                                                        // LED闪烁
    & [- F$ W0 z' x0 G
  273.                         }0 k* c/ R$ R; r; M
  274.                 }$ B1 a9 g: s$ J8 K
  275.                 t++;
    # I3 y: L' m. L
  276.         }         : g5 M+ X" X- S. ~7 K' B
  277. }( \3 u6 M" d, h; C# O
  278. /****** Copyright (C)2020 Aaron. All Rights Reserved ****** END OF FILE *******/
复制代码
# w- S+ s3 H; K5 e
10.6 移植文件
# `8 Y4 z, Z  |8 k& I+ o9 a0 B1 R文件中代码太多,这里主要关注:/ ^( v* }6 H7 I, T0 g9 ^1 h
1)inv_mpu.c源文件中定义了四个函数:- n  s* [% N" C. p/ Q, |: J9 d+ \  ^' V
; e$ U0 g5 X! Q3 d5 g! G- u
  1. #define i2c_write   MPU_Write_Len  // 对应mpu6050.c源文件里的MPU_Write_Len
    * l/ ]% |$ O* \/ G$ I
  2. #define i2c_read    MPU_Read_Len   // 对应mpu6050.c源文件里的MPU_Read_Len
    : y0 G! _1 S+ w; u* p
  3. #define delay_ms    delay_ms       // 对应delay.c源文件里的delay_ms. e8 m( I: \4 m+ Z% |4 L
  4. #define get_ms      mget_ms        // 这是一个空函数
复制代码
7 ^; F5 H0 ?+ I7 R' ]6 m
2)inv_mpu.c源文件中u8 mpu_dmp_init()初始化函数:6 L; G' q4 m% o: G2 A

! R  |: `: D8 c0 M
  1. u8 mpu_dmp_init(void)
    8 J4 H7 M% |4 O2 X2 M9 k! d9 Z
  2. {
    2 I9 _3 Y0 t3 {3 a2 q) h
  3. u8 res=0;3 v/ Y# O; N2 |
  4. MPU_IIC_Init();   // 初始化IIC总线
    1 M3 y. |# y( E' V+ x) W
  5. if(mpu_init()==0) // 初始化MPU6050' J7 r  n; G5 o  e
  6. {  $ x. E. K6 l7 T. ~% B3 x
  7.   res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);     // 设置所需要的传感器% ?7 C8 Z9 O, b
  8.   if(res)return 1;
    ) g: {& h  i7 ?' c
  9.   res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);  // 设置FIFO( F, h+ ]1 V4 q; Q1 {
  10.   if(res)return 2;
    ' I7 }4 r9 C( E# @# N# j
  11.   res=mpu_set_sample_rate(DEFAULT_MPU_HZ);             // 设置采样率, S! U6 Q; ^* k# k  m$ Y0 c2 n; |
  12.   if(res)return 3;
    : J1 s$ i6 k: J% ]$ d
  13.   res=dmp_load_motion_driver_firmware();               // 加载dmp固件
    0 G) G( G: d7 c2 q6 o  o1 ?
  14.   if(res)return 4; " ~3 Z! l. E# G3 `
  15.   res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪方向; w- Z# ?( ?8 t& @" g5 X
  16.   if(res)return 5;
    5 W+ a5 Q+ {$ E; }$ M
  17.   res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|               // 设置dmp功能
    3 |0 v- i6 U, h& Q' Z$ H1 c
  18.       DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
    ; {" ]: c9 U% E$ Y2 f, W( [
  19.       DMP_FEATURE_GYRO_CAL);  v1 L! s& @8 }# L6 a3 B
  20.   if(res)return 6;
    + L5 f2 ?- Q/ u' v9 _  a
  21.   res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);  // 设置DMP输出速率(最大不超过200Hz)
    # C( z; o" P2 v) Y+ z) l7 h+ \) z% y% x
  22.   if(res)return 7;   
    ; y, _' n) `. e; H6 M6 m4 ]/ }3 [
  23.   res=run_self_test();                    // 自检/ e0 c; P* N4 r; x
  24.   if(res)return 8;    . v$ h6 P; B3 ?: V8 M
  25.   res=mpu_set_dmp_state(1);               // 使能DMP: a5 D; _9 E) W: W
  26.   if(res)return 9;     
    8 m2 A  l( j$ R1 ]( a8 a' m- B& K
  27. }else return 10;
    , T9 `& B; C& a7 y) {2 W# d  \' K, ^
  28. return 0;; M2 I* R, ?/ \6 D
  29. }
复制代码

; n* H0 [! X$ h7 ~) ?3)inv_mpu.c源文件中u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)获取欧拉角的函数:5 o) r& @- _1 e5 d0 R
1 t9 q5 D$ d5 `: i6 u3 Q
  1. u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)  L8 v* s  ^4 H& |: j
  2. {$ g% O4 e& W4 g$ c
  3. float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    8 B( L1 t' `8 A8 P% F: V; L
  4. unsigned long sensor_timestamp;
    + P( V* X9 g9 G! x$ l* v
  5. short gyro[3], accel[3], sensors;( D* M0 E" N1 x; t# W
  6. unsigned char more;
      _  h- J8 w+ T+ g- b
  7. long quat[4];
    - i0 K5 J4 p# y8 a7 a- v/ F  @
  8. if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))4 j1 \: J9 T, T4 z$ N% E7 m% D( [% ~
  9.    return 1;  : A$ m' C) V" @; D+ e, ^; m: `9 Z
  10. ors&INV_WXYZ_QUAT) 5 u# P7 y- _! w" Y) h( G
  11. {2 j  t1 ]9 m5 c: s3 @5 H) Z
  12.   q0 = quat[0] / q30;    // q30格式转换为浮点数
      i+ O+ d& B5 k+ d2 ?
  13.   q1 = quat[1] / q30;
    5 o% `% q! Z% X9 j& `
  14.   q2 = quat[2] / q30;
    % X6 B. p4 X  h. W
  15.   q3 = quat[3] / q30;
    : @  B+ Q& d  Y, l1 }
  16.   // 计算得到俯仰角/横滚角/航向角; S1 _* L' ]6 E- P0 m
  17.   *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch" d& p0 I* n' T, `2 \
  18.   *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll2 O/ @$ c( w& x" v8 E2 l! p
  19.   *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
    " r3 r6 \/ Y' P. T1 R
  20. }
      I% {3 u) s7 w( J9 ^
  21. else ( B) q& w( G  n& a" i! U
  22.    return 2;, n4 s( W, s; J  z: ?
  23. return 0;9 i( K( \$ P! o4 |+ j
  24. }
复制代码
- L0 c6 j7 `9 f
十一、实验结果
& }, a/ P  D, g" U
该程序可用匿名四轴上位机软件实时查看6轴的原始数据和姿态数据,即欧拉角:航向角(yaw)、横滚角(roll)和俯仰角(pitch)。(这里就不放截图了)。! D! Y1 B, Q1 B9 b( L! o
另外在LCD屏上,也可实时查看6轴的原始数据和姿态数据,如下图:) i/ s, I4 W5 L. r0 _8 O; C' |
20201204235655599.png ) Z! d- y( ~1 Q
1 P, r( e! d% W7 I
————————————————$ T, Y3 F& O  `( L4 h7 U
版权声明:天亮继续睡: P% v3 v- a- W" ?4 u/ i$ g# T
# u  k, M8 A- F, Z( Q3 a

0 j& |. n0 e% Y* }8 s7 m& R, ~, s& ~' i- D2 A* `
赞 收藏 评论0 发布时间:2022-11-18 19:00

举报

0个回答

所属标签

相似技术帖

官网相关资源

关于
我们是谁
投资者关系
意法半导体可持续发展举措
创新与技术
意法半导体官网
联系我们
联系ST分支机构
寻找销售人员和分销渠道
社区
媒体中心
活动与培训
隐私策略
隐私策略
Cookies管理
行使您的权利
官方最新发布
人形机器人运动控制、感知与智能配电
半导体创新技术与应用方向
EE架构与软件定义汽车
12V/48V 汽车智能配电(SPD)
区域控制单元(ZCU)与分区架构
关注我们
st-img 微信公众号
st-img 手机版