三軸加速規靜止時頃角計算方法in C

g921002

說明：ATTITUDE是struct宣告，裡面放roll和pitch的資料，後面._是因為用在MCU上的小手段，可以當作沒看見。ax,ay,az是加速規輸出，用的時候注意一下XY軸向即可。return 值當作沒看見就好...

float get_attitude_from_accel(struct ATTITUDE *_att,int ax,int ay,int az)
{
        _att->roll._  = 180.0*atan(ay/sqrt((float)(ax*ax+az*az)))/PI;       
        _att->pitch._ = -180.0*atan(ax/sqrt((float)(az*az+ay*ay)))/PI;
        if(az<0){
                if   (ay>0){_att->roll._  = 180.0 -_att->roll._;}
                else {_att->roll._  = -(_att->roll._+180.0);}
        }
        return (3969.0-(ax*ax+ay*ay+az*az)/10.0);       
}

g921002

沒有講得很清楚....XD(有點懶..)
ATTITUDE是struct，宣告如下:
struct ATTITUDE{
        union f2bbbb roll;
        union f2bbbb pitch;
};

roll和pitch宣告為 union 結構
union f2bbbb
{
        float _;
        struct
        {
                char b1;
                char b2;
                char b3;
                char b4;
        };
};
所以要取出roll,或pitch浮點值就用roll._取值。為何要這樣用？因為roll和pitch用UART傳到電腦時要把浮點數的四個byte值(b1-b4)依序傳輸。我在用UART傳資料時，基本上不用ASCII模式，都是用binary模式傳輸。所以基本上看官只要注意數學的方程式就好，那些小技巧的東西其實不是那麼重要。

g921002

>return值怎麼代表方向呢?
>這看起來只有一個數值輸出,但方向性表示在哪裡?
>用個別的正負號嗎?

姿態資料是用傳址方式傳入，當姿態值改變時就改變了，不需要return出來，return值當作沒看見就好，或喜歡的話，函式實作成以下形式即可：
struct ATTITUDE get_attitude_from_accel(int ax,int ay,int az)
{
        struct ATTITUDE _att;
        _att.roll._  = 180.0*atan(ay/sqrt((float)(ax*ax+az*az)))/PI;      
        _att.pitch._ = -180.0*atan(ax/sqrt((float)(az*az+ay*ay)))/PI;
        if(az<0){
                if   (ay>0){_att.roll._  = 180.0 -_att.roll._;}
                else {_att.roll._  = -(_att.roll._+180.0);}
        }
        return _att;        
}

g921002

可以介紹一下你用的是哪顆G-SENSOR嗎???另外我想瞭解更多有關於公式轉換的細節
可以提供加速度計方面的資訊 ...
tomhwang2001 發表於 2011-3-13 01:01

我是用ADXL350
手冊上會寫1g=?mv，你從ADC得到的值轉成電壓，再從電壓轉成g值。手算完後就知道你的scaler facter是多少了。
另外sinθ=θ，單位要用弳度，小角度近似才能成立，儘量用弳度為單位計算。

用加速度計量姿態，你的載具的dV(速度變化)要接近0(等速運動)，如果你的車子是基於這種假設下，那就沒問題。