ENC03和adxl330
焊接完三片ENC03+ADXL330之后该怎么利用数据的输出?软件该怎么写才能得到倾斜度数据? 靜止的時候,加速規可以直接量傾斜角。自己的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.0mV/°/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_SCALE0.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,所以沒有用定點去做。 我想要测量是动态时候的倾斜度,嗯,还是谢谢"g921002"提供的静态方法,高手来帮帮我吧 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運作前,我們不知道飛機姿態,必須在「靜止」狀態下用加速規先
抓一段時間的姿態資料算出初值,因為用陀螺解算姿態是需要姿態初值的。
另外算式哪裡不明白?麻煩您發個聲...都是課本的東西,如果一一解說滿累的^_^。 不好意思最近刚出差回来, 接着话题
书的名字是什么? 麻烦 g921002告诉我一下 ,我自己去研究一下 最近还是没什么头绪用enc03+adxl330测这个倾斜角 MARK
页:
[1]