MPU9250/6050 偏航角归零算法

我不是电工

已于 2023-12-11 13:22:51 修改

阅读量312

点赞数 3

文章标签：算法

于 2023-06-26 22:45:48 首次发布

本文链接：https://blog.csdn.net/weixin_62775618/article/details/131406214

版权

/*******************************************************

该算法是由本人在一个项目中突发奇想的，最后经过实践，验证了该算法的可行性。

在改项目中需要在旋转一定角度后，需要将旋转后的角度方向重新设为偏航角的零点，

下面以旋转 0度，90度，180度，-90度，-180度为例。

*********************************************************/

float yaw;实际的偏航角

float yaw_next;//零点更新后得到的偏航角

float Target_yaw;//相对于起始坐标的旋转角度

1，旋转角度变换

 if(yaw<0)
 {
   yaw+=360;
 }

将Target_yaw的范围从-180~180变换为 0~360

2坐标变换

          if(Target_yaw==90)
            {
              if(yaw<=180 && yaw>=-90)
              {
                yaw_next=yaw-90;
              }
              else if(yaw >=-180 && yaw<=-90)
              {
                yaw_next=yaw+270;
              }             
            }
            else if(Target_yaw==270)
            {
              if(yaw<=90 && yaw>=-180)
              {
                yaw_next=yaw+90;
              } 
              else if(yaw<=90 && yaw>=-180)
              {
                yaw_next=yaw;
              }
            }

            else if(Target_yaw==180)
            {
              if(yaw>=0 && yaw<=180)
              {
                yaw_next=yaw-180;
              }
              else if(yaw>=-180 && yaw<=0)
              {
                yaw_next=yaw+180;
              }
            }
            else if(Target_yaw==0)
            {
              yaw_next=yaw;
            }

yaw_next为更新后的偏航角，是Target_yaw的角度为零点。可以理解为是旋转后的新坐标。