watchrobot/modules/mavlink/pymavlink/examples/mav2pcap.py

#!/usr/bin/env python

# Copyright 2012, Holger Steinhaus
# Released under the GNU GPL version 3 or later

# This program packetizes a binary MAVLink stream. The resulting packets are stored into a PCAP file, which is
# compatible to tools like Wireshark.

# The program tries to synchronize to the packet structure in a robust way, using the SOF magic, the potential
# packet length information and the next SOF magic. Additionally the CRC is verified.

# Hint: A MAVLink protocol dissector (parser) for Wireshark may be generated by mavgen.py.

# dependency: Python construct library (python-construct on Debian/Ubuntu), "easy_install construct" elsewhere


from __future__ import print_function
from future import standard_library
standard_library.install_aliases()
from builtins import object
from builtins import open
import sys
import os

from pymavlink import mavutil

from construct import ULInt16, Struct, Byte, Bytes, Const
from construct.core import FieldError
from argparse import ArgumentParser, FileType


MAVLINK_MAGIC = 0xfe
write_junk = True

# copied from ardupilotmega.h (git changeset 694536afb882068f50da1fc296944087aa207f9f, Dec 02 2012
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)


import struct

# Helper class for writing pcap files
class pcap(object):
    """
       Used under the terms of GNU GPL v3.
       Original author: Neale Pickett
       see http://dirtbags.net/py-pcap.html
    """
    _MAGIC = 0xA1B2C3D4
    def __init__(self, stream, mode='rb', snaplen=65535, linktype=1):
        try:
            self.stream = open(stream, mode)
        except TypeError:
            self.stream = stream
        try:
            # Try reading
            hdr = self.stream.read(24)
        except IOError:
            hdr = None

        if hdr:
            # We're in read mode
            self._endian = None
            for endian in '<>':
                (self.magic,) = struct.unpack(endian + 'I', hdr[:4])
                if self.magic == pcap._MAGIC:
                    self._endian = endian
                    break
            if not self._endian:
                raise IOError('Not a pcap file')
            (self.magic, version_major, version_minor,
             self.thiszone, self.sigfigs,
             self.snaplen, self.linktype) = struct.unpack(self._endian + 'IHHIIII', hdr)
            if (version_major, version_minor) != (2, 4):
                raise IOError('Cannot handle file version %d.%d' % (version_major,
                                                                    version_minor))
        else:
            # We're in write mode
            self._endian = '='
            self.magic = pcap._MAGIC
            version_major = 2
            version_minor = 4
            self.thiszone = 0
            self.sigfigs = 0
            self.snaplen = snaplen
            self.linktype = linktype
            hdr = struct.pack(self._endian + 'IHHIIII',
                              self.magic, version_major, version_minor,
                              self.thiszone, self.sigfigs,
                              self.snaplen, self.linktype)
            self.stream.write(hdr)
        self.version = (version_major, version_minor)

    def read(self):
        hdr = self.stream.read(16)
        if not hdr:
            return
        (tv_sec, tv_usec, caplen, length) = struct.unpack(self._endian + 'IIII', hdr)
        datum = self.stream.read(caplen)
        return ((tv_sec, tv_usec, length), datum)

    def write(self, packet):
        (header, datum) = packet
        (tv_sec, tv_usec, length) = header
        hdr = struct.pack(self._endian + 'IIII', tv_sec, tv_usec, length, len(datum))
        self.stream.write(hdr)
        self.stream.write(datum)

    def __iter__(self):
        while True:
            r = self.read()
            if not r:
                break
            yield r


def find_next_frame(data):
    """
    find a potential start of frame
    """
    return data.find('\xfe')


def parse_header(data):
    """
    split up header information (using construct)
    """
    mavlink_header = Struct('header',
        Const(Byte('magic'), MAVLINK_MAGIC),
        Byte('plength'),
        Byte('sequence'),
        Byte('sysid'),
        Byte('compid'),
        Byte('msgid'),
    )
    return mavlink_header.parse(data[0:6])


def write_packet(number, data, flags, pkt_length):
    pcap_header = (number, flags, pkt_length)
    pcap_file.write((pcap_header, data))


def convert_file(mavlink_file, pcap_file):
    # the whole file is read in a bunch - room for improvement...
    data = mavlink_file.read()

    i=0
    done = False
    skipped_char = None
    junk = ''
    cnt_ok = 0
    cnt_junk = 0
    cnt_crc = 0

    while not done:
        i+=1
        # look for potential start of frame
        next_sof = find_next_frame(data)
        if next_sof > 0:
            print("skipped " + str(next_sof) + " bytes")
            if write_junk:
                if skipped_char is not None:
                    junk = skipped_char + data[:next_sof]
                    skipped_char = None
                write_packet(i, junk, 0x03, len(junk))
            data = data[next_sof:]
            data[:6]
            cnt_junk += 1

        # assume, our 0xFE was the start of a packet
        header = parse_header(data)
        payload_len = header['plength']
        pkt_length = 6 + payload_len + 2
        try:
            pkt_crc = ULInt16('crc').parse(data[pkt_length-2:pkt_length])
        except FieldError:
            # ups, no more data
            done = True
            continue

        # peek for the next SOF
        try:
            cc = mavutil.x25crc(data[1:6+payload_len])
            cc.accumulate(chr(MAVLINK_MESSAGE_CRCS[header['msgid']]))
            x25_crc = cc.crc
            if x25_crc != pkt_crc:
                crc_flag = 0x1
            else:
                crc_flag = 0
            next_magic = data[pkt_length]
            if chr(MAVLINK_MAGIC) != next_magic:
                # damn, retry
                print("packet %d has invalid length, crc error: %d" % (i, crc_flag))

                # skip one char to look for a new SOF next round, stow away skipped char
                skipped_char = data[0]
                data = data[1:]
                continue

            # we can consider it a packet now
            pkt = data[:pkt_length]
            write_packet(i, pkt, crc_flag, len(pkt))
            print("packet %d ok, crc error: %d" % (i, crc_flag))
            data = data[pkt_length:]

            if crc_flag:
                cnt_crc += 1
            else:
                cnt_ok += 1


        except IndexError:
            # ups, no more packets
            done = True
    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)))

###############################################################################

parser = ArgumentParser()
parser.add_argument("input_file", type=FileType('rb'))
parser.add_argument("output_file", type=FileType('wb'))
args = parser.parse_args()


mavlink_file = args.input_file
args.output_file.close()
pcap_file = pcap(args.output_file.name, args.output_file.mode, linktype=147) # special trick: linktype USER0

convert_file(mavlink_file, pcap_file)

mavlink_file.close()
#pcap_file.close()