|
/*
簡易的電子羅盤校正方法
只是一個簡單的概念, 離應用還有差距
方法:
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;
}
這個方法必須每次啟動時都校正
若是預先計錄下來, 換地方後就不靠譜了
不知道有什麼方法可以解決每次啟動時都需要重新校正的問題 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|