航迹推算张

import serial
import time
import math


def get_date():
    """
    此函数通过串口接收数据，并把每一帧数据提取出来，以列表的形式存储每一帧数据，为后续数据提取做基础
    :return: a_list 每一帧数据的列表
    """
    ser = serial.Serial(  # 下面这些参数根据情况修改
        port='COM5',  # 串口
        baudrate=115200,  # 波特率
        parity=serial.PARITY_NONE,  # 校验位
        stopbits=serial.STOPBITS_ONE,  # 停止位
        bytesize=serial.EIGHTBITS,  # 字节大小
        timeout=1)
    while True:
        msg = ser.read_all()  # 读取全部数据
        # msg = ser.readline() # 读取遇换行符换行
        # msg = b'\x00\x00\x00\x00\x00\x00\x00\xfb\xa6\xfe46(\x00\x00\x00\x01\x00\x01_\xe8E' 原始数据格式，后续要做转换
        # print('raw', msg)
        """
        备用代码
        d = 1
        for key in msg:
            if d % 24 == 0:
                print(hex(key))
            else:
                print(hex(key), end=' ')
            d = d + 1
        """
        a_list = []  # 空列表
        for key in msg:
            a = hex(key)  # 将stm32发来的原始数据转换为16进制

            # 提取包含帧头帧尾24字节的16进制数据并保存到a_list列表中
            while a == "0x7b" and len(a_list) < 25:
                for key01 in msg:
                    a = hex(key01)
                    a_list.append(a)
                    if len(a_list) == 24 and a_list[2] != "0x7b":
                        return a_list


pos_x = 0  # 此处赋予小车初始位姿 接收二维码赋予的初值
pos_y = 0
theta = 0
sampling_time = 0.02  # 积分时间
while True:
    b_list = get_date()

    # 测试代码
    # print(b_list[2], b_list[3])
    # print(int(b_list[2], 16), int(b_list[3], 16),  int(b_list[19], 16))
    # print(type(int(b_list[3], 16)))
    # print("小车速度：%f 小车角速度：%f" % ((float(int(b_list[3], 16))) / 1000, (float(int(b_list[19], 16))) / 3753))


    # 此处用来判断小车前进或者后退，逆时针旋转或者顺时针旋转。然后在此基础上赋予速度变量和角速度变量正负号
    # print(type(bin(int(b_list[2], 16))[2])) 注意此处为字符串类型，需要做转换才能判断  【易搞错，重要！】
    if int(bin(int(b_list[2], 16))[2]) == 0:  # 判断高八位中的最高位二进制是否为0，若为0，则小车前进,在这里进行了二进制转换
        robot_vel = (float(int(b_list[2] + b_list[3][2:], 16))) / 1000
    else:
        robot_vel = -((int("0xffff", 16) - int(b_list[2] + b_list[3][2:], 16)) / 1000)

    if int(bin(int(b_list[18], 16))[2]) == 0:  # 判断高八位中的最高位二进制是否为0，若为0，则小车绕Z轴逆时针旋转正方向，在这里进行了二进制转换
        angular_velocity_z = (float(int(b_list[18] + b_list[19][2:], 16))) / 3753
    else:
        angular_velocity_z = -((int("0xffff", 16) - int(b_list[18] + b_list[19][2:], 16)) / 3753)

    # robot_vel = (float(int(b_list[3], 16))) / 1000           此为未加判断速度和角速度方向时的旧代码
    # angular_velocity_z = (float(int(b_list[19], 16))) / 3753

    if robot_vel != float(0):  # 有编码器的速度则进行计算
        if angular_velocity_z < float(0.03):  # imu静止时也会有微小输出，设置一个阈值来表示小车实际运动了
            angular_velocity_z = float(0)
            theta += angular_velocity_z * sampling_time
            pos_x += (robot_vel * math.cos(theta)) * sampling_time
            pos_y += (robot_vel * math.sin(theta)) * sampling_time
        else:
            theta += angular_velocity_z * sampling_time
            pos_x += (robot_vel * math.cos(theta)) * sampling_time
            pos_y += (robot_vel * math.sin(theta)) * sampling_time
    else:
        angular_velocity_z = 0  # 若小车速度为0,此处应赋值等于初始位姿，即位姿不发生变化

    print("小车速度是:%f     航向数据是:%f     x轴数据是:%f     y轴的数据是:%f" % (robot_vel, theta, pos_x, pos_y))