Robofun 機器人論壇

標題: 三軸加速規靜止時頃角計算方法in C [打印本頁]

作者: g921002 時間: 2010-2-16 14:00
標題: 三軸加速規靜止時頃角計算方法in C
說明：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 時間: 2010-2-18 04:58
這裡面我對C語言比較有興趣
請教幾個問題
struct ATTITUDE *_att 這樣的指令在做什麼用的？
._ 是MCU的小手段？何種小手段？
C語言有"->" 這樣的指令嗎？
不好意思，我是C語言菜鳥

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

作者: doubletime 時間: 2010-2-18 13:55
mzw2008大大方便建議一下c語的書嗎？
我看到的結構只有教怎麼使用結構變數而已

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

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

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

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

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

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

作者: g921002 時間: 2010-2-19 00:53
本帖最後由 g921002 於 2010-2-19 00:55 編輯

沒有講得很清楚....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 時間: 2010-2-19 02:02
>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 時間: 2011-3-13 01:01
可以介紹一下你用的是哪顆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 時間: 2011-3-14 09:06

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

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

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

作者: zhongguoniu99 時間: 2011-7-17 21:34
回復 1# g921002

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

作者: nichal 時間: 2011-7-18 13:05
這樣的程式碼一放進8bits MCU馬上爆增數KB程式空間
所以這時就非得選用高階一點的 mcu了

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

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)