【Python】WHEELTEC惯导IMU主代码读取传感器数据代码

【Python】WHEELTEC GPS G60主代码读取传感器数据代码
读取Pitch角度

#WHEELTEC惯导IMU主代码读取传感器数据代码
import argparse
import numpy as np
import sys
import serial  # 导入模块
import serial.tools.list_ports
import threading
import struct
import time
import platform
from serial import EIGHTBITS, PARITY_NONE, STOPBITS_ONE
# 0.宏定义参数
PI = 3.1415926
FRAME_HEAD = str('fc')
FRAME_END = str('fd')
TYPE_IMU = str('40')
TYPE_AHRS = str('41')
TYPE_INSGPS = str('42')
TYPE_GEODETIC_POS = str('5c')
TYPE_GROUND = str('f0')
TYPE_SYS_STATE = str('50')
TYPE_BODY_ACCELERATION = str('62')
TYPE_ACCELERATION = str('61')
IMU_LEN = str('38')  # //56
AHRS_LEN = str('30')  # //48
INSGPS_LEN = str('48')  # //72
GEODETIC_POS_LEN = str('20')  # //32
SYS_STATE_LEN = str('64')  # // 100
BODY_ACCELERATION_LEN = str('10') #// 16
ACCELERATION_LEN = str('0c')  # 12
PI = 3.141592653589793
DEG_TO_RAD = 0.017453292519943295
isrun = True
# 1.获取命令行输入参数
def parse_opt(known=False):
    parser = argparse.ArgumentParser()
    # parser.add_argument('--debugs', type=bool, default=False, help='if debug info output in terminal ')
    parser.add_argument('--port', type=str, default='COM4', help='the models serial port receive data; example: '
                                                                 '    Windows: COM3'
                                                                 '    Linux: /dev/ttyUSB0')

    parser.add_argument('--bps', type=int, default=921600, help='the models baud rate set; default: 921600')
    parser.add_argument('--timeout', type=int, default=20, help='set the serial port timeout; default: 20')
    # parser.add_argument('--device_type', type=int, default=0, help='0: origin_data, 1: for single imu or ucar in ROS')

    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
    return receive_params


# 2.接收数据线程
def receive_data():
    open_port()
    # 尝试打开串口
    try:
        serial_ = serial.Serial(port=opt.port, baudrate=opt.bps, bytesize=EIGHTBITS, parity=PARITY_NONE,
                                stopbits=STOPBITS_ONE,
                                timeout=opt.timeout)
        #print("baud rates = " + str(serial_.baudrate))
    except:
        print("error:  unable to open port .")
        #exit(1)
    # 循环读取数据
    while serial_.isOpen(): #and tr.is_alive() :
        if not threading.main_thread().is_alive():
            print('done')
            break
        check_head = serial_.read().hex()
        # 校验帧头
        if check_head != FRAME_HEAD:
            continue
        head_type = serial_.read().hex()
        # 校验数据类型
        if (head_type != TYPE_IMU and head_type != TYPE_AHRS and head_type != TYPE_INSGPS and
                head_type != TYPE_GEODETIC_POS and head_type != 0x50 and head_type != TYPE_GROUND and
                head_type != TYPE_SYS_STATE and head_type!=TYPE_BODY_ACCELERATION and head_type!=TYPE_ACCELERATION):
            continue
        check_len = serial_.read().hex()
        # 校验数据类型的长度
        if head_type == TYPE_IMU and check_len != IMU_LEN:
            continue
        elif head_type == TYPE_AHRS and check_len != AHRS_LEN:
            continue
        elif head_type == TYPE_INSGPS and check_len != INSGPS_LEN:
            continue
        elif head_type == TYPE_GEODETIC_POS and check_len != GEODETIC_POS_LEN:
            continue
        elif head_type == TYPE_SYS_STATE and check_len != SYS_STATE_LEN:
            continue
        elif head_type == TYPE_GROUND or head_type == 0x50:
            continue
        elif head_type == TYPE_BODY_ACCELERATION and check_len != BODY_ACCELERATION_LEN:
            print("check head type "+str(TYPE_BODY_ACCELERATION)+" failed;"+" check_LEN:"+str(check_len))
            continue
        elif head_type == TYPE_ACCELERATION and check_len != ACCELERATION_LEN:
            print("check head type "+str(TYPE_ACCELERATION)+" failed;"+" ckeck_LEN:"+str(check_len))
            continue
        check_sn = serial_.read().hex()
        head_crc8 = serial_.read().hex()
        crc16_H_s = serial_.read().hex()
        crc16_L_s = serial_.read().hex()

        # 读取并解析IMU数据
        '''
        if head_type == TYPE_IMU:
            data_s = serial_.read(int(IMU_LEN, 16))
            IMU_DATA = struct.unpack('12f ii',data_s[0:56])
            return(IMU_DATA)
        '''
        if head_type == TYPE_AHRS:
            data_s = serial_.read(int(AHRS_LEN, 16))
            AHRS_DATA = struct.unpack('10f ii',data_s[0:48])
            return(AHRS_DATA)


# 3.寻找输入的port串口
def find_serial():
    port_list = list(serial.tools.list_ports.comports())
    for port in port_list:
        if port.device == opt.port:
            return True
    return False

def open_port():
    if find_serial()==False:
        print("error:  unable to find this port : " + opt.port)
        exit(1)

def UsePlatform():
    sys_str = platform.system()
    if sys_str == "Windows":
        print("Call Windows tasks")
    elif sys_str == "Linux":
        print("Call Linux tasks")
    else:
        print("Other System tasks: %s" % sys_str)
    return sys_str

#4.主程序
if __name__ == "__main__":
    print(UsePlatform())
    opt = parse_opt()
    timestamp1=time.time()#strftime("%Y-%m-%d %H:%M:%S")
    #print(f"当前时间：{timestamp1}")
    SimNum=100#采样点数
    for i in range(0,SimNum):
        Results_Data=receive_data()
        print(Results_Data[4]/PI*180)#Pitch角，弧度转度数
        #time.sleep(0.01)
    timestamp2=time.time()#strftime("%Y-%m-%d %H:%M:%S")
    #print(f"当前时间：{timestamp2}")
    print('平均一次采样用时：')
    print((timestamp2-timestamp1)/SimNum) 
    
    #tr = threading.Thread(target=receive_data)
    #tr.start()