通过stm32对mpu6050用互补滤波算法算出角度用串口输出,数据和dmp一样稳定
代码片段和文件信息
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 4419 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角APPIMUimu.c
文件 579 2016-07-05 21:01 MPU6050 互补滤波算法算出欧拉角APPIMUimu.h
文件 17273 2010-06-07 10:25 MPU6050 互补滤波算法算出欧拉角COREcore_cm3.c
文件 85714 2011-02-09 14:59 MPU6050 互补滤波算法算出欧拉角COREcore_cm3.h
文件 15503 2011-03-10 10:52 MPU6050 互补滤波算法算出欧拉角COREstartup_stm32f10x_hd.s
文件 1687 2016-07-06 09:26 MPU6050 互补滤波算法算出欧拉角HARDWAREIICiic.c
文件 855 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角HARDWAREIICiic.h
文件 291 2016-07-05 21:49 MPU6050 互补滤波算法算出欧拉角HARDWARELEDled.c
文件 211 2016-07-05 21:06 MPU6050 互补滤波算法算出欧拉角HARDWARELEDled.h
文件 3635 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角HARDWAREMPU6050mpu6050.c
文件 1387 2019-04-02 15:34 MPU6050 互补滤波算法算出欧拉角HARDWAREMPU6050mpu6050.h
文件 1423 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角HARDWARETIM3 imet.c
文件 136 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角HARDWARETIM3 imet.h
文件 399 2011-04-23 10:24 MPU6050 互补滤波算法算出欧拉角keilkilll.bat
文件 3942 2019-04-02 11:21 MPU6050 互补滤波算法算出欧拉角OBJcore_cm3.crf
文件 104 2019-04-02 11:21 MPU6050 互补滤波算法算出欧拉角OBJcore_cm3.d
文件 11328 2019-04-02 11:21 MPU6050 互补滤波算法算出欧拉角OBJcore_cm3.o
文件 349839 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJdelay.crf
文件 1710 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJdelay.d
文件 384412 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJdelay.o
文件 401976 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.axf
文件 2147 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.build_log.htm
文件 65563 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.hex
文件 117687 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.htm
文件 1182 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.lnp
文件 3657 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角OBJDEMO.tra
文件 74461 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO_DEMO.dep
文件 19 2016-07-06 09:49 MPU6050 互补滤波算法算出欧拉角OBJExtDll.iex
文件 352010 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJiic.crf
文件 1601 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJiic.d
............此处省略206个文件信息
#include “imu.h“
#include “mpu6050.h“
#include “math.h“
#define RTA 57.295780 //弧度到角度需要乘的系数(180/3.14)
#define ATR 0.0174533 //角度到弧度需要乘的系数(3.14/180)
#define Gyro_A 0.0610351f //陀螺仪初始化时量程是陀螺仪角速度+-2000度每秒转化成我们所认知的度每秒,所以1 / (65536 / 4000) = 0.06103515625f
#define Gyro_R 0.0010653f //转化成弧度每秒 即上式*ATR
#define Prepare_Period 0.001 //执行Prepare_Data函数的周期 这里设置为1ms
#define Filter_Num 20 //滤波数组长度
/*----浮点型角度和加速度值得定义,用于存放解算后的值----*/
S_float_XYZ Angle_Valu = {000};
S_float_XYZ GYRO_Valu = {000};
S_short_XYZ ACC_Avg; //平均值滤波后的ACC
S_float_XYZ GYRO_I; //陀螺仪积分
S_float_XYZ EXP_Angled; //目标角度
S_float_XYZ DIF_Angle; //目标角度与实际角度的偏差
S_float_XYZ Euler_Angle; //四元数计算出的欧拉角
int16_t ACC_X_Buffer[Filter_Num]ACC_Y_Buffer[Filter_Num]ACC_Z_Buferr[Filter_Num]; //加速度滑动窗口滤波数组
void Prepare_Data(void)
{
static uint8_t filter_cnt = 0;
int32_t temp1 = 0temp2 = 0temp3 = 0;
uint8_t i;
/*-------6050最近一次得到的值-------*/
MPU6050_ReadValu();
/*-------6050最近一次得到的值装进滤波数组-------*/
ACC_X_Buffer[filter_cnt] = ACC_Valu_First.X;
ACC_Y_Buffer[filter_cnt] = ACC_Valu_First.Y;
ACC_Z_Buferr[filter_cnt] = ACC_Valu_First.Z;
/*-------软件一阶惯性滤波-------*/
for(i=0;i {
temp1 += ACC_X_Buffer[i];
temp2 += ACC_Y_Buffer[i];
temp3 += ACC_Z_Buferr[i];
}
ACC_Avg.X = temp1 / Filter_Num;
ACC_Avg.Y = temp2 / Filter_Num;
ACC_Avg.Z = temp3 / Filter_Num;
filter_cnt++;
if(filter_cnt==Filter_Num) filter_cnt=0;
/*------角速度的积分值计算 角速度乘以周期 所得的和 即为 积分--------*/
// GYRO_I.X += GYRO_Valu_First.X*Gyro_A*Prepare_Period;
// GYRO_I.Y += GYRO_Valu_First.Y*Gyro_A*Prepare_Period;
GYRO_I.Z += GYRO_Valu_First.Z*Gyro_A*Prepare_Period;
}
/*------获取姿态-------*/
void Get_Attitude(void)
{
IMUupdate(GYRO_Valu_First.X * Gyro_R
GYRO_Valu_First.Y * Gyro_R
GYRO_Valu_First.Z * Gyro_R
ACC_Avg.XACC_Avg.YACC_Avg.Z
);
}
#define kp 10.0f //比例增益控制加速度计的收敛速度
#define ki 0.008f //积分增益控制陀螺仪偏差的收敛速度
#define halfT 0.001f //周期的一半
float q0 = 1q1 = 0q2 = 0q3 = 0; //四元数元素代表估计取向,四元素初始值
float exInt = 0 eyInt = 0ezInt = 0; //比例积分误差,初始值误差
/*------四元数解析-----*/
/*
* axayaz为在XYZ轴上的加速度
* gxgygz为在XYZ轴上的陀螺仪
*/
void IMUupdate(float gx float gy float gz float ax float ay float az)
{
float norm; //归一化变量
float vxvyvz;
float exeyez;
/*---计算平方项,节省运算时间- 其实鄙人觉得这一步并没有什么卵用,但由于参考别人的四元数算法,鄙人也没怎么搞太懂---*/
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q1q1 = q1 * q1;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
if(ax*ay*az == 0)
return;
norm = sqrt((ax * ax) + (ay * ay) + (az *az)); //加速度数据归一化
/*----计算 规范后的加速度-----*/
ax = ax / norm;
ay = ay / norm;
az = az / norm;
/*-----估计重力方向和流量/变迁----*/
vx = 2*(q1q3 - q0q2); ////四元素中xyz的表示
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
ey = (az*vx - ax*vz)
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 4419 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角APPIMUimu.c
文件 579 2016-07-05 21:01 MPU6050 互补滤波算法算出欧拉角APPIMUimu.h
文件 17273 2010-06-07 10:25 MPU6050 互补滤波算法算出欧拉角COREcore_cm3.c
文件 85714 2011-02-09 14:59 MPU6050 互补滤波算法算出欧拉角COREcore_cm3.h
文件 15503 2011-03-10 10:52 MPU6050 互补滤波算法算出欧拉角COREstartup_stm32f10x_hd.s
文件 1687 2016-07-06 09:26 MPU6050 互补滤波算法算出欧拉角HARDWAREIICiic.c
文件 855 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角HARDWAREIICiic.h
文件 291 2016-07-05 21:49 MPU6050 互补滤波算法算出欧拉角HARDWARELEDled.c
文件 211 2016-07-05 21:06 MPU6050 互补滤波算法算出欧拉角HARDWARELEDled.h
文件 3635 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角HARDWAREMPU6050mpu6050.c
文件 1387 2019-04-02 15:34 MPU6050 互补滤波算法算出欧拉角HARDWAREMPU6050mpu6050.h
文件 1423 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角HARDWARETIM3 imet.c
文件 136 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角HARDWARETIM3 imet.h
文件 399 2011-04-23 10:24 MPU6050 互补滤波算法算出欧拉角keilkilll.bat
文件 3942 2019-04-02 11:21 MPU6050 互补滤波算法算出欧拉角OBJcore_cm3.crf
文件 104 2019-04-02 11:21 MPU6050 互补滤波算法算出欧拉角OBJcore_cm3.d
文件 11328 2019-04-02 11:21 MPU6050 互补滤波算法算出欧拉角OBJcore_cm3.o
文件 349839 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJdelay.crf
文件 1710 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJdelay.d
文件 384412 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJdelay.o
文件 401976 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.axf
文件 2147 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.build_log.htm
文件 65563 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.hex
文件 117687 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.htm
文件 1182 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO.lnp
文件 3657 2016-07-06 09:58 MPU6050 互补滤波算法算出欧拉角OBJDEMO.tra
文件 74461 2019-04-02 15:33 MPU6050 互补滤波算法算出欧拉角OBJDEMO_DEMO.dep
文件 19 2016-07-06 09:49 MPU6050 互补滤波算法算出欧拉角OBJExtDll.iex
文件 352010 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJiic.crf
文件 1601 2019-04-02 11:22 MPU6050 互补滤波算法算出欧拉角OBJiic.d
............此处省略206个文件信息
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容, 请发送邮件举报,一经查实,本站将立刻删除。
评论列表(条)