PYTHON 之 IMU & GPS

协议

ST\n

RD\n

II\n

IMU

GPS

案例

# TEST FOR IMU & GPS
import serial #导入模块
import threading
import time  
import serial.tools.list_ports

def get_time_stamp():
    ct = time.time()
    local_time = time.localtime(ct)
    data_head = time.strftime("%Y-%m-%d %H:%M:%S", local_time)
    data_secs = (ct - int(ct)) * 1000
    time_stamp = "%s.%03d" % (data_head, data_secs)
    return time_stamp

def doRead(ser,term):
    matcher = re.compile(term)    #gives you the ability to search for anything
    tic     = time.time()
    buff    = ser.read(128)
    print(' buff = ',buff)
    tout    = 1
    # you can use if not ('\n' in buff) too if you don't like re
    while ((time.time() - tic) < tout) and (not matcher.search(buff)):
       buff += ser.read(128)

    return buff

port_list = list(serial.tools.list_ports.comports())
print(port_list)
if len(port_list) == 0:
    print('无可用串口')
else:
    for i in range(0,len(port_list)):
        #help(port_list[i])
        #print(port_list[i].__dict__)
        print(port_list[i])
        
            
for i in port_list:
    print('START TO TEST IMU & GPS ',i.device)
    #set buad ==115200, timeout = 0.1
    IMU_serial = serial.Serial(i.device,115200, timeout=1)
    if IMU_serial.isOpen():
        print(i.device,' is open')
    else:
        IMU_serial.open()
    
    
    print('start to write IMU\n')
    
    IMU_serial.flushInput()#清除输入缓冲区数据
    IMU_serial.flushOutput()#清除输出缓冲区数据
    IMU_serial.write(b'II\n')
    try:
        result = IMU_serial.readline()
        print(get_time_stamp())
        time.sleep(1)
        print('start to read IMU\n')
        #result = doRead(IMU_serial,'\n')
        #result = IMU_serial.readtill('\n')
        print(get_time_stamp())
    except Exception as e:
        print(e)
        print(i.device,' has no response!')
        IMU_serial.close()
    
    if result == b'IMU&GPS\n':
        print(i.name, ' is connected to IMU & GPS!')
        
        while True:
            print(' START ************************************',get_time_stamp())
            IMU_serial.write(b'RD\n')
            info = IMU_serial.readline()# 接收到bytes数组
            
            
            
            #处理收到的信息
            info_str = info.decode()# bytes解码为str
            #print('info_str: ',info_str)
            
            #str分解为5个列表
            info_list = info_str.split(';')
            
            #分别对每个列表进行解析
            time_imu =info_list[0]
            angle_speed_imu = info_list[1].split(',')
            angle_imu = info_list[2].split(',')
            gps_imu = info_list[3].split(',')
            gps_imu_etc = info_list[4].split(',')
            
            print(time_imu,'\n')
            print(angle_imu,'\n')
            print(angle_speed_imu,'\n')
            print(gps_imu,'\n')
            print(gps_imu_etc,'\n')
            #时间
            #t = time.strptime(info_list[0],"%Y-%m-%d-%H-%M-%S.%f")
            #姿态角
            alpha = float(angle_imu[1])
            beta = float(angle_imu[2])
            gamma = float(angle_imu[3])
            #角速度
            dalpha = float(angle_speed_imu[1])
            dbeta = float(angle_speed_imu[2])
            dgamma = float(angle_speed_imu[3])
            # 经纬度
            longitude = [float(gps_imu[1]),float(gps_imu[2])]
            latitude = [float(gps_imu[4]),float(gps_imu[5])]
            # GPS信息
            gps_height = float(gps_imu_etc[1])
            gps_yaw = float(gps_imu_etc[3])
            gps_speed = float(gps_imu_etc[5])
            
            print(' STOP ************************************',get_time_stamp())
            #print()
        IMU_serial.close()
        #return i.name
    else:
        print(i.name, ' is not connected to IMU & GPS!')
        IMU_serial.close()
        #return None