mav2pcap.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright 2012, Holger Steinhaus
4 # Released under the GNU GPL version 3 or later
5 
6 # This program packetizes a binary MAVLink stream. The resulting packets are stored into a PCAP file, which is
7 # compatible to tools like Wireshark.
8 
9 # The program tries to synchronize to the packet structure in a robust way, using the SOF magic, the potential
10 # packet length information and the next SOF magic. Additionally the CRC is verified.
11 
12 # Hint: A MAVLink protocol dissector (parser) for Wireshark may be generated by mavgen.py.
13 
14 # dependency: Python construct library (python-construct on Debian/Ubuntu), "easy_install construct" elsewhere
15 
16 
17 from __future__ import print_function
18 from future import standard_library
19 standard_library.install_aliases()
20 from builtins import object
21 from builtins import open
22 import sys
23 import os
24 
25 from pymavlink import mavutil
26 
27 from construct import ULInt16, Struct, Byte, Bytes, Const
28 from construct.core import FieldError
29 from argparse import ArgumentParser, FileType
30 
31 
32 MAVLINK_MAGIC = 0xfe
33 write_junk = True
34 
35 # copied from ardupilotmega.h (git changeset 694536afb882068f50da1fc296944087aa207f9f, Dec 02 2012
36 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)
37 
38 
39 import struct
40 
41 # Helper class for writing pcap files
42 class pcap(object):
43  """
44  Used under the terms of GNU GPL v3.
45  Original author: Neale Pickett
46  see http://dirtbags.net/py-pcap.html
47  """
48  _MAGIC = 0xA1B2C3D4
49  def __init__(self, stream, mode='rb', snaplen=65535, linktype=1):
50  try:
51  self.stream = open(stream, mode)
52  except TypeError:
53  self.stream = stream
54  try:
55  # Try reading
56  hdr = self.stream.read(24)
57  except IOError:
58  hdr = None
59 
60  if hdr:
61  # We're in read mode
62  self._endian = None
63  for endian in '<>':
64  (self.magic,) = struct.unpack(endian + 'I', hdr[:4])
65  if self.magic == pcap._MAGIC:
66  self._endian = endian
67  break
68  if not self._endian:
69  raise IOError('Not a pcap file')
70  (self.magic, version_major, version_minor,
71  self.thiszone, self.sigfigs,
72  self.snaplen, self.linktype) = struct.unpack(self._endian + 'IHHIIII', hdr)
73  if (version_major, version_minor) != (2, 4):
74  raise IOError('Cannot handle file version %d.%d' % (version_major,
75  version_minor))
76  else:
77  # We're in write mode
78  self._endian = '='
79  self.magic = pcap._MAGIC
80  version_major = 2
81  version_minor = 4
82  self.thiszone = 0
83  self.sigfigs = 0
84  self.snaplen = snaplen
85  self.linktype = linktype
86  hdr = struct.pack(self._endian + 'IHHIIII',
87  self.magic, version_major, version_minor,
88  self.thiszone, self.sigfigs,
89  self.snaplen, self.linktype)
90  self.stream.write(hdr)
91  self.version = (version_major, version_minor)
92 
93  def read(self):
94  hdr = self.stream.read(16)
95  if not hdr:
96  return
97  (tv_sec, tv_usec, caplen, length) = struct.unpack(self._endian + 'IIII', hdr)
98  datum = self.stream.read(caplen)
99  return ((tv_sec, tv_usec, length), datum)
100 
101  def write(self, packet):
102  (header, datum) = packet
103  (tv_sec, tv_usec, length) = header
104  hdr = struct.pack(self._endian + 'IIII', tv_sec, tv_usec, length, len(datum))
105  self.stream.write(hdr)
106  self.stream.write(datum)
107 
108  def __iter__(self):
109  while True:
110  r = self.read()
111  if not r:
112  break
113  yield r
114 
115 
116 def find_next_frame(data):
117  """
118  find a potential start of frame
119  """
120  return data.find('\xfe')
121 
122 
123 def parse_header(data):
124  """
125  split up header information (using construct)
126  """
127  mavlink_header = Struct('header',
128  Const(Byte('magic'), MAVLINK_MAGIC),
129  Byte('plength'),
130  Byte('sequence'),
131  Byte('sysid'),
132  Byte('compid'),
133  Byte('msgid'),
134  )
135  return mavlink_header.parse(data[0:6])
136 
137 
138 def write_packet(number, data, flags, pkt_length):
139  pcap_header = (number, flags, pkt_length)
140  pcap_file.write((pcap_header, data))
141 
142 
143 def convert_file(mavlink_file, pcap_file):
144  # the whole file is read in a bunch - room for improvement...
145  data = mavlink_file.read()
146 
147  i=0
148  done = False
149  skipped_char = None
150  junk = ''
151  cnt_ok = 0
152  cnt_junk = 0
153  cnt_crc = 0
154 
155  while not done:
156  i+=1
157  # look for potential start of frame
158  next_sof = find_next_frame(data)
159  if next_sof > 0:
160  print("skipped " + str(next_sof) + " bytes")
161  if write_junk:
162  if skipped_char is not None:
163  junk = skipped_char + data[:next_sof]
164  skipped_char = None
165  write_packet(i, junk, 0x03, len(junk))
166  data = data[next_sof:]
167  data[:6]
168  cnt_junk += 1
169 
170  # assume, our 0xFE was the start of a packet
171  header = parse_header(data)
172  payload_len = header['plength']
173  pkt_length = 6 + payload_len + 2
174  try:
175  pkt_crc = ULInt16('crc').parse(data[pkt_length-2:pkt_length])
176  except FieldError:
177  # ups, no more data
178  done = True
179  continue
180 
181  # peek for the next SOF
182  try:
183  cc = mavutil.x25crc(data[1:6+payload_len])
184  cc.accumulate(chr(MAVLINK_MESSAGE_CRCS[header['msgid']]))
185  x25_crc = cc.crc
186  if x25_crc != pkt_crc:
187  crc_flag = 0x1
188  else:
189  crc_flag = 0
190  next_magic = data[pkt_length]
191  if chr(MAVLINK_MAGIC) != next_magic:
192  # damn, retry
193  print("packet %d has invalid length, crc error: %d" % (i, crc_flag))
194 
195  # skip one char to look for a new SOF next round, stow away skipped char
196  skipped_char = data[0]
197  data = data[1:]
198  continue
199 
200  # we can consider it a packet now
201  pkt = data[:pkt_length]
202  write_packet(i, pkt, crc_flag, len(pkt))
203  print("packet %d ok, crc error: %d" % (i, crc_flag))
204  data = data[pkt_length:]
205 
206  if crc_flag:
207  cnt_crc += 1
208  else:
209  cnt_ok += 1
210 
211 
212  except IndexError:
213  # ups, no more packets
214  done = True
215  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)))
216 
217 ###############################################################################
218 
219 parser = ArgumentParser()
220 parser.add_argument("input_file", type=FileType('rb'))
221 parser.add_argument("output_file", type=FileType('wb'))
222 args = parser.parse_args()
223 
224 
225 mavlink_file = args.input_file
226 args.output_file.close()
227 pcap_file = pcap(args.output_file.name, args.output_file.mode, linktype=147) # special trick: linktype USER0
228 
229 convert_file(mavlink_file, pcap_file)
230 
231 mavlink_file.close()
232 #pcap_file.close()

mavlink
Author(s): Lorenz Meier
autogenerated on Sun Apr 7 2019 02:06:02