mavlink协议解析的一些理解

关于mavlink,要进行考试了，随便写了一些代码用来解析协议，简单进行了绘图展示

# import xml.etree.ElementTree as ET
import struct
import matplotlib.pyplot as plt
# import folium
def hex_to_int32(hex_string):
    # 将十六进制字符串转换为整数
    value = int(hex_string, 16)
    # 使用 struct.pack 将整数打包为 4 字节的二进制数据（小端序）
    packed_data = struct.pack("<I", value)
    # 使用 struct.unpack 将二进制数据解析为 32 位有符号整数
    int32_value = struct.unpack("<i", packed_data)[0]
    return int32_value

def hex2dec(hex_str: str) -> int:
    return int(hex_str.strip(), base = 16)

def hex_to_uint32(hex_string):
    # 将十六进制字符串转换为整数
    value = int(hex_string, 16)
    # 使用位操作确保结果是一个32位无符号整数
    result = value & 0xFFFFFFFF
    return result

def mavlinkv1(msg):
    msg = msg.split(' ') 
    msglen = len(msg)

    try:
        msgid = hex2dec(msg[5])
    except:
        return 0,0,0,0   
    if msgid == 33:
        time_boot_s = hex2dec(msg[9]+msg[8]+msg[7]+msg[6])/1000
        lat = hex2dec(msg[13]+msg[12]+msg[11]+msg[10])/(10**7)
        lon = hex2dec(msg[17]+msg[16]+msg[15]+msg[14])/(10**7)
        alt = hex2dec(msg[21]+msg[20]+msg[19]+msg[18])/1000
    else: return 0,0,0,0
    return time_boot_s, lat, lon, alt


def mavlinkv2(msg):
    msg = msg.split(' ') 
    msglen = len(msg)
    msg = msg[0: msglen-2]
    # seq_id = hex2dec(msg[4])
    try:
        msgid = hex2dec(msg[9]+msg[8]+msg[7])
    except:
        return 0,0,0,0   
    if msgid == 33:
        time_boot_s = hex_to_uint32(msg[13]+msg[12]+msg[11]+msg[10])/1000
        lat = hex2dec(msg[17]+msg[16]+msg[15]+msg[14])/(10**7)
        lon = hex2dec(msg[21]+msg[20]+msg[19]+msg[18])/(10**7)
        alt = hex2dec(msg[25]+msg[24]+msg[23]+msg[22])/1000
    else: return 0,0,0,0
    return time_boot_s, lat, lon, alt

def MAV_TYPE(msg):
    msg = msg.split(' ') 
    msglen = len(msg)
    try:
        msgid = hex2dec(msg[9]+msg[8]+msg[7])
    except:
        return 'None'
    '''
    if msgid == 0 and hex2dec(msg[10])== 0:
        return 'id = 0, Generic micro air vehicle 通用微型飞行器'
    '''
    if msgid == 0 and hex2dec(msg[10])== 1:
        return 'id = 1, Fixed wing aircraft_固定翼'
    elif msgid == 0 and hex2dec(msg[10])==2:
        return 'id = 2, Quadrotor_四旋翼'
    elif msgid == 0 and hex2dec(msg[10])==3:
        return 'id = 3, Coaxial helicopter_同轴直升机'
    elif msgid == 0 and hex2dec(msg[10])==4:
        return 'id = 4, helicopter_普通直升机'
    elif msgid == 0 and hex2dec(msg[10])==6:
        return 'id = 6, GCS_地面控制站_不可信'
    elif msgid == 0 and hex2dec(msg[10])==10:
        return 'id = 10, GROUND_ROVER_车辆'
    else:
        return 'None'
print('输入实例: D:/学习/毕业联考/信号/dabao/13.txt')
fliepath = input('输入保存数据路径: ')
with open(fliepath) as f:
    msg = f.readlines()
'''
mavlinkv1_tree = ET.parse('mavlink_v1.xml')
mavlinkv1_root = mavlinkv1_tree.getroot()
'''


msg_len = len(msg)
i = 0
time_boot_s0 = []
lat0 = []
lon0 = []
alt0 = []
mavtype_v1 =[]
mavtype_v2 =[]
lstv1 = []
lstv2 = []
mav1_flag = True
mav2_flag = True
while i <msg_len:
    if 'FE' in msg[i]:
        time_boot_s, lat, lon, alt = mavlinkv1(msg[i])
        if lat != 0:
            time_boot_s0.append(time_boot_s)
            lat0.append(lat)
            lon0.append(lon)
            alt0.append(alt)
        i+=1
        mavtype = MAV_TYPE(msg[i-1])
        if 'None' not in mavtype:
            mavtype_v1.append(mavtype)

    elif 'FD' in msg[i]:
        time_boot_s, lat, lon, alt = mavlinkv2(msg[i])
        if lat != 0:
            time_boot_s0.append(time_boot_s)
            lat0.append(lat)
            lon0.append(lon)
            alt0.append(alt)
        i+=1
        mavtype = MAV_TYPE(msg[i-1])
        if 'None' not in mavtype:
            mavtype_v2.append(mavtype)
for item in mavtype_v2:
    if item not in lstv2:
        lstv2.append(item)
for i in range(len(lstv2)):
    print('v2: '+lstv2[i])
for item in mavtype_v1:
    if item not in lstv1:
        lstv1.append(item)
for i in range(len(lstv1)):
    print('v1: '+lstv1[i])
for i in range(len(lstv1)):
    if '不可信' in lstv1[i]:
        mav1_flag = False
for i in range(len(lstv2)):
    if '不可信' in lstv2[i]:
        mav2_flag = False
if mav1_flag == False:
    print('mavlink_v1 是不可信协议')
elif mav2_flag == False:
    print('mavlink_v2 是不可信协议')
    
    
print('最大飞行高度: '+ str(max(alt0)) + '米')
print('经度_init: '+str(lon0[0]))
print('纬度_init: '+str(lat0[0]))
plt.figure()
plt.plot(lat0,lon0)
plt.show()
plt.figure()
plt.plot(time_boot_s0,alt0)
plt.show()

'''
# 设置地图
offline_map_path = 'satellite/{z}/{x}/{y}.jpg'
# offline_map_path = 'TEST影像/全球基础数据/{z}/{x}/{y}.jpg'
location = [lon0[0], lat0[0]]
m = folium.Map(location = location, zoom_start=13, max_zoom = 20, tiles=offline_map_path, attr='My Offline Map')
m.add_child(folium.LatLngPopup())  # 点击显示经纬度
for i in range(0, len(lon0)-1):
    folium.PolyLine(locations=[[lat0[i], lon0[i]], [lat0[i+1], lon0[i+1]]], color='red', weight=6, opacity=0.8).add_to(m)
    # folium.PolyLine(locations=[[39.9177, 116.3953], [ 39.9173, 116.4131]], color='red', weight=6, opacity=0.8).add_to(m)
m.save('schools_map.html') #保存到本地

'''

with open('lat.txt', 'w') as latitude:
    for i in range(len(lat0)):
        latitude.write(str(lat0[i])+'\n')
with open('lon.txt', 'w') as lontitude:
    for i in range(len(lat0)):
        lontitude.write(str(lon0[i])+'\n')
latitude.close()
lontitude.close()