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