7 U- a+ w, X# E Z9 C
: |3 j0 }' _* G6 P, P0 Z9 h2 o
5 m/ G0 W( G. ^9 ]% _1 \) l
$ C- [7 s( |! |/ g& u1 T5 x
9 ]& g! u0 g9 m9 U
背景
* ]+ u1 E9 P! }& w3 | 本文用于记录平衡自行车的制作过程,及制作中遇到的问题;总体方案如下:采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。7 ] l. ^. _/ x0 J- O8 U
4 D; W( F! Z( I8 w
MPU-6050简介9 ~/ o0 q* _; ]& l
MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器,并且预留一个IIC 接口,可用于连接外部磁力传感器,并利用自带的数字运动处理器(DMP: Digital Motion Processor)硬件加速引擎,通过主 IIC 接口,向应用端输出完整的 9 轴融合演算数据。
' `6 O; I1 s5 J. G* Y$ L: d, o# U1 f0 N3 O5 t' @
MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC(0~65535),将其测量的模拟量转化为可输出的数字量。且传感器的测量范围都是可以根据需求设定的,陀螺仪可测范围为±250,±500,±1000,±2000°/秒(dps),加速度计可测范围为±2,±4,±8,±16g。
$ W Q8 J# }5 L! ]6 s. ~; e: \( h
; [, k& x: Y" ?% u5 O
5 F, h, v. ]$ {
& ?9 I: y1 a1 d3 }0 \- `1 A) O
7 \- ]5 h6 A, P% d/ EMPU-6050姿态获取与处理(惭愧,我也一知半解)
( T- [. ^2 A, s9 `% M 理论上只需要对3轴陀螺仪的角度进行积分,就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响,只对陀螺仪积分并不能得到完全准确的姿态,所以需要用加速度计进行辅助矫正,常用的方法有三种:互补滤波、卡尔曼滤波、硬件DMP解算四元数。: W) b, t0 Q* {7 w8 X1 n
7 l9 K7 X( M, K/ O$ _" Z
$ ~! f. h" H& M2 h* u w3 P
互补滤波、卡尔曼滤波计算位姿示例
z- c9 L' X0 r+ C4 O
+ t8 K" V p0 \" O+ B6 O! t+ v! d9 `' |
# n# c6 i& V6 O: \
% o% h+ Z# ^7 y3 I
互补滤波、卡尔曼滤波计算位姿示例
3 f. x( _. S6 _+ t' T! m% v
+ C, c) i! {! _ 除了使用加速度计计算的噪声较大,卡尔曼滤波,一阶互补滤波,二阶互补滤波看着差不多,O(∩_∩)O哈哈~(四元数计算的示例就不演示了,有兴趣的可以自己研究一下。)" D8 _5 c) ?5 `1 D% p: _# `
1)一阶互补滤波:因为加速度计有高频噪声,陀螺仪有低频噪声,需要互补滤波融合得到较可靠的角度值。0 }3 A, V9 K/ V" b. c% `
2)卡尔曼滤波:利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程(来源于百度词条,我也看不懂这说的啥)。
) C" y- x G' w 3)硬件DMP解算四元数:DMP将原始数据直接转换成四元数输出,运用欧拉角转换算法,从而得到Yaw、Roll和Pitch(使用四元数计算位姿,好像会将初始化的位姿设定为零位)。1 y; _ S* L) `. J* h- [2 c
5 i9 W! G$ m9 o% x
; b7 c2 S, Q' ]0 n( I ?: I
( o& U. \$ O/ p- P$ J/ l
; U1 l) E0 f: t3 {: F6 m) [' x一、一阶互补滤波算法3 d. T8 b' p: Z, W/ ~' K
MPU-6050 的加速度计和陀螺仪各有优缺点,三轴的加速度值没有累积误差,通过简单的计算即可得到倾角,但是它包含的噪声太多(因为待测物运动时会产生加速度,电机运行时振动会产生加速度等),不能直接使用;陀螺仪对外界振动影响小,精度高,通过对角速度积分可以得到倾角,但是会产生累积误差。所以不能单独使用MPU-6050的加速度计或陀螺仪来得到倾角,需要二者进行互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值,把它们结合到一起进行修正,通过加速度和角速度就可以计算 Pitch 和 Roll 角(单靠 MPU6050 无法准确得到 Yaw 角,需要和地磁传感器结合使用)。
% w- t; B) B% @8 ` ]# C' L8 x
: ^3 ?1 J ], h
* L# j7 z7 |& r9 Y: b0 x! j+ s一阶互补算法如下:
# M& o' d6 K5 u- h7 q9 s. E- //一阶互补滤波& u4 t5 B5 q7 P8 N9 ~
- float K1 =0.1; // 对加速度计取值的权重
# u% I3 d& \% U, ?" x# v - float FirstOrder_Pitch;& Z* g& x% {5 y8 v1 |. ?/ q
& s, y3 O+ Y& _, ^% |- float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
$ }; i( y/ q z - {
5 V4 f2 L( S$ q - FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
. q, }2 U H& Z/ A0 x. K4 _* z - return FirstOrder_Pitch;
& R& ] z" {0 F" {9 w - }
复制代码
4 ^' C' l2 W/ z0 R% K二阶互补算法如下:, Y2 j- w R* H5 `+ F6 l* P7 n
- //二阶互补滤波
" x' q' r' W* _9 w- `2 f$ t; } - float K2 =0.2; // 对加速度计取值的权重8 ]+ v0 G* U0 z6 ~ J8 ]! Q
- float x1,x2,y1;$ P/ j! z( V/ P9 M
- float SecondOrder_Pitch;
8 A7 `' [; S8 K( {0 L7 B - ' V; ], L1 Z! @2 g2 _
- float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度1 \2 I8 c4 f* f
- {1 ?. x9 H z% Y8 m/ P6 j6 E4 z+ K' }
- x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);- |3 K. R" D( r; `" m
- y1=y1+x1*dt;
5 b0 Q7 ?) c6 m7 j4 { - x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;2 c8 z# f: `! F. \" S
- SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
* d0 m( W+ ~& w - return SecondOrder_Pitch;7 e3 f' G, P/ }# q0 U: E5 Z' J
- }
复制代码
) l3 A9 K" U) W* ]二、卡尔曼滤波
4 y+ g/ G$ c S3 z' G% S- /* Kalman filter variables */% [1 ^) X) f" O, ?: p
- float Q_angle = 0.001; // Process noise variance for the accelerometer
+ y+ f- E2 ?& ?& f( m - float Q_bias = 0.003; // Process noise variance for the gyro bias0 n. a- G) ?& M: b6 q
- float R_measure = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise5 y, L' y' j d: D
- - c4 r# p+ v8 t5 R
- float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
. f( q6 ^4 g" l, C' w# v/ K - float bias = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector' u6 e' M) ]: z* g8 q
- float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate2 Y: |9 a: L( k' n8 B$ h/ K" _
- ( Q& N; y" a2 q- p3 A
- float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix5 o9 S; S: w9 p
- 3 a+ B4 v: K. ] c
- # H/ k" ?8 [# H" ~# F$ b' E! R; L. c/ d
- // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
/ Q! \4 }/ m( s& B9 I - float Kalman_filter(float newAngle, float newRate, float dt) {& d- p! @! w: ?( n' q" i
- // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
% U) V: T6 w' j" T - // Modified by Kristian Lauszus
: }! @/ p- j& i - // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it- d0 c/ u) y$ N6 k. g+ t! H. U! E
- 7 G$ v- ~$ M' W. N- {1 u" q
- // Discrete Kalman filter time update equations - Time Update ("Predict")3 M, s, {1 S4 q$ ^+ n! S7 E* J0 e3 a
- // Update xhat - Project the state ahead
* \5 G. r) [5 n' X) H1 l0 r) u - /* Step 1 */, C: S: W0 e3 N" P, f; v
- rate = newRate - bias;1 c: }1 A" X ~
- angle += dt * rate;
! z3 |6 k5 h6 X/ i. r% Y
0 B4 l0 s8 ]* p: I5 s- // Update estimation error covariance - Project the error covariance ahead- O8 ~0 j& r% \5 O( Z# h. h; K
- /* Step 2 */
* c8 l r8 T8 D2 } - P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);" C) _/ R( C3 `# B8 e/ D( ^1 x
- P[0][1] -= dt * P[1][1];
4 C) u; l; Z6 L- O) X- A0 \ - P[1][0] -= dt * P[1][1];) f9 ]' K9 P6 w! ?* r+ o6 F6 Z
- P[1][1] += Q_bias * dt;* @. a* j9 P2 P( f4 [
- # N6 p7 a8 g, U
- // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
2 l- P# \- i* P& e$ I, @0 q L. z - // Calculate Kalman gain - Compute the Kalman gain
- X5 Y3 t5 {4 r0 L* } - /* Step 4 */( V3 z4 z- w- ^; S6 e
- float S = P[0][0] + R_measure; // Estimate error/ Q5 @3 e! k" f+ T" i3 u" G1 D
- /* Step 5 */0 V% O* A) I3 v; G, U7 v: D Z
- float K[2]; // Kalman gain - This is a 2x1 vector, j% n( Q$ e3 I
- K[0] = P[0][0] / S;; x" p G* N! z+ {3 N
- K[1] = P[1][0] / S;
: \0 [8 f* @+ i! e
. @; ~% W7 s; P8 [+ V1 ?5 Y8 r- // Calculate angle and bias - Update estimate with measurement zk (newAngle)
! o1 v6 H& R, c8 W$ f" m% @ I - /* Step 3 */
0 r! ? R6 M/ x: [ - float y = newAngle - angle; // Angle difference. [( e T$ P% s9 r6 w' m2 l# \
- /* Step 6 */2 c5 x( B. Z- s) t' l* W
- angle += K[0] * y;
" S7 k$ Q) Y: \! C/ z. h - bias += K[1] * y;, n0 h% {7 S1 w& s
; Q9 |( f6 Y2 H- // Calculate estimation error covariance - Update the error covariance
; r; ^: C+ Z- v0 n1 ~ - /* Step 7 */
) v4 o- r& Y) w' m2 N2 ^& x* R; [ - float P00_temp = P[0][0];
1 u/ |% Q' j- q- u5 l: q - float P01_temp = P[0][1];
" @ s7 Y3 X" w) l, ?
/ I9 S2 I. Q1 U0 g& ~; c+ B8 A- P[0][0] -= K[0] * P00_temp;/ E) @' X1 W" q K# _3 K# o$ V, j8 F
- P[0][1] -= K[0] * P01_temp;
4 B( V. Q' k) f/ ?5 r m2 m& m - P[1][0] -= K[1] * P00_temp;! v! N# L3 D: [1 |8 i e
- P[1][1] -= K[1] * P01_temp;
# X8 ~# N8 ~3 B- ?- H' `
0 |3 N9 E" f6 M4 a( p# E- return angle;
; ~) r, v ]9 k! z) G' T1 D - };
复制代码 & }2 \( c3 E) M4 S$ @
三、四元数法
2 z6 R3 b Q5 N$ A" j/ s! {* L5 \6 N( g% y3 W- U6 {
MPU-6050 自带了数字运动处理器,即 DMP,并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库,结合 MPU-6050 的 DMP,可以将我们的原始数据直接转换成四元数输出,而得到四元数之后,可以很方便的计算出欧拉角,从而得到 Yaw、Roll 和Pitch。
1 s( G4 I; b" B1 v8 J* V- k" h. X/ P1 D c
使用内置的 DMP,大大简化了代码设计,且 MCU 不用进行姿态解算过程,大大降低了 MCU 的负担,从而有更多的时间去处理其他事件,提高系统实时性。
* G! l$ |( J+ U! K% T7 _# e0 w- `- void Read_DMP(float *Pitch,float *Roll,float *Yaw)
X0 e4 F* B6 i7 \/ _+ ~+ ` - {
6 C. {/ d7 s9 E2 c - unsigned long sensor_timestamp;9 B: n8 ~9 X! j. w( m9 e& {8 [1 j
- unsigned char more;, o: M e/ z- v9 f( B8 q
- long quat[4];7 u$ `$ d9 ]* w. {# v; C( P0 R
E- T1 }! C9 A) b d5 H0 \2 o" i2 l, Z- dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
8 a: |' m' l+ B& Q - if (sensors & INV_WXYZ_QUAT )" B9 d( W- P( a/ z6 q% G
- {" k( U3 m0 _4 \4 J% e& x4 S
- q0=quat[0] / q30;: n0 v* c+ E8 U% c& O1 Y
- q1=quat[1] / q30;- X6 ]+ x9 i1 L2 t4 v; U6 \3 g+ B
- q2=quat[2] / q30;
0 n* k" g2 \" u3 v/ I d - q3=quat[3] / q30;' V; I+ ~# H& Z
- *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
# T7 L; \. G: j. {4 j4 @! A - *Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
6 I" T( V! n) ~' G- Y2 Y5 F - *Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
3 C* G1 d4 W; r2 X - }) T' W0 J0 @! k1 N$ t
- : l B3 U7 P* f' `
- }
复制代码
6 Q) Z1 H% {9 G1 d+ A 以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程,老鸟请忽略。% o9 u% R0 S5 X5 p+ q7 z
6 h G7 l. v( }% e0 E% M: b
固件开发采用STM32CubeIDE开发,对于我这种菜鸡来说还是很友好的。
# h- G }" g# ~8 J! I+ Y& g" m7 d+ V* W/ {# N5 b% i9 N
我使用的是STM32CubeIDE1.9.0,打开软件后,依次点击 File-->New-->Stm32 Project,弹出如下界面,输入MCU型号-->选择封装形式-->Next
7 W; U& u+ w2 n$ K4 I! X" ^: |* u5 ]& q& T, j- k) a$ P
% n3 U2 r G4 v6 {
8 B0 e. O; }- r0 I& q输入工程名字,点击Next- d( b& w& Z4 a# q
, n3 l8 X# E" S- }, o
( P, ~& B0 }' u! Q5 e( f+ ^7 J. ~% [0 @1 k* }
设置完成后,点击Finish* x, T* u; ~3 [: b) E5 F
$ t# T n4 a; _, X* i
2 b0 A, G, g# H l/ ^5 `8 e
+ N7 w3 }0 \9 b6 R9 x进入MCU配置界面
- E$ I+ l( m3 x# E
, ?9 O2 c/ I9 z9 K$ q
- M/ r' }" b6 u% R5 _0 c! R/ ^ F
+ B% P; ?2 ^: N5 m2 l+ v* {
设置I2C与MPU-6050通讯。9 @+ w4 e8 B5 q0 z+ F
7 v4 g- \- g( a) W6 ^; O. h
# E) t, ?0 H1 a9 u+ p* q' Y& n7 B# X' P1 ]5 n6 O; c- l( v
设置串口与上位机通讯,实时显示MPU-6050位姿。
5 y6 m% {" \. R: b" @
1 |: F$ R; [ s! E+ |
0 z1 M9 i L' `9 a% p( c# K
6 v& C9 j* I7 w4 n% \设置系统时钟及调试模式。3 G: x4 E& H- L) U* w0 k
+ F% Z2 G6 j* M5 ?& P7 \& O$ h+ U( v8 C+ p
1 E! m$ ~" _2 l- f
5 c6 _9 j. r2 C- T8 I设置时钟源。5 O3 p; a9 C m+ T0 @
3 |) `8 T. D- a: U% Z) y, [
$ s# ^0 j* ~- l7 b
/ Y7 e2 m) h# k% R8 I7 v; l( N z全部设置好后,芯片引脚的分配情况。
$ G/ u4 j! K/ V9 d$ E/ K- G
* S/ T8 c( N" r3 q/ T
4 m. ?: b- x! S+ Q* B# o* _) a5 s" P% ~1 L6 _' `, K: ?
在时钟配置里设置时钟最大频率。# r) [/ z; K- k$ n8 P6 W
6 W' d& E+ H6 q+ V
: u8 E+ v3 e7 A& f& A( ?
6 k4 Q* B7 L5 ?! r* w/ ^
在工程管理——>代码生成器中进行设置
! H' R; }/ s9 d5 e! S
3 Y" Q0 G# b: a; j
( e& Z+ U3 l7 }: K5 M6 D+ s0 _
}: p) i" X5 j5 {6 N点击此处,生成代码
. h' b3 r# m- z, w8 d) y* M+ \& \( ~* n5 @6 [
3 S8 Y$ t. n! w u
% p5 y0 J; R; f# f3 I
在工程文件夹中创建iCode文件。$ x: ~1 [' T# F
( _! C4 c; a( G3 W
2 @$ r8 L7 `8 o6 Y
; A s7 |# j h3 b
将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。. }0 ^+ W- ^9 n( N: G4 p7 e. _
/ O5 I8 {0 `3 J* d r/ ^# b
6 Z' q' j* v% ~1 L F: L
& V2 }! n6 L6 o点击Debug,刚刚添加的驱动文件全部在工程文件中显示出来。
! W1 O* X h! I; b
, K" T/ d' K# J+ Y" \% B
! B! S# ^7 H/ P2 I2 c( u9 V9 W5 k) D) ?/ b& W0 Z# ^: c9 W
按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。; a7 q5 n- _. ?" r ~
* ^) ]1 h% W6 a" G, \* H- ^4 U
% W! Q0 c2 r6 q% U+ r! p; C3 b
8 X5 Y& J4 O1 c
/ z/ ]- p! P$ E- Y/ b
: z# b& X/ R' t# s+ m2 {
然后再main.c文件中添加如下代码,用于重定义printf函数,用于串口输出;获取系统时间,用于滤波。
% h7 ]0 j0 b* J7 [" G6 r- /* USER CODE END Header */
) B+ G# I9 Y/ I- J( a# A - /* Includes ------------------------------------------------------------------*/
- o( `5 _0 ^5 H! N! Q: Q - #include "main.h"* }1 e) U3 ]5 p& ?; l, f; A
- #include "i2c.h"
3 H' W% V1 l# \* y" R - #include "usart.h"' q! I. h, X, ?. M$ S
- #include "gpio.h"' T% [, l, `- ]: D
3 ]0 E; O3 m- K* Q5 h- /* Private includes ----------------------------------------------------------*/
& r. ~6 W+ r k, F I$ e! u - /* USER CODE BEGIN Includes */
) C: X8 f2 n" ~4 Z$ s+ T- F3 @ - #include "mpu6050.h"
% V# T. [, K# E7 b$ D - #include <stdio.h>
, d# }( h1 @0 y" y - /* USER CODE END Includes */
% Z9 Z8 e: o( f) i* i2 Z7 ^
9 \5 t1 V, L, Z+ a5 a7 A; a% `4 Z- /* Private typedef -----------------------------------------------------------*/" S o/ ]! ^2 b
- /* USER CODE BEGIN PTD */6 Z7 v% b& P& E( e0 u
, w) `+ W* A. L- /* USER CODE END PTD */
6 C* U+ Q% i1 l' V4 G, w - C; ?3 ^: G. N" |
- /* Private define ------------------------------------------------------------*/" Y1 A2 f; D5 X+ b$ n; ]
- /* USER CODE BEGIN PD */; G1 D2 q8 G' y! k- V& ]; u6 d
- /* USER CODE END PD */' G+ b' n2 Y: M# d4 p
7 T0 Z @! b6 o. D; l2 D- /* Private macro -------------------------------------------------------------*/6 p" a7 O* U/ h) v1 y
- /* USER CODE BEGIN PM */) A i8 H9 L0 R
7 B; [, A5 G, {- /* USER CODE END PM */. Z& D$ [# m' n8 K3 B: _
% [ M1 A, ?: y2 D4 R- /* Private variables ---------------------------------------------------------*/
/ B8 }# \* t/ g! Y
2 A* R& Z6 G3 p+ f- K! J- /* USER CODE BEGIN PV */* T9 o5 @0 o( M4 O$ i" N2 D
& @0 T z; Z& |5 ]+ u- //重定义printf函数,用于串口输出
- n* N/ `! |$ \* b" p4 X - #ifdef __GNUC__- e: e' V" M* o) j8 E3 k
- #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
4 {- V1 ]# p/ V1 | - #else( o. H2 A" {7 r: J
- #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f); m$ [# f: y4 D z( @' y
- #endif r1 E" @3 J# l
- F3 _+ ^ w4 I4 H7 r# C- PUTCHAR_PROTOTYPE! Q% O. L* y6 t' A; G
- {; ?9 ] \/ Z6 M# V6 |: K
- HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);& I# g. e) P5 e- T
- return ch;+ b" [8 B! D8 u% t. z5 W' T
- }
# ]( e. U' T6 { }8 q8 A" n - /* ---------------------------------------------------------------------------*/
8 M# H9 x- q b0 @ p0 I - $ K9 E8 C a3 f# _. ^$ l( y. a, b
- //获取系统时间,用于卡尔曼滤波
" N8 U6 }% N7 W& y% J/ T1 t - __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)7 d! s2 H/ r: M4 @
- {( q( [ _8 S, P- u7 n( _
- return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));) W( O3 U! c9 U0 d' I
- }
, ^7 X( X5 m/ n& f+ V6 O1 I8 E - static uint32_t getCurrentMicros(void)
9 f% v5 E5 R3 R( O - {
9 X, e. P% p% q& C7 t; N - /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
" k: a8 y- t9 {0 r i - GXT_SYSTICK_IsActiveCounterFlag();% u6 q; q' P+ Q+ q0 p
- uint32_t m = HAL_GetTick(); r3 |: { {6 @5 w/ D7 w1 u$ P
- const uint32_t tms = SysTick->LOAD + 1;. U3 J3 N0 R# T1 c
- __IO uint32_t u = tms - SysTick->VAL;. x5 I- \: b! D+ g
- if (GXT_SYSTICK_IsActiveCounterFlag()) { d: \; ~, {; h2 e4 ^
- m = HAL_GetTick(); ?2 k+ s. \( {
- u = tms - SysTick->VAL;
- p1 D1 y# {" r F7 s# L - }
% _6 X( G8 z4 e: s+ d( H - return (m * 1000 + (u * 1000) / tms);6 J" l0 S$ t8 W# e
- }: q0 Y1 }1 |9 I, M/ e
- //获取系统时间,单位us% V% m- b5 w# s) r' n! w2 i( p7 O
- uint32_t micros(void)
" o% t7 @7 S p/ s2 j$ Q - {+ ~2 Z% a4 b- D( ]8 W
- return getCurrentMicros();! q" [# }6 M' `2 q2 P
- }
# A7 @$ N8 U8 g6 [: U2 i5 X- K - e; U# z k! e/ c4 ]
1 X$ v# @* S/ l$ D- /* USER CODE END PV */
复制代码 3 j! w) W7 [$ k$ l q; L* t
使用printf()函数,还需进行如下设置:点击菜单栏 Project-->properties,弹出如下界面 H: K8 Y$ ^) Q! i
, ~) }) `+ m% y, q6 [( V
( |; ]/ P" F7 f' k' _ R( ^6 ]0 [$ V+ f$ a0 r
在main()函数中添加MPU6050初始化代码和发送位姿数据代码0 w& d- E4 c. u2 ^* K: Y* r& e9 Y1 K6 S
- int main(void)7 E2 R7 T, p) f; e; b6 a$ d
- {
+ e2 i) g8 {( h5 N B8 I8 E. w1 B - /* USER CODE BEGIN 1 */& Q% s( o: V; v( j; Z
+ C9 L2 ~" d* o) y$ L" i8 E. t- /* USER CODE END 1 */
, q$ a# |+ \6 [8 L7 f
/ u7 \* b+ K4 u$ z- /* MCU Configuration--------------------------------------------------------*/0 a$ k! x8 v( u. r
- ; q- s0 h% X8 Q, h; u9 c$ Z2 b! V9 _
- /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
2 V+ U: H3 C5 I8 ?# H1 K" h6 ] - HAL_Init();
1 S: S0 Y# z* ]6 ~; {
$ b4 }9 O) u, t. T6 ^- /* USER CODE BEGIN Init */
" y+ {' a5 `. S, Z0 P/ U+ Z! J9 g5 @ - ' k/ o) O, D6 d T
- /* USER CODE END Init */
2 f6 F8 d8 s! p& D( o2 f# V% p! T - 2 u% k3 b6 q& Y" o. H+ v+ `5 f
- /* Configure the system clock */
4 h) k F& a' ^ b4 \& m - SystemClock_Config();$ E1 \, r4 |) V N5 s
- : l* }, _) N6 l& t! l9 o" H$ ^3 |
- /* USER CODE BEGIN SysInit */
9 N& F {5 n4 _+ B2 j - 5 x7 Z! ^, J$ g5 E2 D* ]
- /* USER CODE END SysInit */
9 k! V) H) t+ v7 f. P - " m; S* ^4 {+ d# y% Z+ `- Y( v
- /* Initialize all configured peripherals */
+ B8 a' K, e; e- m% \& u: p - MX_GPIO_Init();
/ t7 j8 ]0 D& \; P - MX_I2C1_Init();# }* w1 H8 M4 I$ t' f4 f2 X: e( a
- MX_USART1_UART_Init();8 r1 m% S/ `9 j3 ]) ?' ]3 W5 U
- /* USER CODE BEGIN 2 */
: W; i# I5 }- _1 \# t
( s# q/ p0 V6 N* C9 M9 g- while (MPU_Init()){ //如果初始化失败,继续初始化
: n7 }; ]) u. [4 | - printf("MPU_Init_Fail...");% ~% l: R1 ~7 k3 p& j/ i
- }; O6 o$ R5 }: B" x- y3 ?& O* r! }
1 r( j) t3 Y7 r- I6 ]5 O& I- /* USER CODE END 2 */- r- g( L5 E: F% |# Z$ c
- $ w2 ]. F: U. ?( A, r- _
- /* Infinite loop */
1 y$ p: k2 `: E5 Q4 J" V* n. T - /* USER CODE BEGIN WHILE */- s) X( [" \2 I7 D2 K/ W
- while (1)% O( z0 q2 U6 k* L9 c* U* a- B
- {. M- I: u- N9 \* ~
- GetAngle();1 m$ ]& B# N* n/ U, R" E
- GetPitch();
8 d# X+ I8 S* T, L - HAL_Delay(10);
8 w+ {( U; a( L# _# O - /* USER CODE END WHILE */$ z$ s3 Y: P3 v3 C) a, k, c
7 e% H9 L) g$ m/ h9 ^- /* USER CODE BEGIN 3 */
; ~- o* ^* A. X" d - }
- ^( R, W* Y& p7 G+ g$ h/ s& e - /* USER CODE END 3 */
! q8 Y6 B3 x! Q0 W% A: N - }
复制代码 # ^+ @' d. O1 V/ F' W
点击,下载程序5 _7 ?9 s$ E# `, ~; `" ?% x( g
( ~ }! K& R# [2 c5 Q0 `# u
% g! D; R- j# G6 E+ Z
) \ U9 g0 v# M/ e; t6 r b程序烧录好后就会通过串口通讯向上位机发送位姿数据(加速度计、卡尔曼、一阶二阶互补滤波计算的位姿),上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。
2 V1 {4 }% T; l% _
% o, |! ^2 O) N% o
U& U+ h# r/ f( o9 G; H; ~8 K
5 L* M; w9 q2 C: R9 K( P 作者:DIY攻城狮 5 V% B/ S; o' T3 D$ z0 ?5 b
7 v( r! w- K7 ?; y
1 e6 A% ]9 k ]# X |
MPU-6050、卡尔曼滤波库在哪里呀