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

基于STM32CubeIDE+MPU6050做的动量轮平衡自行车(一)

[复制链接]
STMCU小助手 发布时间:2023-2-9 17:00

- m. e+ J1 L% e

" z* i& ~% a: y N1UI`V7SWCGTLM$D1GQ54.png $ @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 Q(D3Y}_OBQFJ0FA501Q1PWI.png
* 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; }
3M4ZAPN6Y61O]`Q(`F`%P%T.png 7 l; Z' P( b( D2 @8 p
互补滤波、卡尔曼滤波计算位姿示例5 t, \' G% N, F  }5 O: c0 }0 Q

% ~6 ]  ^7 Z! P LTC%Z6L{1J6%~U}3VE@]A$P.png ) 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
~3FY21~}NEG2BK(M@DI]NBR.png
+ 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, c
5 X0 t4 z( R3 y$ k; S1 r
一阶互补算法如下:
& I" l1 m8 j; Y( ^% W( f
  1. //一阶互补滤波
    2 p" h* `/ K7 g: i. Z; W
  2. float K1 =0.1;         // 对加速度计取值的权重
    8 V+ L' ^; i% s. i
  3. float FirstOrder_Pitch;: l, `. ]3 v6 F: v# `! G
  4. " Y9 ^5 i* F; `% D* N; I, B
  5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度' m8 h8 e3 c- S# y: `7 g
  6. {
    7 f( h" ^1 P0 w/ s8 y* ]  `' H
  7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);- s- Y6 U1 @9 S1 P
  8.   return FirstOrder_Pitch;
    9 f- r2 b1 N; v$ _+ t) C$ ]
  9. }
复制代码

5 ?$ P& X# G* s# m二阶互补算法如下:" Y1 Q8 ?8 ^  h7 k$ f
  1. //二阶互补滤波
    # p' k7 c7 U4 c0 ]
  2. float K2 =0.2; // 对加速度计取值的权重
    ! v; z  }3 o+ q
  3. float x1,x2,y1;  q# m- u; N9 u' Q( H% ^
  4. float SecondOrder_Pitch;# d% H! ^7 F; a: J

  5. : [* O+ W1 N( ]" ~
  6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
    ' _( E) v* r' M1 e1 Z
  7. {
    3 V% S5 O6 q; C& z: v, X) u6 J" L# T
  8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
    % O* E5 p6 G* D% ?% @  K' r1 D8 e
  9. y1=y1+x1*dt;
    % U, c; i! x- Z6 T8 t
  10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;8 L0 {8 l- ~4 h0 w7 B' |; [
  11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    7 W6 T+ d4 a6 R' U+ u6 V8 s6 x% |
  12. return SecondOrder_Pitch;0 Q+ m4 S9 m$ h0 G7 _5 d" I
  13. }
复制代码
2 b6 m- W/ Q4 \3 M/ q
二、卡尔曼滤波
" c1 V/ w1 ^+ X
  1. /* Kalman filter variables */% A2 n7 J" u7 i
  2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
    6 O7 v( `0 L% f" ?
  3.         float Q_bias = 0.003; // Process noise variance for the gyro bias
    ) O" M4 d' @, j/ i
  4.         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
  5. ( c" O0 A. @2 l5 R8 Q
  6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
    % O' w) ?; f  X: d0 X
  7.         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
  8.         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

  9. " G  c8 ?; U1 y5 _+ H" ^
  10.         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

  11. + c* T9 Y/ {) V& Z- Y' d" e
  12. 3 i+ j6 G- \4 x! r
  13. // 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
  14. float Kalman_filter(float newAngle, float newRate, float dt) {
    7 ~1 T$ H3 R- R- u4 t
  15.     // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145/ `( F2 Z* _$ ~2 X- ~& N
  16.     // Modified by Kristian Lauszus: I. i' G$ R6 s* R! i2 L. z* i
  17.     // 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

  18. * e6 w! e$ N& `: Z9 D9 H' O4 L
  19.     // Discrete Kalman filter time update equations - Time Update ("Predict")8 e! d% y1 O1 f9 ]
  20.     // Update xhat - Project the state ahead
      d2 S. o7 [( h; T) r* \
  21.     /* Step 1 */; [5 {7 u# z5 f. V# U0 q8 c  H$ U
  22.     rate = newRate - bias;
    , A7 J& x. l3 w- q" r& ~5 t1 G
  23.     angle += dt * rate;2 c+ O9 l8 A, b, j- U- \

  24. / G) _# e% S8 l
  25.     // Update estimation error covariance - Project the error covariance ahead! |( J. S5 a" Q" k
  26.     /* Step 2 */* i! A7 U( U6 B, Z  K' m
  27.     P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    6 W& {: f, i  t; ^; @4 e3 Z
  28.     P[0][1] -= dt * P[1][1];2 K# j' M! d1 r3 L- h
  29.     P[1][0] -= dt * P[1][1];& w6 ~8 Z# M2 t$ u& i
  30.     P[1][1] += Q_bias * dt;: {' g- p) A7 ~3 L& ?$ X. d
  31. 6 c9 [/ n8 K8 Q+ _& n" i
  32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    2 s( V1 S1 i, k8 d0 K5 y2 b7 \
  33.     // Calculate Kalman gain - Compute the Kalman gain& P3 @; Y; ~/ l. g- m  Y$ q* D
  34.     /* Step 4 */" ?0 {$ w1 Y4 c) V- U
  35.     float S = P[0][0] + R_measure; // Estimate error" _: V; D* v3 i
  36.     /* Step 5 */2 n6 W. R0 T* J; n( A
  37.     float K[2]; // Kalman gain - This is a 2x1 vector
    + F4 B" _# I- u, C* l
  38.     K[0] = P[0][0] / S;
    2 g& _" B1 [" @+ _9 b! P
  39.     K[1] = P[1][0] / S;
    8 V8 I/ c( o; v- c8 [' C
  40. 9 k( d* w+ h, H
  41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    6 P% a) z5 ~, X
  42.     /* Step 3 */
    5 v- t7 z7 O, N) }  }: d: P
  43.     float y = newAngle - angle; // Angle difference: J( L0 s0 P/ b6 Q
  44.     /* Step 6 */
    ( b& m. A8 N$ s+ y( E9 g8 F# B
  45.     angle += K[0] * y;( c0 ~2 P# ^, H1 V- Z
  46.     bias += K[1] * y;1 I& i1 t0 R4 \8 I+ @

  47. + h# \% f4 c8 A' F
  48.     // Calculate estimation error covariance - Update the error covariance
    ' Y+ u% h0 e& `! K, O
  49.     /* Step 7 */
    + k- w0 |. g: Q' Y/ o. i
  50.     float P00_temp = P[0][0];- O$ b2 U, ]# _4 y9 d7 _. B! r9 E3 L
  51.     float P01_temp = P[0][1];
    0 b  A7 Z  }) e% A: c" H

  52. 6 M; j6 x! p0 Z* s: m2 c) Y& {/ ]
  53.     P[0][0] -= K[0] * P00_temp;0 J1 b' K" h. B
  54.     P[0][1] -= K[0] * P01_temp;8 s9 C8 i! Z6 F" z/ A% B4 m1 V! E
  55.     P[1][0] -= K[1] * P00_temp;
    ! D% D5 b3 x: q4 s! c7 W0 V# A( m
  56.     P[1][1] -= K[1] * P01_temp;
    ) K  h; q( {* b' E: R0 s
  57. 5 N$ W: z: Q. _! J$ Q$ l
  58.     return angle;( f' d" {' W) i% J3 |& e* c' Q+ B
  59. };
复制代码

- 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
  1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)5 k7 z9 ?8 S  k. a, q9 u
  2. {1 J  U8 }: }6 x/ o, P5 O2 C2 W
  3.         unsigned long sensor_timestamp;% Q0 F) a2 d+ q4 D( ?' u6 K9 l
  4.         unsigned char more;
    % g6 b9 b8 y3 B
  5.         long quat[4];
    ! B- i7 L& |6 E' K3 t

  6. $ E8 S/ Q* A) J- @
  7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
    , S5 k$ i" I' t9 ?
  8.         if (sensors & INV_WXYZ_QUAT ). h" e5 A3 h0 k( V6 r/ ~; Z" f
  9.         {: m4 \7 }; [" f, _6 m
  10.                 q0=quat[0] / q30;
    - R) K/ r% O; k. F. d
  11.                 q1=quat[1] / q30;
    2 O, y. y$ z  I" V
  12.                 q2=quat[2] / q30;
    ' y- [# `$ M* p. y+ Z( Q6 n
  13.                 q3=quat[3] / q30;# U# M2 y& z7 i2 G  T- D: ^
  14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;7 Z- X7 v2 L5 U* Q
  15.                 *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
  16.                  *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
  17.         }/ {+ i# S8 Z* z$ t

  18. 7 t9 Y3 B- b  z! |8 a6 O. M& v7 c
  19. }
复制代码

& 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
6Z1)SWD0)NGR@]O%J3~_A`G.png
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 @
KU9167BQ{6[RN@@Z{Q}@K7B.png
7 x3 E+ q. L/ O1 W% v6 y0 y$ v4 T# V" o
1 B2 K& e9 C# Z7 _/ W& I
设置完成后,点击Finish
2 C" C8 a7 t3 r8 c6 ]- y& ?2 q  H/ g
)W%A{MBY6WZ9DC`3B@F1KM8.png . 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
LG2T6_XWK%N3B66H$BJS(D0.png   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
CO(V62(7YX0CZ4}@DZ)$Q.png
& C1 O% m3 U# g1 j5 d
0 _, r4 O" I7 A
设置串口与上位机通讯,实时显示MPU-6050位姿。
7 d7 }* T" \6 b; A0 i4 z+ J  c
9 C3 ?: P' N7 ]) m) z3 y
ZJQU$P3UR1R4)729_G][%{S.png
5 P5 f- H% K+ j6 x4 `- P
4 ~) C. f5 h. B% i% B+ h6 n
设置系统时钟及调试模式。
( e- n7 S6 c: g  V# j$ E
) }8 E8 }8 z$ |5 p
(~J`34%%TXA1IXKWT(T@WN7.png , 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
~VCI0F0AO}W_{IQM08)~J44.png 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
SL6GUUTZ2Q)$R5{_){T9V@T.png " 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_0F%FN3W27QQvC$O5S$I.png ' |& _' \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+ @
2ET9LUB057L`M4NNOJ@@HRK.png , 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
NB$AP684`5LM@ZCOK85OHM6.png : \/ @" 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
~3XUPXUKJ3X311X9L%MRRSX.png 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 ~
XFBF%G7M)$$FIFDRGQDM6W1.png
' 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
Y%XW13SG7SJ%JBPZA1JIEAH.png
+ {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
DJZL)$_$FQL20QWJM(8)A.png
7 ^8 u9 _6 ?# Q# P' c8 q! Q- n7 R, F  X
WQUT]3V991@5_$%1Q%8XS`S.png
4 l. u9 e# h4 I; ~0 g- F. U

( o& ]: Q; ^: \# ?然后再main.c文件中添加如下代码,用于重定义printf函数,用于串口输出;获取系统时间,用于滤波。
1 H" K/ |. @* d2 W
  1. /* USER CODE END Header */
    + z5 @6 _" i( w, L# ?; N7 c
  2. /* Includes ------------------------------------------------------------------*/  `1 |8 P$ k$ q7 f1 J4 i3 |
  3. #include "main.h"
      u  j" D4 R9 R! `2 Z" A
  4. #include "i2c.h"
    ; e8 f( `; i7 i, q: A% \; L5 r
  5. #include "usart.h". Z, {( ?4 r* K3 z
  6. #include "gpio.h"
    3 W/ D  C2 r8 g% j3 {
  7. 6 c# a( n" ]0 L
  8. /* Private includes ----------------------------------------------------------*/
    . L* ^2 K# @, z- R- y) ?
  9. /* USER CODE BEGIN Includes */
    . m9 _3 r, d" ?! X5 f
  10. #include "mpu6050.h"
    ; x* e. K: p5 M9 S" Y' A- U, i
  11. #include <stdio.h>
    , r7 V) `. Q# S8 c$ j0 y
  12. /* USER CODE END Includes */" q' a! p# u( c7 m
  13. # g+ T  g: C" p/ e( z: l" w
  14. /* Private typedef -----------------------------------------------------------*/
    6 g* c0 m/ w9 v: i& g
  15. /* USER CODE BEGIN PTD */. p. g3 {; u$ h7 h& ]

  16.   R8 G" Y/ u9 [: B$ e
  17. /* USER CODE END PTD */  A( F3 h$ v% a* V! e' D, {1 Q

  18. . h0 P  T+ ]. L1 N4 t5 B, i
  19. /* Private define ------------------------------------------------------------*/$ ~0 L5 c( R) I; S" U, m
  20. /* USER CODE BEGIN PD */
    ! [4 v/ t, e4 }
  21. /* USER CODE END PD */8 m9 I+ _3 S% ]* u$ V
  22. ) w; ~3 L9 r4 G
  23. /* Private macro -------------------------------------------------------------*/
    1 `$ G' `0 r- c4 _  o9 K/ r4 Z
  24. /* USER CODE BEGIN PM */+ Z$ j/ H% u3 u. j

  25. 0 [4 }8 S+ y" e6 M2 R' R% e
  26. /* USER CODE END PM */# @6 N8 p) v5 d! T

  27. 0 n& _; q6 I; h7 m1 p9 r
  28. /* Private variables ---------------------------------------------------------*/
    $ m! t' C, o: ^- F1 V* o! K

  29. ( S* N& g/ n/ L
  30. /* USER CODE BEGIN PV */8 _. y) i. ^  q4 _- C, b( U  U

  31. , _! R: E5 B6 d
  32. //重定义printf函数,用于串口输出
    2 B5 s. H& Z+ X6 Q
  33. #ifdef __GNUC__
    ) ]5 p! s+ X! o( l: N& |& t5 u
  34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    $ @" j; O" V# D) Z& U4 o8 n$ d
  35. #else
    + g# F2 \' K: V8 [3 R& Y$ B: _
  36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    0 R- U. d% Z  X2 {- i
  37. #endif
    - K/ A, X# X* _
  38. 9 H% o7 |% T" A
  39. PUTCHAR_PROTOTYPE
    1 k. L$ U% E/ l8 z
  40. {+ R) R" `6 E7 e$ Y7 Q' C. f$ ^
  41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    & |* _, f% ?0 x. W" Z6 ]
  42.     return ch;
    ) h1 w! S9 @9 ^2 p! F" T- D5 H: O
  43. }  ^+ Y6 s: K2 z
  44. /* ---------------------------------------------------------------------------*/, N+ ~* p4 g6 _) n0 D7 H

  45. , P1 d0 Y1 d2 n  e; X: y0 L
  46. //获取系统时间,用于卡尔曼滤波9 M0 \( q$ \: I( \  q5 X4 @1 \
  47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)9 ~' ~* W9 T' q
  48. {
    . j! b, |( d# S7 p" M
  49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    0 ~# b0 U- U4 B$ Y" Q* L
  50. }
    - T5 M% I! X; u% i
  51. static uint32_t getCurrentMicros(void)
    + i% _, u- ^$ L8 W. G
  52. {
    9 t: e$ P  {- y* o* p  R1 @' W7 H
  53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
      b1 T- @% V$ e
  54.   GXT_SYSTICK_IsActiveCounterFlag();! z, Y5 g+ Y' ^3 t6 z6 A7 n
  55.   uint32_t m = HAL_GetTick();" E0 C1 D1 v! |( e& n' J5 [7 v
  56.   const uint32_t tms = SysTick->LOAD + 1;( u* I* |+ a$ ~" F$ Q+ _
  57.   __IO uint32_t u = tms - SysTick->VAL;8 o) M5 L3 a5 P+ e) w
  58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    5 K' L/ [4 |7 t$ i5 j7 _$ X8 n- ~
  59.     m = HAL_GetTick();2 m  D! @  E, o3 A/ ~$ Q
  60.     u = tms - SysTick->VAL;, q& Z( Z5 X2 G1 p
  61.   }+ }& B' Y% U% R. _
  62.   return (m * 1000 + (u * 1000) / tms);4 |( A- }. i; K8 y; v
  63. }$ x  e% {9 X0 p; D. R4 z
  64. //获取系统时间,单位us* n( D& o; E" l3 f. W! h. m( u
  65. uint32_t micros(void)" U7 \0 m1 W0 Z3 b! J4 I) Z
  66. {
    - V& ^3 n! V% l) B/ r
  67.   return getCurrentMicros();: F/ B7 V' \5 _) Z. f/ a% X6 [
  68. }4 u2 e3 [& {0 p' B, q

  69. 7 |  N9 d2 e3 Z* L+ \1 a0 y
  70. % N# M9 H1 Q& G; J, k0 {' r
  71. /* 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* ~
[I](O`R0}YFTDA{2SX~2%)J.png
4 v0 [* F4 @$ b5 t0 U! H/ v* G) m! Y1 t4 t
在main()函数中添加MPU6050初始化代码和发送位姿数据代码
  U; g, T5 i- Y: M! R* `& z4 y
  1. int main(void)1 c+ |8 T, K1 j+ T
  2. {
    ! e, `9 ^: x3 y: f
  3.   /* USER CODE BEGIN 1 */
    2 Y" k0 q+ G- q6 J
  4.   t! K2 `% ~  c
  5.   /* USER CODE END 1 */
    ! ?4 {; N: z3 u! O$ P  m
  6. & J8 T7 J/ D! P, I4 |$ V; X
  7.   /* MCU Configuration--------------------------------------------------------*/
    9 f% ~* d5 s, }
  8. 2 _  ?) O6 I0 h3 i* C# @( B# j9 E
  9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    4 K1 ~1 s( a( j1 r" X
  10.   HAL_Init();0 @( q) S1 a% u6 _" m. h8 y
  11. $ n# ?# x6 q+ u* G  O) g9 F( H" X
  12.   /* USER CODE BEGIN Init */
    + w& k9 e: w. B3 o+ y! ?& J' v( h
  13. # N* j( z# q7 O
  14.   /* USER CODE END Init */
    2 A% k" c5 d/ |$ z: c0 l! E6 J$ [

  15. 4 Q0 N* T7 h0 X4 f9 \$ P  d
  16.   /* Configure the system clock */
    ) v: L: |4 o. h
  17.   SystemClock_Config();2 @% |2 K1 e7 P/ v

  18. ! w% E0 ]$ W0 E$ i9 B# J& P2 l
  19.   /* USER CODE BEGIN SysInit */: k: q' B6 o6 x! @

  20. # ~0 i$ }. i( x# T9 E
  21.   /* USER CODE END SysInit */
    7 A3 ]6 n7 L% Q1 u( e  ]

  22. $ p. X/ k/ b) C) n4 c) C
  23.   /* Initialize all configured peripherals */: U3 ]. d. |" p: s
  24.   MX_GPIO_Init();/ Q/ o; r1 D8 w0 L1 d/ p
  25.   MX_I2C1_Init();1 F/ }; K6 x: X
  26.   MX_USART1_UART_Init();
    2 H( E1 M& P# ]" A; C
  27.   /* USER CODE BEGIN 2 */
    5 Y: x, j. K, w! |. m

  28. 9 i9 I  R) l2 R# }
  29.   while (MPU_Init()){                //如果初始化失败,继续初始化
    & A, C# p) f' g) P& c0 k
  30.           printf("MPU_Init_Fail...");
    ) G$ B; B/ o" d' O1 f  U. d
  31.   }
      ]9 K  R* s$ |
  32. 7 W) C& S; Q2 K
  33.   /* USER CODE END 2 */2 t' o5 M2 f9 z% {

  34. $ o+ Y. U! m! N9 c% X- ?) Q
  35.   /* Infinite loop */5 _* ?3 H+ f8 ^5 l/ x* @
  36.   /* USER CODE BEGIN WHILE */
    ( g$ a; ~$ ?3 S) X$ S" ]. _
  37.   while (1)
    ' ]; R( q1 |- T  h6 m0 i
  38.   {
    9 Q: L9 z5 P5 t9 d
  39.           GetAngle();
    8 B$ c0 P+ h; u/ c) D: J6 j8 U, _
  40.           GetPitch();7 O3 l. e% j8 \( Z- W
  41.           HAL_Delay(10);" t! H) v  h2 f) D. {
  42.     /* USER CODE END WHILE */* @! b5 B% |! X& _8 W: s" f

  43. ) |# Y# h. f9 |4 |4 U; y
  44.     /* USER CODE BEGIN 3 */
    " M3 ]2 M" p4 f) ~& u# T
  45.   }
    9 t: h9 a6 R+ q  k0 r$ B: H/ Y4 [
  46.   /* USER CODE END 3 */
    4 B( [2 ~$ m( W& t3 p0 j
  47. }
复制代码

' `& f: k" f0 ]7 U, W$ Y点击,下载程序  s# y( B+ Q3 W$ k7 e# m- {
6 |# \) a' P7 p
{90_LT8S2ACLQ4(5~ZX943B.png 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 H5PKUP6Y02TULI6(N)Y_NZ5.png
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
赞 收藏 评论1 发布时间:2023-2-9 17:00

举报

1个回答
比奇堡成功人士 回答时间:2024-10-23 18:15:57

MPU-6050、卡尔曼滤波库在哪里呀

所属标签

相似技术帖

官网相关资源

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