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

时间：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 = False
        flag_4 = True
        flag_7 = False
elif angle>90 and flag_7:
    if flag_2 :
        flag_2 = False
        flag_1 = True
        flag_8 = False
elif angle>-90 and angle<90 and flag_9:
    flag_2 = True
    flag_5 = True
    flag_7 = True
    flag_8 = True

    flag_1 = False
    flag_4 = False
if flag_1:
    if angle<0:
        angle_new = angle+360
        flag_9 = False
    if angle<90 and angle>0:
        flag_9 = True

if flag_4:
    if angle>0:
        angle_new = angle-360
        flag_9 = False
    if angle>-90 and angle<0:
        flag_9 = True

print(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 = 0

while True:
    # angle从0开始累加至90,...,179,-179,...,-90,0
    angle = angle+1
    if(angle == 180):
        angle = -179
    # 算法开始
    angle_new = angle
    if angle<-90 and flag_8:
        if flag_5 :
            flag_5 = False
            flag_4 = True
            flag_7 = False
    elif angle>90 and flag_7:
        if flag_2 :
            flag_2 = False
            flag_1 = True
            flag_8 = False
    elif angle>-90 and angle<90 and flag_9:
        flag_2 = True
        flag_5 = True
        flag_7 = True
        flag_8 = True

        flag_1 = False
        flag_4 = False
    if flag_1:
        if angle<0:
            angle_new = angle+360
            flag_9 = False
        if angle<90 and angle>0:
            flag_9 = True

    if flag_4:
        if angle>0:
            angle_new = angle-360
            flag_9 = False
        if angle>-90 and angle<0:
            flag_9 = True

    # 算法结束
    # 当扩展后yaw值大于等于359，跳出循环
    if angle_new >= 359:
        break
    print(f"angle = {angle}\t\tangle_new = {angle_new}")

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

使用程序注意事项：

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