OpenEdv-开源电子网

 找回密码
 立即注册
正点原子全套STM32/Linux/FPGA开发资料,上千讲STM32视频教程免费下载...
查看: 2306|回复: 2

四轴AHRS算法九轴拟合,直接读得的磁力计数据怎么化为直接能代入的数据。

[复制链接]

24

主题

63

帖子

0

精华

初级会员

Rank: 2

积分
52
金钱
52
注册时间
2018-1-19
在线时间
50 小时
发表于 2018-3-31 11:15:39 | 显示全部楼层 |阅读模式
3金钱
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;
            // auxiliary variables to reduce number of repeated operations
            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;         
           
            // normalise the measurements
            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;         
           
            // compute reference direction of flux
            hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);         
            bx = sqrt((hx*hx) + (hy*hy));
            bz = hz;        
           
            // estimated direction of gravity and flux (v and w)
            vx = 2*(q1q3 - q0q2);
            vy = 2*(q0q1 + q2q3);
            vz = q0q0 - q1q1 - q2q2 + q3q3;
            wx = 2*bx*(0.5f - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
            wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
            wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5f - q1q1 - q2q2);  
           
            // error is sum of cross product between reference direction of fields and direction measured by sensors
            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);
           
            // integral error scaled integral gain
            exInt = exInt + ex*Ki;
            eyInt = eyInt + ey*Ki;
            ezInt = ezInt + ez*Ki;
           
            // adjusted gyroscope measurements
            gx = gx + Kp*ex + exInt;
            gy = gy + Kp*ey + eyInt;
            gz = gz + Kp*ez + ezInt;
           
            // integrate quaternion rate and normalise
            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;  
           
            // normalise quaternion
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
            q0 = q0 / norm;
            q1 = q1 / norm;
            q2 = q2 / norm;
            q3 = q3 / norm;
       yaw1 = atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1)*57.3; // yaw  unit:degree
            pitch1 = asin(-2*q1*q3+2*q0*q2)*57.3; // pitch  
            roll1= atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*57.3; // roll
}

最佳答案

查看完整内容[请看2#楼]

例如mpu6050角速度满量程为正负2000°/s,那么角速度读取得到的原始数据gx,代入时gx=gx/(65536/4000*57.3),那么磁力计数据要如何处理一下
正点原子逻辑分析仪DL16劲爆上市
回复

使用道具 举报

24

主题

63

帖子

0

精华

初级会员

Rank: 2

积分
52
金钱
52
注册时间
2018-1-19
在线时间
50 小时
 楼主| 发表于 2018-3-31 11:15:40 | 显示全部楼层
例如mpu6050角速度满量程为正负2000°/s,那么角速度读取得到的原始数据gx,代入时gx=gx/(65536/4000*57.3),那么磁力计数据要如何处理一下
回复

使用道具 举报

530

主题

11万

帖子

34

精华

管理员

Rank: 12Rank: 12Rank: 12

积分
165536
金钱
165536
注册时间
2010-12-1
在线时间
2117 小时
发表于 2018-4-1 01:18:03 | 显示全部楼层
帮顶
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则



关闭

原子哥极力推荐上一条 /2 下一条

正点原子公众号

QQ|手机版|OpenEdv-开源电子网 ( 粤ICP备12000418号-1 )

GMT+8, 2025-6-11 01:18

Powered by OpenEdv-开源电子网

© 2001-2030 OpenEdv-开源电子网

快速回复 返回顶部 返回列表