新手入门
- 积分
- 16
- 金钱
- 16
- 注册时间
- 2016-9-6
- 在线时间
- 2 小时
|
if (sensors & INV_WXYZ_QUAT && hal.report & PRINT_QUAT)
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch *57.3(180.0/3.14)是将弧度转为更直观的角度
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
}
|
|