解码程序（依照mavlinkv2.0 字节协议，fields即paylode内容）：
import serial
import struct
import crcmod # 使用 crcmod 替代原 crc 库
from checksum import mavlink_get_crc_extra
class MavLinkV2Parser:
def __init__(self):
self.buffer = bytearray()
self.message_definitions = {
0: { # HEARTBEAT 消息定义 (ID=0)
'name': 'HEARTBEAT',
# 'fields': [
# ('custom_mode', 'I'), # uint32_t (4字节)
# ('type', 'B'), # uint8_t (1字节)
# ('autopilot', 'B'), # uint8_t (1字节)
# ('base_mode', 'B'), # uint8_t (1字节)
# ('system_status', 'B'), # uint8_t (1字节)
# ('mavlink_version', 'B') # uint8_t (1字节) # 修正为1字节
# ],
'fields': [
('time_boot_ms', 'I'), # uint32_t (4字节)
('lat', 'i'), # int32_t (4字节)
('lon', 'i'), # int32_t (4字节)
('alt', 'i'), # int32_t (4字节)
('relative_alt', 'i'), # int32_t (4字节)
('vx', 'h'), # int16_t (2字节)
('vy', 'h'), # int16_t (2字节)
('vz', 'h'), # int16_t (2字节)
('hdg', 'H'), # uint16_t (2字节)
('time_usec', 'Q'), # uint64_t (8字节)
('xacc', 'f'), # float (4字节)
('yacc', 'f'), # float (4字节)
('zacc', 'f'), # float (4字节)
('xgyro', 'f'), # float (4字节)
('ygyro', 'f'), # float (4字节)
('zgyro', 'f'), # float (4字节)
('xmag', 'f'), # float (4字节)
('ymag', 'f'), # float (4字节)
('zmag', 'f'), # float (4字节)
('abs_pressure', 'f'), # float (4字节)
('diff_pressure', 'f'), # float (4字节)
('pressure_alt', 'f'), # float (4字节)
('temperature', 'f'), # float (4字节)
('fields_updated', 'H'),# uint16_t (2字节)
('id', 'B') # uint8_t (1字节)
],
# 总字节数：8 + (13*4) + 2 + 1 = 8 + 52 + 3 = 63字节
'crc_extra': 0xAF
}
}
# 创建 MAVLink 专用的 CRC16 计算函数 (多项式 0x1021，初始值 0xFFFF)
self.crc16_mavlink = crcmod.mkCrcFun(
0x11021, rev=True, initCrc=0xFFFF, xorOut=0x0000 # 关键修正点！
)
def parse(self, data):
self.buffer.extend(data)
messages = []
while True:
# 查找起始位 FD (0xFD)
# print("self.buffer",self.buffer)
start_idx = self.buffer.find(0xFD)
# print("start_idx",start_idx)
if start_idx == -1:
self.buffer.clear()
break
# 检查是否足够解析头部（至少10字节）
# print("len(self.buffer)",len(self.buffer))
if len(self.buffer) - start_idx < 10:
break
header = self.buffer[start_idx:start_idx+10]
# print("header",header)
payload_len = header[1]
# print("payload_len",payload_len)
msg_id = int.from_bytes(header[7:10], byteorder='little')
# print("msg_id",msg_id)
# 检查消息是否完整（头部 + 负载 + CRC = 10 + payload_len + 2）
total_len = 10 + payload_len + 2
# print("total_len",total_len)
# print("len(self.buffer) ",len(self.buffer) )
# print("len(self.buffer) - start_idx",len(self.buffer) - start_idx)
if len(self.buffer) - start_idx < total_len:
break
# 提取完整消息
full_msg = self.buffer[start_idx:start_idx+total_len]
# print("full_msg",full_msg)
self.buffer = self.buffer[start_idx+total_len:]
# 校验CRC
crc_valid = self._check_crc(full_msg, msg_id,)
# print("crc_valid",crc_valid)
if not crc_valid:
continue # CRC校验失败
# 解析消息
msg = {
'header': {
'system_id': header[5],
'component_id': header[6],
'seq': header[4],
'msg_id': msg_id
},
'payload': self._parse_payload(full_msg[10:-2], msg_id),
'crc_valid': crc_valid
}
messages.append(msg)
return messages
def _check_crc(self, msg_bytes, msg_id):
# 提取数据部分（从Payload Length到Payload末尾，不含起始位0xFD和CRC字段）
data_part = msg_bytes[1:-2] # 正确范围
# print("data_part", data_part)
# 附加CRC_EXTRA
if msg_id in self.message_definitions:
msg = {'msgid': msg_id}
crc_value = mavlink_get_crc_extra(msg)
data_part += bytes([crc_value]) # 将整数转为bytes后拼接
# print("data_part", data_part)
# 计算CRC（使用修正后的多项式和非反转模式）
calculated_crc = self.crc16_mavlink(data_part)
# print("calculated_crc",calculated_crc)
# 对比消息中的CRC（小端字节序）
msg_crc = int.from_bytes(msg_bytes[-2:], byteorder='little')
# print("msg_bytes[-2:]",msg_bytes[-2:])
# print("msg_crc",msg_crc)
# calculated_crc = msg_crc
return calculated_crc == msg_crc
def _parse_payload(self, payload_bytes, msg_id):
if msg_id not in self.message_definitions:
return None # 未知消息类型
fmt = '<' + ''.join([f[1] for f in self.message_definitions[msg_id]['fields']])
fields = struct.unpack(fmt, payload_bytes)
parsed = {}
for i, (name, _) in enumerate(self.message_definitions[msg_id]['fields']):
parsed[name] = fields[i]
return parsed
# 示例用法
if __name__ == "__main__":
ser = serial.Serial('COM3', baudrate=57600) # 修改为实际串口
parser = MavLinkV2Parser()
while True:
data = ser.read(ser.in_waiting or 1)
messages = parser.parse(data)
for msg in messages:
if msg['crc_valid']:
print(f"收到消息: ID={msg['header']['msg_id']}, 系统ID={msg['header']['system_id']}")
print(f"负载: {msg['payload']}")
CRC检验：MavLinkV2.0 采用CRC-16/MCRF4XX方式检验，且需要在末尾额外加入额外加上CRC_EXTRA,其值的确定可从官方源码中获得（重点在all/checksum.h）：
mavlink/c_library_v2: Official reference C / C++ library for the v2 protocol
这里直接给出CRC_EXTRA的获取程序：
mavlink_message_crcs = [
{'msgid': entry[0], 'crc_extra': entry[1]} # 提取msgid和crc_extra
for entry in [
(0, 50, 9, 9, 0, 0, 0),
(1, 124, 31, 43, 0, 0, 0),
(2, 137, 12, 12, 0, 0, 0),
(4, 237, 14, 14, 3, 12, 13),
(5, 217, 28, 28, 1, 0, 0),
(6, 104, 3, 3, 0, 0, 0),
(7, 119, 32, 32, 0, 0, 0),
(8, 117, 36, 36, 0, 0, 0),
(11, 89, 6, 6, 1, 4, 0),
(20, 214, 20, 20, 3, 2, 3),
(21, 159, 2, 2, 3, 0, 1),
(22, 220, 25, 25, 0, 0, 0),
(23, 168, 23, 23, 3, 4, 5),
(24, 24, 30, 52, 0, 0, 0),
(25, 23, 101, 101, 0, 0, 0),
(26, 170, 22, 24, 0, 0, 0),
(27, 144, 26, 29, 0, 0, 0),
(28, 67, 16, 16, 0, 0, 0),
(29, 115, 14, 16, 0, 0, 0),
(30, 39, 28, 28, 0, 0, 0),
(31, 246, 32, 48, 0, 0, 0),
(32, 185, 28, 28, 0, 0, 0),
(33, 104, 28, 28, 0, 0, 0),
(34, 237, 22, 22, 0, 0, 0),
(35, 244, 22, 22, 0, 0, 0),
(36, 222, 21, 37, 0, 0, 0),
(37, 212, 6, 7, 3, 4, 5),
(38, 9, 6, 7, 3, 4, 5),
(39, 254, 37, 38, 3, 32, 33),
(40, 230, 4, 5, 3, 2, 3),
(41, 28, 4, 4, 3, 2, 3),
(42, 28, 2, 18, 0, 0, 0),
(43, 132, 2, 3, 3, 0, 1),
(44, 221, 4, 9, 3, 2, 3),
(45, 232, 2, 3, 3, 0, 1),
(46, 11, 2, 2, 0, 0, 0),
(47, 153, 3, 8, 3, 0, 1),
(48, 41, 13, 21, 1, 12, 0),
(49, 39, 12, 20, 0, 0, 0),
(50, 78, 37, 37, 3, 18, 19),
(51, 196, 4, 5, 3, 2, 3),
(54, 15, 27, 27, 3, 24, 25),
(55, 3, 25, 25, 0, 0, 0),
(61, 167, 72, 72, 0, 0, 0),
(62, 183, 26, 26, 0, 0, 0),
(63, 119, 181, 181, 0, 0, 0),
(64, 191, 225, 225, 0, 0, 0),
(65, 118, 42, 42, 0, 0, 0),
(66, 148, 6, 6, 3, 2, 3),
(67, 21, 4, 4, 0, 0, 0),
(69, 243, 11, 30, 1, 10, 0),
(70, 124, 18, 38, 3, 16, 17),
(73, 38, 37, 38, 3, 32, 33),
(74, 20, 20, 20, 0, 0, 0),
(75, 158, 35, 35, 3, 30, 31),
(76, 152, 33, 33, 3, 30, 31),
(77, 143, 3, 10, 3, 8, 9),
(80, 14, 4, 4, 3, 2, 3),
(81, 106, 22, 22, 0, 0, 0),
(82, 49, 39, 51, 3, 36, 37),
(83, 22, 37, 37, 0, 0, 0),
(84, 143, 53, 53, 3, 50, 51),
(85, 140, 51, 51, 0, 0, 0),
(86, 5, 53, 53, 3, 50, 51),
(87, 150, 51, 51, 0, 0, 0),
(89, 231, 28, 28, 0, 0, 0),
(90, 183, 56, 56, 0, 0, 0),
(91, 63, 42, 42, 0, 0, 0),
(92, 54, 33, 33, 0, 0, 0),
(93, 47, 81, 81, 0, 0, 0),
(100, 175, 26, 34, 0, 0, 0),
(101, 102, 32, 117, 0, 0, 0),
(102, 158, 32, 117, 0, 0, 0),
(103, 208, 20, 57, 0, 0, 0),
(104, 56, 32, 116, 0, 0, 0),
(105, 93, 62, 63, 0, 0, 0),
(106, 138, 44, 44, 0, 0, 0),
(107, 108, 64, 65, 0, 0, 0),
(108, 32, 84, 92, 0, 0, 0),
(109, 185, 9, 9, 0, 0, 0),
(110, 84, 254, 254, 3, 1, 2),
(111, 34, 16, 18, 3, 16, 17),
(112, 174, 12, 12, 0, 0, 0),
(113, 124, 36, 39, 0, 0, 0),
(114, 237, 44, 44, 0, 0, 0),
(115, 4, 64, 64, 0, 0, 0),
(116, 76, 22, 24, 0, 0, 0),
(117, 128, 6, 6, 3, 4, 5),
(118, 56, 14, 14, 0, 0, 0),
(119, 116, 12, 12, 3, 10, 11),
(120, 134, 97, 97, 0, 0, 0),
(121, 237, 2, 2, 3, 0, 1),
(122, 203, 2, 2, 3, 0, 1),
(123, 250, 113, 113, 3, 0, 1),
(124, 87, 35, 57, 0, 0, 0),
(125, 203, 6, 6, 0, 0, 0),
(126, 220, 79, 81, 3, 79, 80),
(127, 25, 35, 35, 0, 0, 0),
(128, 226, 35, 35, 0, 0, 0),
(129, 46, 22, 24, 0, 0, 0),
(130, 29, 13, 13, 0, 0, 0),
(131, 223, 255, 255, 0, 0, 0),
(132, 85, 14, 39, 0, 0, 0),
(133, 6, 18, 18, 0, 0, 0),
(134, 229, 43, 43, 0, 0, 0),
(135, 203, 8, 8, 0, 0, 0),
(136, 1, 22, 22, 0, 0, 0),
(137, 195, 14, 16, 0, 0, 0),
(138, 109, 36, 120, 0, 0, 0),
(139, 168, 43, 43, 3, 41, 42),
(140, 181, 41, 41, 0, 0, 0),
(141, 47, 32, 32, 0, 0, 0),
(142, 72, 243, 243, 0, 0, 0),
(143, 131, 14, 16, 0, 0, 0),
(144, 127, 93, 93, 0, 0, 0),
(146, 103, 100, 100, 0, 0, 0),
(147, 154, 36, 54, 0, 0, 0),
(148, 178, 60, 78, 0, 0, 0),
(149, 200, 30, 60, 0, 0, 0),
(150, 134, 42, 42, 0, 0, 0),
(151, 219, 8, 8, 3, 6, 7),
(152, 208, 4, 8, 0, 0, 0),
(153, 188, 12, 12, 0, 0, 0),
(154, 84, 15, 15, 3, 6, 7),
(155, 22, 13, 13, 3, 4, 5),
(156, 19, 6, 6, 3, 0, 1),
(157, 21, 15, 15, 3, 12, 13),
(158, 134, 14, 15, 3, 12, 13),
(160, 78, 12, 12, 3, 8, 9),
(161, 68, 3, 3, 3, 0, 1),
(162, 189, 8, 9, 0, 0, 0),
(163, 127, 28, 28, 0, 0, 0),
(164, 154, 44, 44, 0, 0, 0),
(165, 21, 3, 3, 0, 0, 0),
(166, 21, 9, 9, 0, 0, 0),
(167, 144, 22, 22, 0, 0, 0),
(168, 1, 12, 12, 0, 0, 0),
(169, 234, 18, 18, 0, 0, 0),
(170, 73, 34, 34, 0, 0, 0),
(171, 181, 66, 66, 0, 0, 0),
(172, 22, 98, 98, 0, 0, 0),
(173, 83, 8, 8, 0, 0, 0),
(174, 167, 48, 48, 0, 0, 0),
(175, 138, 19, 19, 3, 14, 15),
(176, 234, 3, 3, 3, 0, 1),
(177, 240, 20, 20, 0, 0, 0),
(178, 47, 24, 24, 0, 0, 0),
(179, 189, 29, 29, 1, 26, 0),
(180, 52, 45, 47, 1, 42, 0),
(181, 174, 4, 4, 0, 0, 0),
(182, 229, 40, 40, 0, 0, 0),
(183, 85, 2, 2, 3, 0, 1),
(184, 159, 206, 206, 3, 4, 5),
(185, 186, 7, 7, 3, 4, 5),
(186, 72, 29, 29, 3, 0, 1),
(191, 92, 27, 27, 0, 0, 0),
(192, 36, 44, 54, 0, 0, 0),
(193, 71, 22, 26, 0, 0, 0),
(194, 98, 25, 33, 0, 0, 0),
(195, 120, 37, 37, 0, 0, 0),
(200, 134, 42, 42, 3, 40, 41),
(201, 205, 14, 14, 3, 12, 13),
(214, 69, 8, 8, 3, 6, 7),
(215, 101, 3, 3, 0, 0, 0),
(216, 50, 3, 3, 3, 0, 1),
(217, 202, 6, 6, 0, 0, 0),
(218, 17, 7, 7, 3, 0, 1),
(219, 162, 2, 2, 0, 0, 0),
(220, 34, 32, 32, 0, 0, 0),
(221, 71, 42, 42, 0, 0, 0),
(222, 15, 3, 3, 0, 0, 0),
(223, 119, 47, 47, 3, 42, 43),
(224, 102, 45, 45, 3, 42, 43),
(225, 208, 65, 73, 0, 0, 0),
(226, 207, 8, 8, 0, 0, 0),
(230, 163, 42, 42, 0, 0, 0),
(231, 105, 40, 40, 0, 0, 0),
(232, 151, 63, 65, 0, 0, 0),
(233, 35, 182, 182, 0, 0, 0),
(234, 150, 40, 40, 0, 0, 0),
(235, 179, 42, 42, 0, 0, 0),
(241, 90, 32, 32, 0, 0, 0),
(242, 104, 52, 60, 0, 0, 0),
(243, 85, 53, 61, 1, 52, 0),
(244, 95, 6, 6, 0, 0, 0),
(245, 130, 2, 2, 0, 0, 0),
(246, 184, 38, 38, 0, 0, 0),
(247, 81, 19, 19, 0, 0, 0),
(248, 8, 254, 254, 3, 3, 4),
(249, 204, 36, 36, 0, 0, 0),
(250, 49, 30, 30, 0, 0, 0),
(251, 170, 18, 18, 0, 0, 0),
(252, 44, 18, 18, 0, 0, 0),
(253, 83, 51, 54, 0, 0, 0),
(254, 46, 9, 9, 0, 0, 0),
(256, 71, 42, 42, 3, 8, 9),
(257, 131, 9, 9, 0, 0, 0),
(258, 187, 32, 232, 3, 0, 1),
(259, 92, 235, 237, 0, 0, 0),
(260, 146, 5, 14, 0, 0, 0),
(261, 179, 27, 61, 0, 0, 0),
(262, 12, 18, 23, 0, 0, 0),
(263, 133, 255, 255, 0, 0, 0),
(264, 49, 28, 32, 0, 0, 0),
(265, 26, 16, 20, 0, 0, 0),
(266, 193, 255, 255, 3, 2, 3),
(267, 35, 255, 255, 3, 2, 3),
(268, 14, 4, 4, 3, 2, 3),
(269, 109, 213, 215, 0, 0, 0),
(270, 59, 19, 20, 0, 0, 0),
(271, 22, 52, 53, 0, 0, 0),
(275, 126, 31, 32, 0, 0, 0),
(276, 18, 49, 50, 0, 0, 0),
(277, 62, 30, 30, 0, 0, 0),
(280, 70, 33, 33, 0, 0, 0),
(281, 48, 13, 13, 0, 0, 0),
(282, 123, 35, 35, 3, 32, 33),
(283, 74, 144, 145, 0, 0, 0),
(284, 99, 32, 32, 3, 30, 31),
(285, 137, 40, 49, 3, 38, 39),
(286, 210, 53, 57, 3, 50, 51),
(287, 1, 23, 23, 3, 20, 21),
(288, 20, 23, 23, 3, 20, 21),
(290, 251, 46, 46, 0, 0, 0),
(291, 10, 57, 57, 0, 0, 0),
(295, 234, 12, 12, 0, 0, 0),
(299, 19, 96, 98, 0, 0, 0),
(300, 217, 22, 22, 0, 0, 0),
(301, 243, 58, 58, 0, 0, 0),
(310, 28, 17, 17, 0, 0, 0),
(311, 95, 116, 116, 0, 0, 0),
(320, 243, 20, 20, 3, 2, 3),
(321, 88, 2, 2, 3, 0, 1),
(322, 243, 149, 149, 0, 0, 0),
(323, 78, 147, 147, 3, 0, 1),
(324, 132, 146, 146, 0, 0, 0),
(330, 23, 158, 167, 0, 0, 0),
(331, 91, 230, 233, 0, 0, 0),
(332, 236, 239, 239, 0, 0, 0),
(333, 231, 109, 109, 0, 0, 0),
(334, 72, 10, 10, 0, 0, 0),
(335, 225, 24, 24, 0, 0, 0),
(336, 245, 84, 84, 0, 0, 0),
(339, 199, 5, 5, 0, 0, 0),
(340, 99, 70, 70, 0, 0, 0),
(350, 232, 20, 252, 0, 0, 0),
(354, 210, 14, 14, 3, 12, 13),
(355, 6, 12, 12, 0, 0, 0),
(360, 11, 25, 25, 0, 0, 0),
(361, 93, 33, 33, 0, 0, 0),
(369, 151, 24, 24, 0, 0, 0),
(370, 75, 87, 109, 0, 0, 0),
(371, 10, 26, 26, 0, 0, 0),
(372, 26, 140, 140, 0, 0, 0),
(373, 117, 42, 42, 0, 0, 0),
(375, 251, 140, 140, 0, 0, 0),
(380, 232, 20, 20, 0, 0, 0),
(385, 147, 133, 133, 3, 2, 3),
(386, 132, 16, 16, 3, 4, 5),
(387, 4, 72, 72, 3, 4, 5),
(388, 8, 37, 37, 3, 32, 33),
(390, 156, 238, 238, 0, 0, 0),
(395, 0, 212, 212, 0, 0, 0),
(396, 50, 160, 160, 0, 0, 0),
(397, 182, 108, 108, 0, 0, 0),
(400, 110, 254, 254, 3, 4, 5),
(401, 183, 6, 6, 3, 4, 5),
(410, 160, 53, 53, 0, 0, 0),
(411, 106, 3, 3, 0, 0, 0),
(412, 33, 6, 6, 3, 4, 5),
(413, 77, 7, 7, 3, 4, 5),
(414, 109, 16, 16, 0, 0, 0),
(415, 161, 16, 16, 0, 0, 0),
(420, 20, 9, 73, 3, 6, 7),
(435, 134, 46, 46, 0, 0, 0),
(436, 193, 9, 9, 0, 0, 0),
(437, 30, 1, 1, 0, 0, 0),
(440, 66, 35, 35, 0, 0, 0),
(441, 169, 17, 17, 0, 0, 0),
(510, 245, 106, 106, 0, 0, 0),
(511, 28, 71, 71, 0, 0, 0),
(512, 184, 2, 2, 0, 0, 0),
(8002, 218, 16, 16, 0, 0, 0),
(8003, 231, 41, 41, 0, 0, 0),
(8004, 172, 98, 98, 0, 0, 0),
(8005, 251, 38, 38, 0, 0, 0),
(8006, 97, 14, 14, 0, 0, 0),
(8007, 64, 32, 32, 0, 0, 0),
(8008, 234, 33, 33, 0, 0, 0),
(8009, 144, 16, 16, 0, 0, 0),
(8010, 155, 41, 41, 0, 0, 0),
(8011, 20, 102, 102, 0, 0, 0),
(8012, 54, 16, 16, 0, 0, 0),
(8013, 222, 46, 46, 0, 0, 0),
(8014, 200, 14, 14, 0, 0, 0),
(8015, 23, 24, 24, 0, 0, 0),
(8016, 149, 18, 18, 0, 0, 0),
(9000, 113, 137, 137, 0, 0, 0),
(9005, 117, 34, 34, 0, 0, 0),
(10001, 209, 20, 20, 0, 0, 0),
(10002, 186, 41, 41, 0, 0, 0),
(10003, 4, 1, 1, 0, 0, 0),
(10004, 133, 9, 9, 0, 0, 0),
(10005, 103, 9, 9, 0, 0, 0),
(10006, 193, 4, 4, 0, 0, 0),
(10007, 71, 17, 17, 0, 0, 0),
(10008, 240, 14, 14, 0, 0, 0),
(10151, 195, 85, 85, 0, 0, 0),
(11000, 134, 51, 52, 3, 4, 5),
(11001, 15, 135, 136, 0, 0, 0),
(11002, 234, 179, 180, 3, 4, 5),
(11003, 64, 5, 5, 0, 0, 0),
(11004, 11, 232, 232, 3, 8, 9),
(11005, 93, 230, 230, 0, 0, 0),
(11010, 46, 49, 49, 0, 0, 0),
(11011, 106, 44, 44, 0, 0, 0),
(11020, 205, 16, 16, 0, 0, 0),
(11030, 144, 44, 44, 0, 0, 0),
(11031, 133, 44, 44, 0, 0, 0),
(11032, 85, 44, 44, 0, 0, 0),
(11033, 195, 37, 37, 3, 16, 17),
(11034, 79, 5, 5, 0, 0, 0),
(11035, 128, 8, 8, 3, 4, 5),
(11036, 177, 34, 34, 0, 0, 0),
(11037, 130, 28, 28, 0, 0, 0),
(11038, 47, 38, 38, 0, 0, 0),
(11039, 142, 9, 9, 0, 0, 0),
(11040, 132, 44, 44, 0, 0, 0),
(11041, 208, 44, 44, 0, 0, 0),
(11042, 201, 44, 44, 0, 0, 0),
(11043, 193, 44, 44, 0, 0, 0),
(11044, 189, 44, 44, 0, 0, 0),
(12900, 114, 44, 44, 3, 0, 1),
(12901, 254, 59, 59, 3, 30, 31),
(12902, 140, 53, 53, 3, 4, 5),
(12903, 249, 46, 46, 3, 0, 1),
(12904, 77, 54, 54, 3, 28, 29),
(12905, 49, 43, 43, 3, 0, 1),
(12915, 94, 249, 249, 3, 0, 1),
(12918, 139, 51, 51, 0, 0, 0),
(12919, 7, 18, 18, 3, 16, 17),
(12920, 20, 5, 5, 0, 0, 0),
(17000, 103, 179, 179, 0, 0, 0),
(17150, 26, 33, 33, 0, 0, 0),
(17151, 72, 16, 16, 0, 0, 0),
(17153, 19, 17, 17, 0, 0, 0),
(17154, 89, 17, 17, 0, 0, 0),
(17155, 27, 10, 10, 0, 0, 0),
(17156, 14, 91, 91, 0, 0, 0),
(17157, 187, 84, 84, 0, 0, 0),
(17158, 106, 24, 24, 0, 0, 0),
(42000, 227, 1, 1, 0, 0, 0),
(42001, 239, 46, 46, 0, 0, 0),
(50001, 246, 32, 32, 0, 0, 0),
(50002, 181, 246, 246, 0, 0, 0),
(50003, 62, 19, 19, 0, 0, 0),
(50004, 240, 10, 10, 3, 8, 9),
(50005, 152, 6, 6, 3, 4, 5),
(52000, 13, 100, 100, 0, 0, 0),
(52001, 239, 1, 1, 0, 0, 0),
(60010, 208, 33, 33, 0, 0, 0),
(60011, 183, 7, 7, 0, 0, 0),
(60012, 99, 36, 36, 3, 32, 33),
(60013, 129, 24, 24, 3, 20, 21),
(60014, 134, 8, 8, 3, 4, 5),
(60020, 202, 4, 4, 0, 0, 0),
(60040, 156, 245, 245, 0, 0, 0),
(60041, 191, 255, 255, 0, 0, 0),
(60050, 220, 14, 14, 0, 0, 0),
(60051, 245, 24, 24, 0, 0, 0),
(60052, 101, 44, 44, 0, 0, 0),
(60053, 45, 6, 6, 0, 0, 0)
]
]
def mavlink_get_msg_entry(msgid):
"""
二分查找实现逻辑 (与C代码逻辑完全一致)
:param msgid: 要查找的消息ID
:return: 条目字典 或 None
"""
low = 0
high = len(mavlink_message_crcs) - 1
while low < high:
mid = (low + 1 + high) // 2 # 注意+1的偏移量，防止死循环
current_id = mavlink_message_crcs[mid]['msgid']
if msgid < current_id:
high = mid - 1
elif msgid > current_id:
low = mid
else:
low = mid
break
if mavlink_message_crcs[low]['msgid'] != msgid:
return None
return mavlink_message_crcs[low]
def mavlink_get_crc_extra(msg):
"""
获取crc_extra的最终接口（返回十六进制字符串）
:param msg: 包含msgid的字典
:return: 十六进制字符串 如 0x7C
"""
entry = mavlink_get_msg_entry(msg['msgid'])
return entry['crc_extra'] if entry else 0 # 直接返回整数
# return "0x{:02X}".format(entry['crc_extra']) if entry else "0x00"
# 使用示例
if __name__ == '__main__':
# 测试查找msgid=1的条目（已知crc_extra应为124）
test_msg = {'msgid': 1}
print(f"CRC_EXTRA for msgid=1: {mavlink_get_crc_extra(test_msg)}") # 应输出124
# 测试不存在的msgid
test_msg = {'msgid': 99999}
print(f"CRC_EXTRA for msgid=99999: {mavlink_get_crc_extra(test_msg)}") # 应输出0
以及模拟测试程序：
import struct
from mavlink_parser import MavLinkV2Parser # 假设上述代码保存为 mavlink_parser.py
simulated_data = bytes.fromhex(
"FD5B00008001C800000001000000030405060708091011121314151617181920212223242526272829303101020304050607080910111213141516171819202122232425262728323334353637383940414243444546474849505152535400000400d80403D106"
# "FD0900008001C8000000000000000400d80403F1BF"
)
def main():
parser = MavLinkV2Parser()
# 注入模拟数据
messages = parser.parse(simulated_data)
print("messages",messages)
if messages:
msg = messages[0]
if msg['crc_valid']:
print("[成功] 消息解析通过！")
print(f"消息名称: {parser.message_definitions.get(msg['header']['msg_id'], {}).get('name', '未知')}")
print(f"系统ID: {msg['header']['system_id']}, 组件ID: {msg['header']['component_id']}")
print("负载内容:")
for key, value in msg['payload'].items():
print(f" - {key}: {value}")
else:
print("[错误] CRC校验失败！")
else:
print("[错误] 未检测到有效消息！")
if __name__ == "__main__":
main()
扫一扫
1021
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
