解决陀螺仪读数范围小的问题

时间：2021/9/15

陀螺仪类型：

正点原子六轴陀螺仪（其他陀螺仪遇到 陀螺仪读数问题描述 中出现的问题也可使用）

陀螺仪参考图片：

陀螺仪用途：

利用陀螺仪测量小车的旋转角度。

陀螺仪读数问题描述：

将陀螺仪水平放置后(由于水平放置只改变yaw值从而测得小车水平面旋转的角度)，上电初始化，读数约为0。

向左旋转（读数增加），陀螺仪读数：
0 -> 90 -> 179 -> -179 -> -90 -> 0
向右旋转（读数减少），陀螺仪读数：
0 -> -90 -> -179 -> 179 -> 90 -> 0

变化区间：-180 -> 180
需求区间：-360 -> 360

修改后的程序：

# 算法需要的标志位
flag_1 = False
flag_2 = True
flag_4 = False
flag_5 = True
flag_7 = True
flag_8 = True
flag_9 = True
# angle为实际接收的yaw值（-180，180）
angle = 0
# angle_new为扩展范围后的yaw值（-360，360）
angle_new = 0
'''
中间省略的部分：实时接收的angle值，例如串口通信接收部分
if f407_ser.in_waiting>0:angle = f407_ser.readline()
'''
# 算法开始
angle_new = angle
if angle<-90 and flag_8:if flag_5 :flag_5 = Falseflag_4 = Trueflag_7 = False
elif angle>90 and flag_7:if flag_2 :flag_2 = Falseflag_1 = Trueflag_8 = False
elif angle>-90 and angle<90 and flag_9:flag_2 = Trueflag_5 = Trueflag_7 = Trueflag_8 = Trueflag_1 = Falseflag_4 = False
if flag_1:if angle<0:angle_new = angle+360flag_9 = Falseif angle<90 and angle>0:flag_9 = Trueif flag_4:if angle>0:angle_new = angle-360flag_9 = Falseif angle>-90 and angle<0:flag_9 = Trueprint(f"angle_new = {angle_new}")

测试程序（可直接运行，观察到效果）：

flag_1 = False
flag_2 = True
flag_4 = False
flag_5 = True
flag_7 = True
flag_8 = True
flag_9 = True
angle = 0
angle_new = 0while True:# angle从0开始累加至90,...,179,-179,...,-90,0angle = angle+1if(angle == 180):angle = -179# 算法开始angle_new = angleif angle<-90 and flag_8:if flag_5 :flag_5 = Falseflag_4 = Trueflag_7 = Falseelif angle>90 and flag_7:if flag_2 :flag_2 = Falseflag_1 = Trueflag_8 = Falseelif angle>-90 and angle<90 and flag_9:flag_2 = Trueflag_5 = Trueflag_7 = Trueflag_8 = Trueflag_1 = Falseflag_4 = Falseif flag_1:if angle<0:angle_new = angle+360flag_9 = Falseif angle<90 and angle>0:flag_9 = Trueif flag_4:if angle>0:angle_new = angle-360flag_9 = Falseif angle>-90 and angle<0:flag_9 = True# 算法结束# 当扩展后yaw值大于等于359，跳出循环if angle_new >= 359:breakprint(f"angle = {angle}\t\tangle_new = {angle_new}")

程序运行后的结果部分截图：

使用程序注意事项：

在执行程序之前，需要保证当前陀螺仪实际读数(yaw值在大于-90且小于90的区间)，或者将陀螺仪重新上电初始化。