|
发表于 2010-2-1 19:55:04
|
显示全部楼层
靜止的時候,加速規可以直接量傾斜角。自己的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,所以沒有用定點去做。 |
|