解决陀螺仪偏航角180不连续的一种巧妙方法，实现记录无限角

问题：当陀螺仪偏航角超过180度时，偏航角会突然改变(从180度变成-180度)，造成角度的不连续性。

解决：利用角度跳变的差值，判断偏航角突然改变，再通过对角度实行变换，使得角度连续。

首先，如何判断偏航角突变？

我们可以先设置一个全局变量，这个变量用来存储当前偏航角的值，这样，当程序再次进行到这个函数时，这个变量就变成了上一次偏航角的值，然后，我们再用当前偏航角的值与上一次偏航角的值作差，在一般情况下，这个差不会超过180，但是，当偏航角从180到-180跳变时，这个差就会超过180，我们就可以利用这个特性来判断偏航角突变。

那么，如何对角度实行变换，使角度连续？

这里我们再设置一个全局变量，用来记录角度突变的次数，我们要使这个变量在偏航角从180突变到-180时+1，偏航角从-180突变到180时-1，此时，我们就可以继续对偏航角变化了。我们需要在偏航角从180突变到-180后，将偏航角-180到180度的值映射成180到540。做法是将偏航角加360，式子如下：

变化后的角度=当前偏航角的值+360*偏航角突变次数。

为什么需要再乘以一个偏航角突变次数？因为当未变换角度的偏航角再一次从180突变到-180时，我们可以将未变换角度的偏航角的-180到180映射成540到720，实现无限角度的记录（实际有上限，要考虑变量的大小)。

附上代码:

float InfiniteYaw(float Now_Yaw)
{

	static int flag=0;//记录偏航角突变次数
	static float Last_Yaw;
	
	if(Now_Yaw-Last_Yaw<-180)//如果存在角度突变
	{
		flag++;
	}
	else if(Now_Yaw-Last_Yaw>180)
	{
		flag--;
	}

    Last_Yaw = Now_Yaw;
    return Now_Yaw+ flag*360;
}

测试：

顺时针第一次突变

 

顺时针第二次突变 

逆时针第一次突变

逆时针第二次突变

 

 最后说明一下代码可能存在的bug：

当偏航角变化在一次计算周期（角度计算周期大多在10ms以内，我测试时是1ms）里突变180，会存在突变记录丢失，这是什么概念，人眼的反应时间大约在16至20毫秒之间，也就是说，陀螺在你没反应过来的时间瞬间掉了一个头，除非把陀螺仪绑在电机上，不然达不到这个速度。

解决办法就是减小计算周期，不过陀螺仪的测量角速度也有上限，一般陀螺仪角速度测量最大速度为2000度每秒，出现bug的角速度为180度/10ms=18000度/s。所以，在程序出bug前，陀螺仪测量会先超量程。