簡易的電子羅盤校正方法

john800422

/*

簡易的電子羅盤校正方法
只是一個簡單的概念, 離應用還有差距

方法:
1. 旋轉SENSOR, 並分別記錄X軸與Y軸的最大最小值( MEG_Max & MEG_Min )
2. 將兩軸做 (MEG_Max + MEG_Min)/2, 求得理想上的圓心位置, 並將球心位置記錄至Meg.OffsetX/Y裡
3. 將球心校正至座標軸中心(0, 0), 也就是 Meg.X/Y -= Meg.OffsetX/Y;
4. 使用 atan2 即可算出方向角度

*/

#define testTimes 4000 // 至少足夠轉一圈的時間

float Sita = 0.0f;
static u8 SensorMode = Mode_Correction;
static u16 CorrectionTimes = 0;

Meg.X = Meg.X - Meg.OffsetX;
Meg.Y = Meg.Y - Meg.OffsetY;

switch(SensorMode) {
        case Mode_Correction:
                if(CorrectionTimes < testTimes) {
                        if(Meg.X > Meg.MaxX)
                        Meg.MaxX = Meg.X;
                        if(Meg.X < Meg.MinX)
                        Meg.MinX = Meg.X;
                        if(Meg.Y > Meg.MaxY)
                        Meg.MaxY = Meg.Y;
                        if(Meg.Y < Meg.MinY)
                        Meg.MinY = Meg.Y;

                        CorrectionTimes++;
                }
                else if(CorrectionTimes == testTimes) {
                        Meg.OffsetX = (MEG_Max[0] + MEG_Min[0])/2;        //  這之前 OffsetX 都為 0
                        Meg.OffsetY = (MEG_Max[1] + MEG_Min[1])/2;        //  這之前 OffsetY 都為 0

                        CorrectionTimes++;
                }
                else {
                        Correction_Time = 0;
                        SensorMode = Mode_Algorithm;
                }
                break;
        case Mode_Algorithm:
                Sita = atan2f(Meg.Y, Meg.X);
                if(Sita<0.0f)
                        Sita = Sita + 360.0f;
                break;
}

這個方法必須每次啟動時都校正
若是預先計錄下來, 換地方後就不靠譜了
不知道有什麼方法可以解決每次啟動時都需要重新校正的問題

roothoo

谢谢分享，这是最简单的校准方法了

mcucow

赞一个
如果在手机上使用  不太现实哦

Andre.Gorz

思路确实很简单～

jlian168

Use eeprom.

yue621

好贴！！帮顶~！！！！！！！！！！！！！！！

silence2455

楼主大哥，看得出来，你是大神。请问你一下，如果我测出了准确的Pitch和Roll,是不是可以通过Yaw=atan2(mx*cos(Pitch) + my*sin(Pitch)*sin(Roll) - mz*sin(Pitch)*cos(Roll),my*cos(Roll) + mz*sin(Roll))* 57.3这个公式用磁力计算出Yaw？？
但是我这样算出的Yaw不正常，我转动的时候Yaw的变化是大幅度地跳着前进，请问这是为什么？   所以我用IMUupdate这个算法的时候做互补滤波不能完全消除Yaw的漂移。
但是我用Yaw=atan2(init_my,init_mx)这个公式时，在XY平面内转动，Yaw各种正常。
求教啊大神，陷入困境了。

小号

silence2455 发表于 2014-3-28 11:35
楼主大哥，看得出来，你是大神。请问你一下，如果我测出了准确的Pitch和Roll,是不是可以通过Yaw=atan2(mx*c ...

你的跳动具体是多大，磁力计噪音是很大的，跳动很正常，要和陀螺做融合才行

silence2455

小号 发表于 2014-3-28 11:39
你的跳动具体是多大，磁力计噪音是很大的，跳动很正常，要和陀螺做融合才行 ...

兄台，我开始用IMUupdate这个算法，只融合陀螺仪和加速度计得到四元数，解算出的Pitch和Roll都很好，但是Yaw一秒飘一度，这应该是正常的吧。然后我想通过Yaw   = -(0.985 * (-yaw+gz*2*halfT*57.3) + 57.3*0.015* atan2(mx*cos(Roll) + my*sin(Roll)*sin(Pitch) + mz*sin(Roll)*cos(Pitch), my*cos(Pitch) - mz*sin(Pitch)))这个公式做互补滤波消除漂移，但是不行，几秒钟飘一度。 atan2(mx*cos(Roll) + my*sin(Roll)*sin(Pitch) + mz*sin(Roll)*cos(Pitch), my*cos(Pitch) - mz*sin(Pitch))这个公式在这里不能解算出正确的Yaw。它是跳着跑的，幅度很大，请问一下这是为什么？

zhang3lu

很有用，学习学习

w12315

值得学习