三軸加速規靜止時頃角計算方法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);       
}

doubletime

這裡面我對C語言比較有興趣
請教幾個問題
struct ATTITUDE *_att       這樣的指令在做什麼用的？
._  是MCU的小手段？   何種小手段？
C語言有"->" 這樣的指令嗎？
不好意思，我是C語言菜鳥

mzw2008

那個叫做結構
裡面那個*叫做"指標"
"->"是用來操作結構指標用的東西
這都是C語言很基本的東西哩
所有的C語言書幾乎都會提到喔

doubletime

mzw2008大大方便建議一下c語的書嗎？
我看到的結構只有教怎麼使用結構變數而已

另外的是我不懂的是
struct ATTITUDE *_att      
這樣的用法我沒有用過
我不會指標和結構共同存在
不知道有沒有相關範例可以參考？
謝謝

mzw2008

書只要去市面上的書局找, 應該是整排的
隨便挑自己喜歡的囉
C語言的書硬要說哪本好, 我覺得這是基礎的書, 隨便看就好哩
不然去買漫畫好了, 市面有C語言的漫畫, 但好像絕版了, 可去誠品問

struct ATTITUDE *_att 是一個指標
標準來講可以這樣用===>
struct ATTITUDE  aaa;
....如此這般...設定aaa的成員以後...
使用_att =&aaa;   就能把指標指向結構
接下來就能使用
_att -> xxx 之類的方法來存取結構的內容
這應該很簡單才對哩

結構會了才有辦法學後面的資料結構和演算法和物件導向...
所以這算是很基礎的入門功夫喔
另外網路搜尋結構指標應該也可以找到很多的範例

mac1

結構和指標才是C語言的重點阿
當宣告為 struct aa;
則要用到裡面的成員時,就是 aa.xxx;
當宣告城 struct *aa;
則要用到裡面的成員時,就是 aa->xxx;
差別在於星號要使用箭頭,用的是指標方式取數值,不是直接找值出來用
接著說來話長阿...長話短說就是這樣的意思啦

市面上的C語言書籍內容都長的很像
不如挑自己看的懂得來學,而且重點要放在指標和結構上面
這方面在資料結構書籍講的比較多...
----------------------------------------
至於 roll._的用法到是沒見過
難道原形是 float _;這樣宣告嗎?
多一層宣告不是多此一舉嗎?
---------------------------------------------
return值怎麼代表方向呢?
這看起來只有一個數值輸出,但方向性表示在哪裡?
用個別的正負號嗎?

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;        
}

tomhwang2001

可以介紹一下你用的是哪顆G-SENSOR嗎???另外我想瞭解更多有關於公式轉換的細節
可以提供加速度計方面的資訊或網站嗎?
我有在網路上找到片面的資訊如下:
1.
Ax=(Vx-Voff)/(Vm/1g)
角度=arcsin(Ax/1g)
2.
VOUT=Voff+(△V / △G)×1.0G×Sinθ
    式中：VOUT-加速計輸出，Voff-零加速計值，△V/△G-靈敏度，1.0G-地球引力，θ-擺動角。
θ=Sin -1X=X+(X3 / 6)+(1×3X5 / 2×45)+(1×3×5X7 / 2×4×6)+……
    對於擺動角小於10°，可用下面近似式：sinθ=θ
另外加速度計的加速度要如何轉換???由電壓變化得知嗎???如果是在移動載具上傾角及加速度誤差影響大嗎?
謝謝~

g921002

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

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

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

zhongguoniu99

回復 1# g921002


   你好  請問一下你寫這段代碼基於的芯片 是怎麼放置的呢    可以測出三個軸向的傾角？  謝謝

這樣的程式碼一放進8bits MCU馬上爆增數KB程式空間
所以這時就非得選用高階一點的 mcu了

基本上
我的程式大多數還停留在unsigned char與unsigned int的運算中
這種幾乎是PC等級的運算式果然只能看看而已....Orz...