|
|
原子的程序可以通過四元數(shù)的方式求出機(jī)體的俯仰角/橫滾角/航向角
/*
q0 = quat[0] / q30; //q30格式轉(zhuǎn)換為浮點數(shù)
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//計算得到俯仰角/橫滾角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch反正弦
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll反正切
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw反正切
*/
但是與自然軸的角度 怎么去求呢？
若使用下方的程序是需要每次使用前手動校準(zhǔn)嗎？還是通過四元數(shù)可以求出mpu6050的Z軸與重力的角度？
/*
short MPU6050_Get_Angle(float x,float y,float z,u8 dir)
{
float temp;
float res=0;
switch(dir)
{
case 0://與自然Z軸的角度
temp=sqrt((x*x+y*y))/z;
res=atan(temp);
break;
case 1://與自然X軸的角度
temp=x/sqrt((y*y+z*z));
res=atan(temp);
break;
case 2://與自然Y軸的角度
temp=y/sqrt((x*x+z*z));
res=atan(temp);
break;
}
return res*1800/3.14;//把弧度轉(zhuǎn)換成角度
}
*/
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
