ENC03和adxl330

sniper_bing

焊接完三片ENC03+ADXL330之后该怎么利用数据的输出?软件该怎么写才能得到倾斜度数据?

g921002

靜止的時候，加速規可以直接量傾斜角。自己的Code，你可以參考一下。可以量到機體180度反轉的狀況。
ax,ay,az是加速規輸出，軸向請依您的實際狀況調整。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);       
}
gyro的部分可以用一般慣導書上的公式，我的Code如下：
float get_attitude_from_gyro(struct ATTITUDE *_att, int heading,int gx,int gy,int gz)
{
        //For IDG500         2.0  mV/°/s sensitivity @ X,Y axis, ZERO-RATE OUTPUT 1.35V
        //For MLX90609        6.67 mV/°/s sensitivity @ Z axis  , ZERO-RATE OUTPUT 2.5V
        //1 LSB is 5/1024 = 4.8828125 mv
        //gyro rate = gx..y * 5000mv /1024 /2.0 = 2.44140625
        //          = gz * 5000mv /1024 /6.67   = 0.73205584707646176911544227886057
       
        #define GXY_SCALE 2.44140625
        #define GZ_SCALE  0.73205584707646176911544227886057
        float _p,_q,_r;
        float _Sr,_Cr,_Sp,_Cp;
       
        _Sr = cal_sin(_att->roll._);         _Cr = cal_cos(_att->roll._);
        _Sp = cal_sin(_att->pitch._);        _Cp = cal_cos(_att->pitch._);
        _p = gx*GXY_SCALE; _q = gy*GXY_SCALE; _r = gz*GZ_SCALE ;
       
        _att->roll._  = _att->roll._+(_p + (_q*_Sr + _r*_Cr)*(_Sp/_Cp))/100;
        _att->pitch._ = _att->pitch._+(_q*_Cr - _r*_Sp)/100;
       
        heading += (_q*_Sr + _r*_Cr)/_Cp/100;
        return heading;
       

}
這裡我沒有用四元素，是因為pitch超過45度時，姿態估測有其他的排程在做，所以這裡不考慮pitch=90度的狀況。然後後面姿態是用Kalman去做，這個一般書上都有我就不多說了。
程式碼是C30（For dsPIC)去寫的，（反）三角函數試用查表去近似出來的，因為效果還OK，所以沒有用定點去做。

sniper_bing

我想要测量是动态时候的倾斜度,嗯,还是谢谢"g921002"提供的静态方法,高手来帮帮我吧

sniper_bing

g921002,关于你的算式,可以给我说一下原理吗?我不是很明白.

yangyh75

留个脚印

g921002

回复【2楼】sniper_bing  
我想要测量是动态时候的倾斜度,嗯,还是谢谢"g921002"提供的静态方法,高手来帮帮我吧  
-----------------------------------------------------------------------

上面兩個function是我用在UAV上面的，這兩個function輸入都有一個將載體現在的姿態角(pitch和roll)納入。在
飛機有外力介入時get_attitude_from_accel是與真實姿態有誤差的，return值是用來偵測外力存在與否。

float get_attitude_from_gyro(struct ATTITUDE *_att, int heading,int gx,int gy,int gz) 是根據前一步估
測的姿態計算尤拉角變量，然後更新Gyro姿態估測值。

好了，我們有加速規和陀螺儀兩個來源估測的姿態，事前我也知道兩者的Sensor的雜訊協方差Q和R，唯一要做的是
考慮將外力影響補償到Kalman filter裡面，最後得到兩著更新的姿態估測。因為飛機狀態比較複雜，不能用固定的Q、R
值去估測。

為何要說靜止狀態的狀況，因為在Kalman filter運作前，我們不知道飛機姿態，必須在「靜止」狀態下用加速規先
抓一段時間的姿態資料算出初值，因為用陀螺解算姿態是需要姿態初值的。

另外算式哪裡不明白？麻煩您發個聲...都是課本的東西，如果一一解說滿累的^_^。

sniper_bing

不好意思最近刚出差回来, 接着话题
书的名字是什么? 麻烦 g921002  告诉我一下 ,我自己去研究一下 最近还是没什么头绪用enc03+adxl330测这个倾斜角

Joyliz

MARK