mavgen_python.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 '''
3 parse a MAVLink protocol XML file and generate a python implementation
4 
5 Copyright Andrew Tridgell 2011
6 Released under GNU GPL version 3 or later
7 '''
8 from __future__ import print_function
9 
10 from builtins import range
11 
12 import os
13 import textwrap
14 from . import mavtemplate
15 
17 
18 
19 def generate_preamble(outf, msgs, basename, args, xml):
20  print("Generating preamble")
21  t.write(outf, """
22 '''
23 MAVLink protocol implementation (auto-generated by mavgen.py)
24 
25 Generated from: ${FILELIST}
26 
27 Note: this file has been auto-generated. DO NOT EDIT
28 '''
29 from __future__ import print_function
30 from builtins import range
31 from builtins import object
32 import struct, array, time, json, os, sys, platform
33 
34 from ...generator.mavcrc import x25crc
35 import hashlib
36 
37 WIRE_PROTOCOL_VERSION = '${WIRE_PROTOCOL_VERSION}'
38 DIALECT = '${DIALECT}'
39 
40 PROTOCOL_MARKER_V1 = 0xFE
41 PROTOCOL_MARKER_V2 = 0xFD
42 HEADER_LEN_V1 = 6
43 HEADER_LEN_V2 = 10
44 
45 MAVLINK_SIGNATURE_BLOCK_LEN = 13
46 
47 MAVLINK_IFLAG_SIGNED = 0x01
48 
49 native_supported = platform.system() != 'Windows' # Not yet supported on other dialects
50 native_force = 'MAVNATIVE_FORCE' in os.environ # Will force use of native code regardless of what client app wants
51 native_testing = 'MAVNATIVE_TESTING' in os.environ # Will force both native and legacy code to be used and their results compared
52 
53 if native_supported and float(WIRE_PROTOCOL_VERSION) <= 1:
54  try:
55  import mavnative
56  except ImportError:
57  print('ERROR LOADING MAVNATIVE - falling back to python implementation')
58  native_supported = False
59 else:
60  # mavnative isn't supported for MAVLink2 yet
61  native_supported = False
62 
63 # some base types from mavlink_types.h
64 MAVLINK_TYPE_CHAR = 0
65 MAVLINK_TYPE_UINT8_T = 1
66 MAVLINK_TYPE_INT8_T = 2
67 MAVLINK_TYPE_UINT16_T = 3
68 MAVLINK_TYPE_INT16_T = 4
69 MAVLINK_TYPE_UINT32_T = 5
70 MAVLINK_TYPE_INT32_T = 6
71 MAVLINK_TYPE_UINT64_T = 7
72 MAVLINK_TYPE_INT64_T = 8
73 MAVLINK_TYPE_FLOAT = 9
74 MAVLINK_TYPE_DOUBLE = 10
75 
76 
77 class MAVLink_header(object):
78  '''MAVLink message header'''
79  def __init__(self, msgId, incompat_flags=0, compat_flags=0, mlen=0, seq=0, srcSystem=0, srcComponent=0):
80  self.mlen = mlen
81  self.seq = seq
82  self.srcSystem = srcSystem
83  self.srcComponent = srcComponent
84  self.msgId = msgId
85  self.incompat_flags = incompat_flags
86  self.compat_flags = compat_flags
87 
88  def pack(self, force_mavlink1=False):
89  if WIRE_PROTOCOL_VERSION == '2.0' and not force_mavlink1:
90  return struct.pack('<BBBBBBBHB', ${PROTOCOL_MARKER}, self.mlen,
91  self.incompat_flags, self.compat_flags,
92  self.seq, self.srcSystem, self.srcComponent,
93  self.msgId&0xFFFF, self.msgId>>16)
94  return struct.pack('<BBBBBB', PROTOCOL_MARKER_V1, self.mlen, self.seq,
95  self.srcSystem, self.srcComponent, self.msgId)
96 
97 class MAVLink_message(object):
98  '''base MAVLink message class'''
99  def __init__(self, msgId, name):
100  self._header = MAVLink_header(msgId)
101  self._payload = None
102  self._msgbuf = None
103  self._crc = None
104  self._fieldnames = []
105  self._type = name
106  self._signed = False
107  self._link_id = None
108 
109  def format_attr(self, field):
110  '''override field getter'''
111  raw_attr = getattr(self,field)
112  if isinstance(raw_attr, bytes):
113  raw_attr = raw_attr.decode("utf-8").rstrip("\\00")
114  return raw_attr
115 
116  def get_msgbuf(self):
117  if isinstance(self._msgbuf, bytearray):
118  return self._msgbuf
119  return bytearray(self._msgbuf)
120 
121  def get_header(self):
122  return self._header
123 
124  def get_payload(self):
125  return self._payload
126 
127  def get_crc(self):
128  return self._crc
129 
130  def get_fieldnames(self):
131  return self._fieldnames
132 
133  def get_type(self):
134  return self._type
135 
136  def get_msgId(self):
137  return self._header.msgId
138 
139  def get_srcSystem(self):
140  return self._header.srcSystem
141 
142  def get_srcComponent(self):
143  return self._header.srcComponent
144 
145  def get_seq(self):
146  return self._header.seq
147 
148  def get_signed(self):
149  return self._signed
150 
151  def get_link_id(self):
152  return self._link_id
153 
154  def __str__(self):
155  ret = '%s {' % self._type
156  for a in self._fieldnames:
157  v = self.format_attr(a)
158  ret += '%s : %s, ' % (a, v)
159  ret = ret[0:-2] + '}'
160  return ret
161 
162  def __ne__(self, other):
163  return not self.__eq__(other)
164 
165  def __eq__(self, other):
166  if other is None:
167  return False
168 
169  if self.get_type() != other.get_type():
170  return False
171 
172  # We do not compare CRC because native code doesn't provide it
173  #if self.get_crc() != other.get_crc():
174  # return False
175 
176  if self.get_seq() != other.get_seq():
177  return False
178 
179  if self.get_srcSystem() != other.get_srcSystem():
180  return False
181 
182  if self.get_srcComponent() != other.get_srcComponent():
183  return False
184 
185  for a in self._fieldnames:
186  if self.format_attr(a) != other.format_attr(a):
187  return False
188 
189  return True
190 
191  def to_dict(self):
192  d = dict({})
193  d['mavpackettype'] = self._type
194  for a in self._fieldnames:
195  d[a] = self.format_attr(a)
196  return d
197 
198  def to_json(self):
199  return json.dumps(self.to_dict())
200 
201  def sign_packet(self, mav):
202  h = hashlib.new('sha256')
203  self._msgbuf += struct.pack('<BQ', mav.signing.link_id, mav.signing.timestamp)[:7]
204  h.update(mav.signing.secret_key)
205  h.update(self._msgbuf)
206  sig = h.digest()[:6]
207  self._msgbuf += sig
208  mav.signing.timestamp += 1
209 
210  def pack(self, mav, crc_extra, payload, force_mavlink1=False):
211  plen = len(payload)
212  if WIRE_PROTOCOL_VERSION != '1.0' and not force_mavlink1:
213  # in MAVLink2 we can strip trailing zeros off payloads. This allows for simple
214  # variable length arrays and smaller packets
215  while plen > 1 and payload[plen-1] == chr(0):
216  plen -= 1
217  self._payload = payload[:plen]
218  incompat_flags = 0
219  if mav.signing.sign_outgoing:
220  incompat_flags |= MAVLINK_IFLAG_SIGNED
221  self._header = MAVLink_header(self._header.msgId,
222  incompat_flags=incompat_flags, compat_flags=0,
223  mlen=len(self._payload), seq=mav.seq,
224  srcSystem=mav.srcSystem, srcComponent=mav.srcComponent)
225  self._msgbuf = self._header.pack(force_mavlink1=force_mavlink1) + self._payload
226  crc = x25crc(self._msgbuf[1:])
227  if ${crc_extra}: # using CRC extra
228  crc.accumulate_str(struct.pack('B', crc_extra))
229  self._crc = crc.crc
230  self._msgbuf += struct.pack('<H', self._crc)
231  if mav.signing.sign_outgoing and not force_mavlink1:
232  self.sign_packet(mav)
233  return self._msgbuf
234 
235 """, {'FILELIST': ",".join(args),
236  'PROTOCOL_MARKER': xml.protocol_marker,
237  'DIALECT': os.path.splitext(os.path.basename(basename))[0],
238  'crc_extra': xml.crc_extra,
239  'WIRE_PROTOCOL_VERSION': xml.wire_protocol_version})
240 
241 
242 def generate_enums(outf, enums):
243  print("Generating enums")
244  outf.write('''
245 # enums
246 
247 class EnumEntry(object):
248  def __init__(self, name, description):
249  self.name = name
250  self.description = description
251  self.param = {}
252 
253 enums = {}
254 ''')
255  wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent=" # ")
256  for e in enums:
257  outf.write("\n# %s\n" % e.name)
258  outf.write("enums['%s'] = {}\n" % e.name)
259  for entry in e.entry:
260  outf.write("%s = %u # %s\n" % (entry.name, entry.value, wrapper.fill(entry.description)))
261  outf.write("enums['%s'][%d] = EnumEntry('%s', '''%s''')\n" % (e.name,
262  int(entry.value), entry.name,
263  entry.description))
264  for param in entry.param:
265  outf.write("enums['%s'][%d].param[%d] = '''%s'''\n" % (e.name,
266  int(entry.value),
267  int(param.index),
268  param.description))
269 
270 
271 def generate_message_ids(outf, msgs):
272  print("Generating message IDs")
273  outf.write("\n# message IDs\n")
274  outf.write("MAVLINK_MSG_ID_BAD_DATA = -1\n")
275  for m in msgs:
276  outf.write("MAVLINK_MSG_ID_%s = %u\n" % (m.name.upper(), m.id))
277 
278 
279 def generate_classes(outf, msgs):
280  print("Generating class definitions")
281  wrapper = textwrap.TextWrapper(initial_indent=" ", subsequent_indent=" ")
282  for m in msgs:
283  classname = "MAVLink_%s_message" % m.name.lower()
284  fieldname_str = ", ".join(["'%s'" % s for s in m.fieldnames])
285  ordered_fieldname_str = ", ".join(["'%s'" % s for s in m.ordered_fieldnames])
286 
287  fieldtypes_str = ", ".join(["'%s'" % s for s in m.fieldtypes])
288  outf.write("""
289 class %s(MAVLink_message):
290  '''
291 %s
292  '''
293  id = MAVLINK_MSG_ID_%s
294  name = '%s'
295  fieldnames = [%s]
296  ordered_fieldnames = [%s]
297  fieldtypes = [%s]
298  format = '%s'
299  native_format = bytearray('%s', 'ascii')
300  orders = %s
301  lengths = %s
302  array_lengths = %s
303  crc_extra = %s
304  unpacker = struct.Struct('%s')
305 
306  def __init__(self""" % (classname, wrapper.fill(m.description.strip()),
307  m.name.upper(),
308  m.name.upper(),
309  fieldname_str,
310  ordered_fieldname_str,
311  fieldtypes_str,
312  m.fmtstr,
313  m.native_fmtstr,
314  m.order_map,
315  m.len_map,
316  m.array_len_map,
317  m.crc_extra,
318  m.fmtstr))
319  for i in range(len(m.fields)):
320  fname = m.fieldnames[i]
321  if m.extensions_start is not None and i >= m.extensions_start:
322  fdefault = m.fielddefaults[i]
323  outf.write(", %s=%s" % (fname, fdefault))
324  else:
325  outf.write(", %s" % fname)
326  outf.write("):\n")
327  outf.write(" MAVLink_message.__init__(self, %s.id, %s.name)\n" % (classname, classname))
328  outf.write(" self._fieldnames = %s.fieldnames\n" % (classname))
329  for f in m.fields:
330  outf.write(" self.%s = %s\n" % (f.name, f.name))
331  outf.write("""
332  def pack(self, mav, force_mavlink1=False):
333  return MAVLink_message.pack(self, mav, %u, struct.pack('%s'""" % (m.crc_extra, m.fmtstr))
334  for field in m.ordered_fields:
335  if (field.type != "char" and field.array_length > 1):
336  for i in range(field.array_length):
337  outf.write(", self.{0:s}[{1:d}]".format(field.name, i))
338  else:
339  outf.write(", self.{0:s}".format(field.name))
340  outf.write("), force_mavlink1=force_mavlink1)\n")
341 
342 
343 def native_mavfmt(field):
344  '''work out the struct format for a type (in a form expected by mavnative)'''
345  map = {
346  'float': 'f',
347  'double': 'd',
348  'char': 'c',
349  'int8_t': 'b',
350  'uint8_t': 'B',
351  'uint8_t_mavlink_version': 'v',
352  'int16_t': 'h',
353  'uint16_t': 'H',
354  'int32_t': 'i',
355  'uint32_t': 'I',
356  'int64_t': 'q',
357  'uint64_t': 'Q',
358  }
359  return map[field.type]
360 
361 
362 def mavfmt(field):
363  '''work out the struct format for a type'''
364  map = {
365  'float': 'f',
366  'double': 'd',
367  'char': 'c',
368  'int8_t': 'b',
369  'uint8_t': 'B',
370  'uint8_t_mavlink_version': 'B',
371  'int16_t': 'h',
372  'uint16_t': 'H',
373  'int32_t': 'i',
374  'uint32_t': 'I',
375  'int64_t': 'q',
376  'uint64_t': 'Q',
377  }
378 
379  if field.array_length:
380  if field.type == 'char':
381  return str(field.array_length)+'s'
382  return str(field.array_length)+map[field.type]
383  return map[field.type]
384 
385 
386 def mavdefault(field):
387  '''returns default value for field (as string) for mavlink2 extensions'''
388  if field.type == 'char':
389  default_value = "''"
390  else:
391  default_value = "0"
392  if field.array_length == 0:
393  return default_value
394  return "[" + ",".join([default_value] * field.array_length) + "]"
395 
396 
397 def generate_mavlink_class(outf, msgs, xml):
398  print("Generating MAVLink class")
399 
400  outf.write("\n\nmavlink_map = {\n")
401  for m in msgs:
402  outf.write(" MAVLINK_MSG_ID_%s : MAVLink_%s_message,\n" % (
403  m.name.upper(), m.name.lower()))
404  outf.write("}\n\n")
405 
406  t.write(outf, """
407 class MAVError(Exception):
408  '''MAVLink error class'''
409  def __init__(self, msg):
410  Exception.__init__(self, msg)
411  self.message = msg
412 
413 class MAVString(str):
414  '''NUL terminated string'''
415  def __init__(self, s):
416  str.__init__(self)
417  def __str__(self):
418  i = self.find(chr(0))
419  if i == -1:
420  return self[:]
421  return self[0:i]
422 
423 class MAVLink_bad_data(MAVLink_message):
424  '''
425  a piece of bad data in a mavlink stream
426  '''
427  def __init__(self, data, reason):
428  MAVLink_message.__init__(self, MAVLINK_MSG_ID_BAD_DATA, 'BAD_DATA')
429  self._fieldnames = ['data', 'reason']
430  self.data = data
431  self.reason = reason
432  self._msgbuf = data
433 
434  def __str__(self):
435  '''Override the __str__ function from MAVLink_messages because non-printable characters are common in to be the reason for this message to exist.'''
436  return '%s {%s, data:%s}' % (self._type, self.reason, [('%x' % ord(i) if isinstance(i, str) else '%x' % i) for i in self.data])
437 
438 class MAVLinkSigning(object):
439  '''MAVLink signing state class'''
440  def __init__(self):
441  self.secret_key = None
442  self.timestamp = 0
443  self.link_id = 0
444  self.sign_outgoing = False
445  self.allow_unsigned_callback = None
446  self.stream_timestamps = {}
447  self.sig_count = 0
448  self.badsig_count = 0
449  self.goodsig_count = 0
450  self.unsigned_count = 0
451  self.reject_count = 0
452 
453 class MAVLink(object):
454  '''MAVLink protocol handling class'''
455  def __init__(self, file, srcSystem=0, srcComponent=0, use_native=False):
456  self.seq = 0
457  self.file = file
458  self.srcSystem = srcSystem
459  self.srcComponent = srcComponent
460  self.callback = None
461  self.callback_args = None
462  self.callback_kwargs = None
463  self.send_callback = None
464  self.send_callback_args = None
465  self.send_callback_kwargs = None
466  self.buf = bytearray()
467  self.buf_index = 0
468  self.expected_length = HEADER_LEN_V1+2
469  self.have_prefix_error = False
470  self.robust_parsing = False
471  self.protocol_marker = ${protocol_marker}
472  self.little_endian = ${little_endian}
473  self.crc_extra = ${crc_extra}
474  self.sort_fields = ${sort_fields}
475  self.total_packets_sent = 0
476  self.total_bytes_sent = 0
477  self.total_packets_received = 0
478  self.total_bytes_received = 0
479  self.total_receive_errors = 0
480  self.startup_time = time.time()
481  self.signing = MAVLinkSigning()
482  if native_supported and (use_native or native_testing or native_force):
483  print("NOTE: mavnative is currently beta-test code")
484  self.native = mavnative.NativeConnection(MAVLink_message, mavlink_map)
485  else:
486  self.native = None
487  if native_testing:
488  self.test_buf = bytearray()
489  self.mav20_unpacker = struct.Struct('<cBBBBBBHB')
490  self.mav10_unpacker = struct.Struct('<cBBBBB')
491  self.mav20_h3_unpacker = struct.Struct('BBB')
492  self.mav_csum_unpacker = struct.Struct('<H')
493  self.mav_sign_unpacker = struct.Struct('<IH')
494 
495  def set_callback(self, callback, *args, **kwargs):
496  self.callback = callback
497  self.callback_args = args
498  self.callback_kwargs = kwargs
499 
500  def set_send_callback(self, callback, *args, **kwargs):
501  self.send_callback = callback
502  self.send_callback_args = args
503  self.send_callback_kwargs = kwargs
504 
505  def send(self, mavmsg, force_mavlink1=False):
506  '''send a MAVLink message'''
507  buf = mavmsg.pack(self, force_mavlink1=force_mavlink1)
508  self.file.write(buf)
509  self.seq = (self.seq + 1) % 256
510  self.total_packets_sent += 1
511  self.total_bytes_sent += len(buf)
512  if self.send_callback:
513  self.send_callback(mavmsg, *self.send_callback_args, **self.send_callback_kwargs)
514 
515  def buf_len(self):
516  return len(self.buf) - self.buf_index
517 
518  def bytes_needed(self):
519  '''return number of bytes needed for next parsing stage'''
520  if self.native:
521  ret = self.native.expected_length - self.buf_len()
522  else:
523  ret = self.expected_length - self.buf_len()
524 
525  if ret <= 0:
526  return 1
527  return ret
528 
529  def __parse_char_native(self, c):
530  '''this method exists only to see in profiling results'''
531  m = self.native.parse_chars(c)
532  return m
533 
534  def __callbacks(self, msg):
535  '''this method exists only to make profiling results easier to read'''
536  if self.callback:
537  self.callback(msg, *self.callback_args, **self.callback_kwargs)
538 
539  def parse_char(self, c):
540  '''input some data bytes, possibly returning a new message'''
541  self.buf.extend(c)
542 
543  self.total_bytes_received += len(c)
544 
545  if self.native:
546  if native_testing:
547  self.test_buf.extend(c)
548  m = self.__parse_char_native(self.test_buf)
549  m2 = self.__parse_char_legacy()
550  if m2 != m:
551  print("Native: %s\\nLegacy: %s\\n" % (m, m2))
552  raise Exception('Native vs. Legacy mismatch')
553  else:
554  m = self.__parse_char_native(self.buf)
555  else:
556  m = self.__parse_char_legacy()
557 
558  if m is not None:
559  self.total_packets_received += 1
560  self.__callbacks(m)
561  else:
562  # XXX The idea here is if we've read something and there's nothing left in
563  # the buffer, reset it to 0 which frees the memory
564  if self.buf_len() == 0 and self.buf_index != 0:
565  self.buf = bytearray()
566  self.buf_index = 0
567 
568  return m
569 
570  def __parse_char_legacy(self):
571  '''input some data bytes, possibly returning a new message (uses no native code)'''
572  header_len = HEADER_LEN_V1
573  if self.buf_len() >= 1 and self.buf[self.buf_index] == PROTOCOL_MARKER_V2:
574  header_len = HEADER_LEN_V2
575 
576  if self.buf_len() >= 1 and self.buf[self.buf_index] != PROTOCOL_MARKER_V1 and self.buf[self.buf_index] != PROTOCOL_MARKER_V2:
577  magic = self.buf[self.buf_index]
578  self.buf_index += 1
579  if self.robust_parsing:
580  m = MAVLink_bad_data(bytearray([magic]), 'Bad prefix')
581  self.expected_length = header_len+2
582  self.total_receive_errors += 1
583  return m
584  if self.have_prefix_error:
585  return None
586  self.have_prefix_error = True
587  self.total_receive_errors += 1
588  raise MAVError("invalid MAVLink prefix '%s'" % magic)
589  self.have_prefix_error = False
590  if self.buf_len() >= 3:
591  sbuf = self.buf[self.buf_index:3+self.buf_index]
592  if sys.version_info.major < 3:
593  sbuf = str(sbuf)
594  (magic, self.expected_length, incompat_flags) = self.mav20_h3_unpacker.unpack(sbuf)
595  if magic == PROTOCOL_MARKER_V2 and (incompat_flags & MAVLINK_IFLAG_SIGNED):
596  self.expected_length += MAVLINK_SIGNATURE_BLOCK_LEN
597  self.expected_length += header_len + 2
598  if self.expected_length >= (header_len+2) and self.buf_len() >= self.expected_length:
599  mbuf = array.array('B', self.buf[self.buf_index:self.buf_index+self.expected_length])
600  self.buf_index += self.expected_length
601  self.expected_length = header_len+2
602  if self.robust_parsing:
603  try:
604  if magic == PROTOCOL_MARKER_V2 and (incompat_flags & ~MAVLINK_IFLAG_SIGNED) != 0:
605  raise MAVError('invalid incompat_flags 0x%x 0x%x %u' % (incompat_flags, magic, self.expected_length))
606  m = self.decode(mbuf)
607  except MAVError as reason:
608  m = MAVLink_bad_data(mbuf, reason.message)
609  self.total_receive_errors += 1
610  else:
611  if magic == PROTOCOL_MARKER_V2 and (incompat_flags & ~MAVLINK_IFLAG_SIGNED) != 0:
612  raise MAVError('invalid incompat_flags 0x%x 0x%x %u' % (incompat_flags, magic, self.expected_length))
613  m = self.decode(mbuf)
614  return m
615  return None
616 
617  def parse_buffer(self, s):
618  '''input some data bytes, possibly returning a list of new messages'''
619  m = self.parse_char(s)
620  if m is None:
621  return None
622  ret = [m]
623  while True:
624  m = self.parse_char("")
625  if m is None:
626  return ret
627  ret.append(m)
628  return ret
629 
630  def check_signature(self, msgbuf, srcSystem, srcComponent):
631  '''check signature on incoming message'''
632  if isinstance(msgbuf, array.array):
633  msgbuf = msgbuf.tostring()
634  timestamp_buf = msgbuf[-12:-6]
635  link_id = msgbuf[-13]
636  (tlow, thigh) = self.mav_sign_unpacker.unpack(timestamp_buf)
637  timestamp = tlow + (thigh<<32)
638 
639  # see if the timestamp is acceptable
640  stream_key = (link_id,srcSystem,srcComponent)
641  if stream_key in self.signing.stream_timestamps:
642  if timestamp <= self.signing.stream_timestamps[stream_key]:
643  # reject old timestamp
644  # print('old timestamp')
645  return False
646  else:
647  # a new stream has appeared. Accept the timestamp if it is at most
648  # one minute behind our current timestamp
649  if timestamp + 6000*1000 < self.signing.timestamp:
650  # print('bad new stream ', timestamp/(100.0*1000*60*60*24*365), self.signing.timestamp/(100.0*1000*60*60*24*365))
651  return False
652  self.signing.stream_timestamps[stream_key] = timestamp
653  # print('new stream')
654 
655  h = hashlib.new('sha256')
656  h.update(self.signing.secret_key)
657  h.update(msgbuf[:-6])
658  sig1 = str(h.digest())[:6]
659  sig2 = str(msgbuf)[-6:]
660  if sig1 != sig2:
661  # print('sig mismatch')
662  return False
663 
664  # the timestamp we next send with is the max of the received timestamp and
665  # our current timestamp
666  self.signing.timestamp = max(self.signing.timestamp, timestamp)
667  return True
668 
669  def decode(self, msgbuf):
670  '''decode a buffer as a MAVLink message'''
671  # decode the header
672  if msgbuf[0] != PROTOCOL_MARKER_V1:
673  headerlen = 10
674  try:
675  magic, mlen, incompat_flags, compat_flags, seq, srcSystem, srcComponent, msgIdlow, msgIdhigh = self.mav20_unpacker.unpack(msgbuf[:headerlen])
676  except struct.error as emsg:
677  raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
678  msgId = msgIdlow | (msgIdhigh<<16)
679  mapkey = msgId
680  else:
681  headerlen = 6
682  try:
683  magic, mlen, seq, srcSystem, srcComponent, msgId = self.mav10_unpacker.unpack(msgbuf[:headerlen])
684  incompat_flags = 0
685  compat_flags = 0
686  except struct.error as emsg:
687  raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
688  mapkey = msgId
689  if (incompat_flags & MAVLINK_IFLAG_SIGNED) != 0:
690  signature_len = MAVLINK_SIGNATURE_BLOCK_LEN
691  else:
692  signature_len = 0
693 
694  if ord(magic) != PROTOCOL_MARKER_V1 and ord(magic) != PROTOCOL_MARKER_V2:
695  raise MAVError("invalid MAVLink prefix '%s'" % magic)
696  if mlen != len(msgbuf)-(headerlen+2+signature_len):
697  raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u headerlen=%u' % (len(msgbuf)-(headerlen+2+signature_len), mlen, msgId, headerlen))
698 
699  if not mapkey in mavlink_map:
700  raise MAVError('unknown MAVLink message ID %s' % str(mapkey))
701 
702  # decode the payload
703  type = mavlink_map[mapkey]
704  fmt = type.format
705  order_map = type.orders
706  len_map = type.lengths
707  crc_extra = type.crc_extra
708 
709  # decode the checksum
710  try:
711  crc, = self.mav_csum_unpacker.unpack(msgbuf[-(2+signature_len):][:2])
712  except struct.error as emsg:
713  raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg)
714  crcbuf = msgbuf[1:-(2+signature_len)]
715  if ${crc_extra}: # using CRC extra
716  crcbuf.append(crc_extra)
717  crc2 = x25crc(crcbuf)
718  if crc != crc2.crc:
719  raise MAVError('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc))
720 
721  sig_ok = False
722  if signature_len == MAVLINK_SIGNATURE_BLOCK_LEN:
723  self.signing.sig_count += 1
724  if self.signing.secret_key is not None:
725  accept_signature = False
726  if signature_len == MAVLINK_SIGNATURE_BLOCK_LEN:
727  sig_ok = self.check_signature(msgbuf, srcSystem, srcComponent)
728  accept_signature = sig_ok
729  if sig_ok:
730  self.signing.goodsig_count += 1
731  else:
732  self.signing.badsig_count += 1
733  if not accept_signature and self.signing.allow_unsigned_callback is not None:
734  accept_signature = self.signing.allow_unsigned_callback(self, msgId)
735  if accept_signature:
736  self.signing.unsigned_count += 1
737  else:
738  self.signing.reject_count += 1
739  elif self.signing.allow_unsigned_callback is not None:
740  accept_signature = self.signing.allow_unsigned_callback(self, msgId)
741  if accept_signature:
742  self.signing.unsigned_count += 1
743  else:
744  self.signing.reject_count += 1
745  if not accept_signature:
746  raise MAVError('Invalid signature')
747 
748  csize = type.unpacker.size
749  mbuf = msgbuf[headerlen:-(2+signature_len)]
750  if len(mbuf) < csize:
751  # zero pad to give right size
752  mbuf.extend([0]*(csize - len(mbuf)))
753  if len(mbuf) < csize:
754  raise MAVError('Bad message of type %s length %u needs %s' % (
755  type, len(mbuf), csize))
756  mbuf = mbuf[:csize]
757  try:
758  t = type.unpacker.unpack(mbuf)
759  except struct.error as emsg:
760  raise MAVError('Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s' % (
761  type, fmt, len(mbuf), emsg))
762 
763  tlist = list(t)
764  # handle sorted fields
765  if ${sort_fields}:
766  t = tlist[:]
767  if sum(len_map) == len(len_map):
768  # message has no arrays in it
769  for i in range(0, len(tlist)):
770  tlist[i] = t[order_map[i]]
771  else:
772  # message has some arrays
773  tlist = []
774  for i in range(0, len(order_map)):
775  order = order_map[i]
776  L = len_map[order]
777  tip = sum(len_map[:order])
778  field = t[tip]
779  if L == 1 or isinstance(field, str):
780  tlist.append(field)
781  else:
782  tlist.append(t[tip:(tip + L)])
783 
784  # terminate any strings
785  for i in range(0, len(tlist)):
786  if type.fieldtypes[i] == 'char':
787  if sys.version_info.major >= 3:
788  tlist[i] = tlist[i].decode('utf-8')
789  tlist[i] = str(MAVString(tlist[i]))
790  t = tuple(tlist)
791  # construct the message object
792  try:
793  m = type(*t)
794  except Exception as emsg:
795  raise MAVError('Unable to instantiate MAVLink message of type %s : %s' % (type, emsg))
796  m._signed = sig_ok
797  if m._signed:
798  m._link_id = msgbuf[-13]
799  m._msgbuf = msgbuf
800  m._payload = msgbuf[6:-(2+signature_len)]
801  m._crc = crc
802  m._header = MAVLink_header(msgId, incompat_flags, compat_flags, mlen, seq, srcSystem, srcComponent)
803  return m
804 """, xml)
805 
806 
807 def generate_methods(outf, msgs):
808  print("Generating methods")
809 
810  def field_descriptions(fields):
811  ret = ""
812  for f in fields:
813  field_info = ""
814  if f.units:
815  field_info += "%s " % f.units
816  field_info += "(type:%s" % f.type
817  if f.enum:
818  field_info += ", values:%s" % f.enum
819  field_info += ")"
820  ret += " %-18s : %s %s\n" % (f.name, f.description.strip(), field_info)
821  return ret
822 
823  wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent=" ")
824 
825  for m in msgs:
826  comment = "%s\n\n%s" % (wrapper.fill(m.description.strip()), field_descriptions(m.fields))
827 
828  selffieldnames = 'self, '
829  for i in range(len(m.fields)):
830  f = m.fields[i]
831  if f.omit_arg:
832  selffieldnames += '%s=%s, ' % (f.name, f.const_value)
833  elif m.extensions_start is not None and i >= m.extensions_start:
834  fdefault = m.fielddefaults[i]
835  selffieldnames += "%s=%s, " % (f.name, fdefault)
836  else:
837  selffieldnames += '%s, ' % f.name
838  selffieldnames = selffieldnames[:-2]
839 
840  sub = {'NAMELOWER': m.name.lower(),
841  'SELFFIELDNAMES': selffieldnames,
842  'COMMENT': comment,
843  'FIELDNAMES': ", ".join(m.fieldnames)}
844 
845  t.write(outf, """
846  def ${NAMELOWER}_encode(${SELFFIELDNAMES}):
847  '''
848  ${COMMENT}
849  '''
850  return MAVLink_${NAMELOWER}_message(${FIELDNAMES})
851 
852 """, sub)
853 
854  t.write(outf, """
855  def ${NAMELOWER}_send(${SELFFIELDNAMES}, force_mavlink1=False):
856  '''
857  ${COMMENT}
858  '''
859  return self.send(self.${NAMELOWER}_encode(${FIELDNAMES}), force_mavlink1=force_mavlink1)
860 
861 """, sub)
862 
863 
864 def generate(basename, xml):
865  '''generate complete python implementation'''
866  if basename.endswith('.py'):
867  filename = basename
868  else:
869  filename = basename + '.py'
870 
871  msgs = []
872  enums = []
873  filelist = []
874  for x in xml:
875  msgs.extend(x.message)
876  enums.extend(x.enum)
877  filelist.append(os.path.basename(x.filename))
878 
879  for m in msgs:
880  m.fielddefaults = []
881  if xml[0].little_endian:
882  m.fmtstr = '<'
883  else:
884  m.fmtstr = '>'
885  m.native_fmtstr = m.fmtstr
886  for f in m.ordered_fields:
887  m.fmtstr += mavfmt(f)
888  m.fielddefaults.append(mavdefault(f))
889  m.native_fmtstr += native_mavfmt(f)
890  m.order_map = [0] * len(m.fieldnames)
891  m.len_map = [0] * len(m.fieldnames)
892  m.array_len_map = [0] * len(m.fieldnames)
893  for i in range(0, len(m.fieldnames)):
894  m.order_map[i] = m.ordered_fieldnames.index(m.fieldnames[i])
895  m.array_len_map[i] = m.ordered_fields[i].array_length
896  for i in range(0, len(m.fieldnames)):
897  n = m.order_map[i]
898  m.len_map[n] = m.fieldlengths[i]
899 
900  print("Generating %s" % filename)
901  outf = open(filename, "w")
902  generate_preamble(outf, msgs, basename, filelist, xml[0])
903  generate_enums(outf, enums)
904  generate_message_ids(outf, msgs)
905  generate_classes(outf, msgs)
906  generate_mavlink_class(outf, msgs, xml[0])
907  generate_methods(outf, msgs)
908  outf.close()
909  print("Generated %s OK" % filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:06