专家
公告
财富商城
电子网
旗下网站
首页
问题库
专栏
标签库
话题
专家
NEW
门户
发布
提问题
发文章
STM32
谁用过HMC5883L这个传感器?对MPU6050的yaw(航向角)进行校正?
2019-08-23 16:58
发布
×
打开微信“扫一扫”,打开网页后点击屏幕右上角分享按钮
站内问答
/
STM32/STM8
8924
49
1522
我用的是MPU6050的DMP,现在roll和pitch都很准,就是yaw不准,据说加速度计不能对yaw进行校正,这是硬件的缺陷,听说磁阻传感器HMC5883L可以对yaw(航向角进行校正),有哪位大神已经校正过了呀?能不能分享一下,指点一下呀…………谢啦
友情提示:
此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
49条回答
阿拓
2019-08-26 11:10
用AHRS算法,融合HMC5883L的三轴磁通和mpu6050的加速度、角速率。
---------------------------------
#define Kp 2.0f // 比例增益支配收敛率accelerometer/magnetometer
#define Ki 0.005f // 积分增益执政速率陀螺仪的衔接gyroscopeases
#define halfT 0.5f // 采样周期的一半
//---------------------------------------------------------------------------------------------------
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素,代表估计
方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
//====================================================================================================
// Function
//====================================================================================================
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// 辅助变量,以减少重复操作数
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// 计算参考磁通方向
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
//估计方向的重力和磁通(V和W)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// 错误是跨产品的总和之间的参考方向的领域和方向测量传感器
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// 积分误差比例积分增益
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// 调整后的陀螺仪测量
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 整合四元数率和正常化
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
/*输入参数为加速度xyz原始数据,磁力计xyz原始数据,陀螺仪数据必须转换为弧度制*/
加载中...
查看其它49个回答
一周热门
更多
>
相关问题
STM32F4上I2C(在PROTEUS中模拟)调试不通的问题
6 个回答
芯片供应紧张,准备换个MCU,MM32L系列替换STM32L系列的怎么样?
7 个回答
STM32同时使用两个串口进行数据收发时数据丢包的问题
5 个回答
STM32F103串口通信死机问题
4 个回答
STM32WLE5CC连接SX1268在LoRa模式下能与 SX1278互通吗?
2 个回答
相关文章
ST公司第一款无线低功耗单片机模块有效提高物联网设计生产效率
0个评论
如何实现对单片机寄存器的访问
0个评论
通过USB用STM32片内自带Bootloader下载程序及注意事项
0个评论
欲练此功必先自宫之STM32汇编启动,放慢是为了更好的前行
0个评论
×
关闭
采纳回答
向帮助了您的知道网友说句感谢的话吧!
非常感谢!
确 认
×
关闭
编辑标签
最多设置5个标签!
STM32
保存
关闭
×
关闭
举报内容
检举类型
检举内容
检举用户
检举原因
广告推广
恶意灌水
回答内容与提问无关
抄袭答案
其他
检举说明(必填)
提交
关闭
×
打开微信“扫一扫”,打开网页后点击屏幕右上角分享按钮
×
付费偷看金额在0.1-10元之间
确定
×
关闭
您已邀请
0
人回答
查看邀请
擅长该话题的人
回答过该话题的人
我关注的人
---------------------------------
#define Kp 2.0f // 比例增益支配收敛率accelerometer/magnetometer
#define Ki 0.005f // 积分增益执政速率陀螺仪的衔接gyroscopeases
#define halfT 0.5f // 采样周期的一半
//---------------------------------------------------------------------------------------------------
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素,代表估计
方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
//====================================================================================================
// Function
//====================================================================================================
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// 辅助变量,以减少重复操作数
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// 计算参考磁通方向
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
//估计方向的重力和磁通(V和W)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// 错误是跨产品的总和之间的参考方向的领域和方向测量传感器
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// 积分误差比例积分增益
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// 调整后的陀螺仪测量
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 整合四元数率和正常化
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
/*输入参数为加速度xyz原始数据,磁力计xyz原始数据,陀螺仪数据必须转换为弧度制*/
一周热门 更多>