mav2pcap.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright 2012, Holger Steinhaus
00004 # Released under the GNU GPL version 3 or later
00005 
00006 # This program packetizes a binary MAVLink stream. The resulting packets are stored into a PCAP file, which is
00007 # compatible to tools like Wireshark.
00008 
00009 # The program tries to synchronize to the packet structure in a robust way, using the SOF magic, the potential
00010 # packet length information and the next SOF magic. Additionally the CRC is verified.
00011 
00012 # Hint: A MAVLink protocol dissector (parser) for Wireshark may be generated by mavgen.py.
00013 
00014 # dependency: Python construct library (python-construct on Debian/Ubuntu), "easy_install construct" elsewhere
00015 
00016 
00017 import sys
00018 import os
00019 
00020 from pymavlink import mavutil
00021 
00022 from construct import ULInt16, Struct, Byte, Bytes, Const
00023 from construct.core import FieldError
00024 from argparse import ArgumentParser, FileType
00025 
00026 
00027 MAVLINK_MAGIC = 0xfe
00028 write_junk = True
00029 
00030 # copied from ardupilotmega.h (git changeset 694536afb882068f50da1fc296944087aa207f9f, Dec 02 2012
00031 MAVLINK_MESSAGE_CRCS  = (50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 242, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 0, 0, 0, 0, 235, 93, 124, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 1, 234, 73, 181, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0)
00032 
00033 
00034 import __builtin__
00035 import struct
00036 
00037 # Helper class for writing pcap files
00038 class pcap:
00039     """
00040        Used under the terms of GNU GPL v3.
00041        Original author: Neale Pickett
00042        see http://dirtbags.net/py-pcap.html
00043     """
00044     _MAGIC = 0xA1B2C3D4
00045     def __init__(self, stream, mode='rb', snaplen=65535, linktype=1):
00046         try:
00047             self.stream = __builtin__.open(stream, mode)
00048         except TypeError:
00049             self.stream = stream
00050         try:
00051             # Try reading
00052             hdr = self.stream.read(24)
00053         except IOError:
00054             hdr = None
00055 
00056         if hdr:
00057             # We're in read mode
00058             self._endian = pcap.None
00059             for endian in '<>':
00060                 (self.magic,) = struct.unpack(endian + 'I', hdr[:4])
00061                 if self.magic == pcap._MAGIC:
00062                     self._endian = endian
00063                     break
00064             if not self._endian:
00065                 raise IOError('Not a pcap file')
00066             (self.magic, version_major, version_minor,
00067              self.thiszone, self.sigfigs,
00068              self.snaplen, self.linktype) = struct.unpack(self._endian + 'IHHIIII', hdr)
00069             if (version_major, version_minor) != (2, 4):
00070                 raise IOError('Cannot handle file version %d.%d' % (version_major,
00071                                                                     version_minor))
00072         else:
00073             # We're in write mode
00074             self._endian = '='
00075             self.magic = pcap._MAGIC
00076             version_major = 2
00077             version_minor = 4
00078             self.thiszone = 0
00079             self.sigfigs = 0
00080             self.snaplen = snaplen
00081             self.linktype = linktype
00082             hdr = struct.pack(self._endian + 'IHHIIII',
00083                               self.magic, version_major, version_minor,
00084                               self.thiszone, self.sigfigs,
00085                               self.snaplen, self.linktype)
00086             self.stream.write(hdr)
00087         self.version = (version_major, version_minor)
00088 
00089     def read(self):
00090         hdr = self.stream.read(16)
00091         if not hdr:
00092             return
00093         (tv_sec, tv_usec, caplen, length) = struct.unpack(self._endian + 'IIII', hdr)
00094         datum = self.stream.read(caplen)
00095         return ((tv_sec, tv_usec, length), datum)
00096 
00097     def write(self, packet):
00098         (header, datum) = packet
00099         (tv_sec, tv_usec, length) = header
00100         hdr = struct.pack(self._endian + 'IIII', tv_sec, tv_usec, length, len(datum))
00101         self.stream.write(hdr)
00102         self.stream.write(datum)
00103 
00104     def __iter__(self):
00105         while True:
00106             r = self.read()
00107             if not r:
00108                 break
00109             yield r
00110 
00111 
00112 def find_next_frame(data):
00113     """
00114     find a potential start of frame
00115     """
00116     return data.find('\xfe')
00117 
00118 
00119 def parse_header(data):
00120     """
00121     split up header information (using construct)
00122     """
00123     mavlink_header = Struct('header',
00124         Const(Byte('magic'), MAVLINK_MAGIC),
00125         Byte('plength'),
00126         Byte('sequence'),
00127         Byte('sysid'),
00128         Byte('compid'),
00129         Byte('msgid'),
00130     )
00131     return mavlink_header.parse(data[0:6])
00132 
00133 
00134 def write_packet(number, data, flags, pkt_length):
00135     pcap_header = (number, flags, pkt_length)
00136     pcap_file.write((pcap_header, data))
00137 
00138 
00139 def convert_file(mavlink_file, pcap_file):
00140     # the whole file is read in a bunch - room for improvement...
00141     data = mavlink_file.read()
00142 
00143     i=0
00144     done = False
00145     skipped_char = None
00146     junk = ''
00147     cnt_ok = 0
00148     cnt_junk = 0
00149     cnt_crc = 0
00150 
00151     while not done:
00152         i+=1
00153         # look for potential start of frame
00154         next_sof = find_next_frame(data)
00155         if next_sof > 0:
00156             print("skipped " + str(next_sof) + " bytes")
00157             if write_junk:
00158                 if skipped_char != None:
00159                     junk = skipped_char + data[:next_sof]
00160                     skipped_char = None
00161                 write_packet(i, junk, 0x03, len(junk))
00162             data = data[next_sof:]
00163             data[:6]
00164             cnt_junk += 1
00165 
00166         # assume, our 0xFE was the start of a packet
00167         header = parse_header(data)
00168         payload_len = header['plength']
00169         pkt_length = 6 + payload_len + 2
00170         try:
00171             pkt_crc = ULInt16('crc').parse(data[pkt_length-2:pkt_length])
00172         except FieldError:
00173             # ups, no more data
00174             done = True
00175             continue
00176 
00177         # peek for the next SOF
00178         try:
00179             cc = mavutil.x25crc(data[1:6+payload_len])
00180             cc.accumulate(chr(MAVLINK_MESSAGE_CRCS[header['msgid']]))
00181             x25_crc = cc.crc
00182             if x25_crc != pkt_crc:
00183                 crc_flag = 0x1
00184             else:
00185                 crc_flag = 0
00186             next_magic = data[pkt_length]
00187             if chr(MAVLINK_MAGIC) != next_magic:
00188                 # damn, retry
00189                 print("packet %d has invalid length, crc error: %d" % (i, crc_flag))
00190 
00191                 # skip one char to look for a new SOF next round, stow away skipped char
00192                 skipped_char = data[0]
00193                 data = data[1:]
00194                 continue
00195 
00196             # we can consider it a packet now
00197             pkt = data[:pkt_length]
00198             write_packet(i, pkt, crc_flag, len(pkt))
00199             print("packet %d ok, crc error: %d" % (i, crc_flag))
00200             data = data[pkt_length:]
00201 
00202             if crc_flag:
00203                 cnt_crc += 1
00204             else:
00205                 cnt_ok += 1
00206 
00207 
00208         except IndexError:
00209             # ups, no more packets
00210             done = True
00211     print("converted %d valid packets, %d crc errors, %d junk fragments (total %f%% of junk)" % (cnt_ok, cnt_crc, cnt_junk, 100.*float(cnt_junk+cnt_crc)/(cnt_junk+cnt_ok+cnt_crc)))
00212 
00213 ###############################################################################
00214 
00215 parser = ArgumentParser()
00216 parser.add_argument("input_file", type=FileType('rb'))
00217 parser.add_argument("output_file", type=FileType('wb'))
00218 args = parser.parse_args()
00219 
00220 
00221 mavlink_file = args.input_file
00222 args.output_file.close()
00223 pcap_file = pcap(args.output_file.name, args.output_file.mode, linktype=147) # special trick: linktype USER0
00224 
00225 convert_file(mavlink_file, pcap_file)
00226 
00227 mavlink_file.close()
00228 #pcap_file.close()


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57