在平面的时候Yaw = tan(my/mx)。当存在Roll 和 Pitch时,只要把三维的数值分解到平面就可以。公式如下:
yh = my * cos(Roll) + mz * sin(Roll);
xh = mx*cos(Pitch) + my*sin(Roll)*sin(Pitch) - mz * cos(Roll)*sin(Pitch)
tan(Yaw)=(yh/xh)
Yaw = arctan(yh/xh)。
公式就是这样。但是MPU6050里面的DMP没有磁力计的修正,YAW是不能用的,会很漂
|
|