你的浏览器版本过低,可能导致网站不能正常访问!为了你能正常使用网站功能,请使用这些浏览器。
意法半导体微控制器和微处理器拥有广泛的产品线,包含低成本的8位单片机和基于ARM® Cortex®-M0、M0+、M3、M4、M33、M7及A7内核并具备丰富外设选择的32位微控制器及微处理器。关注该话题即可发布文章!
问题:1,我快速改变板子倾角时Angle的变化正常吗?
2,Angle正常变化的时候是应该与Angle_ax的值相近吗?% U( f; o* A! N) Z
现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大2 E! U. Y( p, X! [; X. G$ Q2 b
+ e% Z5 j% e) E( n# C2 N
*************读取数据********************+ }7 Z1 D+ s, E, @" z) n; k: O
//定义MPU6050内部地址5 \! a# N2 T7 |5 G6 U
#define SMPLRT_DIV 0x19 //陀螺仪采样率 典型值 0X07 125Hz7 a- |- a" A% U* `' i. i# i
#define CONFIG 0x1A //低通滤波频率 典型值 0x00
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围 典型值 0x18 不自检 2000deg/s" |$ t1 z% o* u, W0 H
#define ACCEL_CONFIG 0x1C //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG 0x37( i& [. `5 Z+ l$ s
#define INT_ENABLE 0x38; |) ~# Q) H" _$ N9 K1 Q( R# b
#define INT_STATUS 0x3A //只读# i6 P ?2 o6 G3 e1 r
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C0 i* u1 u+ A" Z. X; E4 a0 i
#define ACCEL_YOUT_H 0x3D$ d$ ~6 [3 I- N' ], c G4 i4 [) f
#define ACCEL_YOUT_L 0x3E c- z7 \5 O. S c0 _5 ?. |7 R- h( R, D
#define ACCEL_ZOUT_H 0x3F R" ^/ ^: P h2 K4 q
#define ACCEL_ZOUT_L 0x406 B9 J/ ~& L. R( \6 \9 @( E
#define TEMP_OUT_H 0x412 y" ?5 Q0 _! ^% T7 o2 j9 V
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x455 m& v4 D; T" K1 r; r# Y( [
#define GYRO_YOUT_L 0x46+ F$ p; ?# C/ e0 u) j( v5 Z
#define GYRO_ZOUT_H 0x47: a: [* a& P& j: |( t E% ^# Z
#define GYRO_ZOUT_L 0x48
5 h4 @8 Q! q' s! c4 N! [
//读取寄存器原生数据
MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);- n4 t1 }" }1 P. z7 Q6 }, B
MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);. ]# a% I# Q$ I! B" a4 Y2 o
MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
MPU6050_Raw_Data.Temp = (buf[6]<<8 | buf[7]); 8 b$ I+ J" u' {
MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
& d' m! L5 U9 j9 I% @; n$ ? r' ]: x
! l4 K6 k/ r- z Z* c1 n
//将原生数据转换为实际值,计算公式跟寄存器的配置有关
MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0; " O ?4 Z; w9 a, K! f: R
MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0; - G6 f! b- K- A8 S
MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5; 2 r1 A, q7 A( M4 ]. {, Y6 Q/ w* ^! }
MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;
MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5; % h' q7 |3 s% g/ T
}
4 D! k- x U; y
//******卡尔曼参数************
const float Q_angle=0.001;
const float Q_gyro=0.003;
const float R_angle=0.5;% R$ i) B* @4 }
const float dt=0.01; //dt为kalman滤波器采样时间;
const char C_0 = 1;! k+ E' W" X, m) N' T( n& N1 a
float Q_bias, Angle_err;, y% }" }& n% }# L1 h% T g2 q
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;1 R7 {. [ ~8 s3 g
float Pdot[4] ={0,0,0,0}; x9 G: t" B+ a- b+ Y# n
float PP[2][2] = { { 1, 0 },{ 0, 1 } };/ \6 N1 v9 y. F
/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)
{& h- g& F# b0 R+ t9 N2 ]! O
Angle+=(Gyro - Q_bias) * dt; //先验估计( w) a# {$ j9 f; m
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分' [9 s9 @9 }, F7 V! v6 R
# r9 i" |( |+ i7 l$ a K
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;, E4 f5 |5 w8 R
; P" d/ s0 i- u6 v, j
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;' X0 W: ^* e( D' a3 f
5 I9 |3 t# r$ F4 C6 E6 J9 X/ U
Angle_err = Accel - Angle; //zk-先验估计. a: R2 a; p% G+ z% Q( f
8 C6 |( n: X4 @2 U7 U5 } h
PCt_0 = C_0 * PP[0][0];- @9 z& `+ ?9 i7 ^
PCt_1 = C_0 * PP[1][0];
B# D0 ?' z8 g [0 z2 s1 t0 d2 @+ ]
E = R_angle + C_0 * PCt_0;
7 v1 b5 U5 n0 l ]+ @9 \8 p
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;7 o. a9 @+ N1 g7 W$ G8 D2 N& D( ^
0 W4 ^( ?* L2 f! W
t_0 = PCt_0;& ?" ^# G, H3 U3 `. }6 J( S
t_1 = C_0 * PP[0][1];
: F6 i( l+ g" v# H) z% E4 f
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;' a: O2 p# o; X
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
7 K E' T% e' O, L
Angle += K_0 * Angle_err; //后验估计8 y7 `- r. }2 ]$ q: E$ O4 _
Q_bias += K_1 * Angle_err; //后验估计* v% R" t. R6 {# ~
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度, E0 e- ]. N$ K. t+ x8 P
0 h3 _/ c4 \3 X. L
}
, C. ~; K& v5 D0 O: |' D$ ^- X
******************倾角计算****************** g% c S0 U0 T- q: O$ ]9 b7 L, d
void Angle_Calculate(void)
{
# `! p d% X/ R( o9 A
/****************************加速度****************************************/
8 T' I0 A. ?7 g; v1 ?3 b
Accel_x = MPU6050_Real_Data.Accel_X; //读取X轴加速度
Angle_ax = Accel_x*1.2*180/3.14; //弧度转换为度
0 e+ g- q2 B& C- |0 T# N5 `- l$ _
/****************************角速度****************************************/
Gyro_y = MPU6050_Real_Data.Gyro_Y; / e( l' Q' v' q6 R
时间dt,所以此处不用积分+ d$ e6 S8 s' @ H: u5 m
/***************************卡尔曼融合*************************************/
Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角) N2 ?8 d. F1 w$ f. e' Q# }5 b& _
8 I5 j2 ?# _3 z
0 a& V. M# D$ j% r
楼主这么强,学习一下