mavparse.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 '''
3 mavlink python parse functions
4 
5 Copyright Andrew Tridgell 2011
6 Released under GNU GPL version 3 or later
7 '''
8 from __future__ import print_function
9 from builtins import range
10 from builtins import object
11 
12 import errno
13 import operator
14 import os
15 import sys
16 import time
17 import xml.parsers.expat
18 
19 PROTOCOL_0_9 = "0.9"
20 PROTOCOL_1_0 = "1.0"
21 PROTOCOL_2_0 = "2.0"
22 
23 # message flags
24 FLAG_HAVE_TARGET_SYSTEM = 1
25 FLAG_HAVE_TARGET_COMPONENT = 2
26 
27 class MAVParseError(Exception):
28  def __init__(self, message, inner_exception=None):
29  self.message = message
30  self.inner_exception = inner_exception
31  self.exception_info = sys.exc_info()
32  def __str__(self):
33  return self.message
34 
35 class MAVField(object):
36  def __init__(self, name, type, print_format, xml, description='', enum='', display='', units=''):
37  self.name = name
38  self.name_upper = name.upper()
39  self.description = description
40  self.array_length = 0
41  self.enum = enum
42  self.display = display
43  self.units = units
44  self.omit_arg = False
45  self.const_value = None
46  self.print_format = print_format
47  lengths = {
48  'float' : 4,
49  'double' : 8,
50  'char' : 1,
51  'int8_t' : 1,
52  'uint8_t' : 1,
53  'uint8_t_mavlink_version' : 1,
54  'int16_t' : 2,
55  'uint16_t' : 2,
56  'int32_t' : 4,
57  'uint32_t' : 4,
58  'int64_t' : 8,
59  'uint64_t' : 8,
60  }
61 
62  if type=='uint8_t_mavlink_version':
63  type = 'uint8_t'
64  self.omit_arg = True
65  self.const_value = xml.version
66 
67  aidx = type.find("[")
68  if aidx != -1:
69  assert type[-1:] == ']'
70  self.array_length = int(type[aidx+1:-1])
71  type = type[0:aidx]
72  if type == 'array':
73  type = 'int8_t'
74  if type in lengths:
75  self.type_length = lengths[type]
76  self.type = type
77  elif (type+"_t") in lengths:
78  self.type_length = lengths[type+"_t"]
79  self.type = type+'_t'
80  else:
81  raise MAVParseError("unknown type '%s'" % type)
82  if self.array_length != 0:
83  self.wire_length = self.array_length * self.type_length
84  else:
85  self.wire_length = self.type_length
86  self.type_upper = self.type.upper()
87 
88  def gen_test_value(self, i):
89  '''generate a testsuite value for a MAVField'''
90  if self.const_value:
91  return self.const_value
92  elif self.type == 'float':
93  return 17.0 + self.wire_offset*7 + i
94  elif self.type == 'double':
95  return 123.0 + self.wire_offset*7 + i
96  elif self.type == 'char':
97  return chr(ord('A') + (self.wire_offset + i)%26)
98  elif self.type in [ 'int8_t', 'uint8_t' ]:
99  return (5 + self.wire_offset*67 + i) & 0xFF
100  elif self.type in ['int16_t', 'uint16_t']:
101  return (17235 + self.wire_offset*52 + i) & 0xFFFF
102  elif self.type in ['int32_t', 'uint32_t']:
103  return (963497464 + self.wire_offset*52 + i)&0xFFFFFFFF
104  elif self.type in ['int64_t', 'uint64_t']:
105  return 93372036854775807 + self.wire_offset*63 + i
106  else:
107  raise MAVParseError('unknown type %s' % self.type)
108 
109  def set_test_value(self):
110  '''set a testsuite value for a MAVField'''
111  if self.array_length:
112  self.test_value = []
113  for i in range(self.array_length):
114  self.test_value.append(self.gen_test_value(i))
115  else:
116  self.test_value = self.gen_test_value(0)
117  if self.type == 'char' and self.array_length:
118  v = ""
119  for c in self.test_value:
120  v += c
121  self.test_value = v[:-1]
122 
123 
124 class MAVType(object):
125  def __init__(self, name, id, linenumber, description=''):
126  self.name = name
127  self.name_lower = name.lower()
128  self.linenumber = linenumber
129  self.id = int(id)
130  self.description = description
131  self.fields = []
132  self.fieldnames = []
133  self.extensions_start = None
134 
135  def base_fields(self):
136  '''return number of non-extended fields'''
137  if self.extensions_start is None:
138  return len(self.fields)
139  return len(self.fields[:self.extensions_start])
140 
141 class MAVEnumParam(object):
142  def __init__(self, index, description='', label='', units='', enum='', increment='', minValue='', maxValue='', reserved=False, default=''):
143  self.index = index
144  self.description = description
145  self.label = label
146  self.units = units
147  self.enum = enum
148  self.increment = increment
149  self.minValue = minValue
150  self.maxValue = maxValue
151  self.reserved = reserved
152  self.default = default
153  if self.reserved and not self.default:
154  self.default = '0'
155  self.set_description(description)
156 
157  def set_description(self, description):
158  if not description.strip() and self.reserved:
159  self.description = 'Reserved (default:%s)' % self.default
160  else:
161  self.description = description
162  self.description = '%s: %s' % (self.index, self.description)
163 
164 class MAVEnumEntry(object):
165  def __init__(self, name, value, description='', end_marker=False, autovalue=False, origin_file='', origin_line=0):
166  self.name = name
167  self.value = value
168  self.description = description
169  self.param = []
170  self.end_marker = end_marker
171  self.autovalue = autovalue # True if value was *not* specified in XML
172  self.origin_file = origin_file
173  self.origin_line = origin_line
174 
175 class MAVEnum(object):
176  def __init__(self, name, linenumber, description=''):
177  self.name = name
178  self.description = description
179  self.entry = []
180  self.start_value = None
181  self.highest_value = 0
182  self.linenumber = linenumber
183 
184 class MAVXML(object):
185  '''parse a mavlink XML file'''
186  def __init__(self, filename, wire_protocol_version=PROTOCOL_0_9):
187  self.filename = filename
188  self.basename = os.path.basename(filename)
189  if self.basename.lower().endswith(".xml"):
190  self.basename = self.basename[:-4]
191  self.basename_upper = self.basename.upper()
192  self.message = []
193  self.enum = []
194  # we use only the day for the parse_time, as otherwise
195  # it causes a lot of unnecessary cache misses with ccache
196  self.parse_time = time.strftime("%a %b %d %Y")
197  self.version = 2
198  self.include = []
199  self.wire_protocol_version = wire_protocol_version
200 
201  # setup the protocol features for the requested protocol version
202  if wire_protocol_version == PROTOCOL_0_9:
203  self.protocol_marker = ord('U')
204  self.sort_fields = False
205  self.little_endian = False
206  self.crc_extra = False
207  self.crc_struct = False
208  self.command_24bit = False
209  self.allow_extensions = False
210  elif wire_protocol_version == PROTOCOL_1_0:
211  self.protocol_marker = 0xFE
212  self.sort_fields = True
213  self.little_endian = True
214  self.crc_extra = True
215  self.crc_struct = False
216  self.command_24bit = False
217  self.allow_extensions = False
218  elif wire_protocol_version == PROTOCOL_2_0:
219  self.protocol_marker = 0xFD
220  self.sort_fields = True
221  self.little_endian = True
222  self.crc_extra = True
223  self.crc_struct = True
224  self.command_24bit = True
225  self.allow_extensions = True
226  else:
227  print("Unknown wire protocol version")
228  print("Available versions are: %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0, PROTOCOL_2_0))
229  raise MAVParseError('Unknown MAVLink wire protocol version %s' % wire_protocol_version)
230 
231  in_element_list = []
232 
233  def check_attrs(attrs, check, where):
234  for c in check:
235  if c not in attrs:
236  raise MAVParseError('expected missing %s "%s" attribute at %s:%u' % (
237  where, c, filename, p.CurrentLineNumber))
238 
239  def start_element(name, attrs):
240  in_element_list.append(name)
241  in_element = '.'.join(in_element_list)
242  #print in_element
243  if in_element == "mavlink.messages.message":
244  check_attrs(attrs, ['name', 'id'], 'message')
245  self.message.append(MAVType(attrs['name'], attrs['id'], p.CurrentLineNumber))
246  elif in_element == "mavlink.messages.message.extensions":
247  self.message[-1].extensions_start = len(self.message[-1].fields)
248  elif in_element == "mavlink.messages.message.field":
249  check_attrs(attrs, ['name', 'type'], 'field')
250  print_format = attrs.get('print_format', None)
251  enum = attrs.get('enum', '')
252  display = attrs.get('display', '')
253  units = attrs.get('units', '')
254  if units:
255  units = '[' + units + ']'
256  new_field = MAVField(attrs['name'], attrs['type'], print_format, self, enum=enum, display=display, units=units)
257  if self.message[-1].extensions_start is None or self.allow_extensions:
258  self.message[-1].fields.append(new_field)
259  elif in_element == "mavlink.enums.enum":
260  check_attrs(attrs, ['name'], 'enum')
261  self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber))
262  elif in_element == "mavlink.enums.enum.entry":
263  check_attrs(attrs, ['name'], 'enum entry')
264  # determine value and if it was automatically assigned (for possible merging later)
265  if 'value' in attrs:
266  value = eval(attrs['value'])
267  autovalue = False
268  else:
269  value = self.enum[-1].highest_value + 1
270  autovalue = True
271  # check lowest value
272  if (self.enum[-1].start_value is None or value < self.enum[-1].start_value):
273  self.enum[-1].start_value = value
274  # check highest value
275  if (value > self.enum[-1].highest_value):
276  self.enum[-1].highest_value = value
277  # append the new entry
278  self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value, '', False, autovalue, self.filename, p.CurrentLineNumber))
279  elif in_element == "mavlink.enums.enum.entry.param":
280  check_attrs(attrs, ['index'], 'enum param')
281  self.enum[-1].entry[-1].param.append(
282  MAVEnumParam(attrs['index'],
283  label=attrs.get('label', ''), units=attrs.get('units', ''),
284  enum=attrs.get('enum', ''), increment=attrs.get('increment', ''),
285  minValue=attrs.get('minValue', ''),
286  maxValue=attrs.get('maxValue', ''), default=attrs.get('default', '0'),
287  reserved=attrs.get('reserved', False) ))
288 
289  def is_target_system_field(m, f):
290  if f.name == 'target_system':
291  return True
292  if m.name == "MANUAL_CONTROL" and f.name == "target":
293  return True
294  return False
295 
296  def end_element(name):
297  in_element_list.pop()
298 
299  def char_data(data):
300  in_element = '.'.join(in_element_list)
301  if in_element == "mavlink.messages.message.description":
302  self.message[-1].description += data
303  elif in_element == "mavlink.messages.message.field":
304  if self.message[-1].extensions_start is None or self.allow_extensions:
305  self.message[-1].fields[-1].description += data
306  elif in_element == "mavlink.enums.enum.description":
307  self.enum[-1].description += data
308  elif in_element == "mavlink.enums.enum.entry.description":
309  self.enum[-1].entry[-1].description += data
310  elif in_element == "mavlink.enums.enum.entry.param":
311  self.enum[-1].entry[-1].param[-1].set_description(data.strip())
312  elif in_element == "mavlink.version":
313  self.version = int(data)
314  elif in_element == "mavlink.include":
315  self.include.append(data)
316 
317  f = open(filename, mode='rb')
318  p = xml.parsers.expat.ParserCreate()
319  p.StartElementHandler = start_element
320  p.EndElementHandler = end_element
321  p.CharacterDataHandler = char_data
322  p.ParseFile(f)
323  f.close()
324 
325 
326  #Post process to add reserved params (for docs)
327  for current_enum in self.enum:
328  if not 'MAV_CMD' in current_enum.name:
329  continue
330  print(current_enum.name)
331  for enum_entry in current_enum.entry:
332  print(enum_entry.name)
333  if len(enum_entry.param) == 7:
334  continue
335  params_dict=dict()
336  for param_index in range (1,8):
337  params_dict[param_index] = MAVEnumParam(param_index, label='', units='', enum='', increment='',
338  minValue='', maxValue='', default='0', reserved='True')
339 
340  for a_param in enum_entry.param:
341  params_dict[int(a_param.index)] = a_param
342  enum_entry.param=params_dict.values()
343 
344 
345 
346  self.message_lengths = {}
347  self.message_min_lengths = {}
348  self.message_flags = {}
349  self.message_target_system_ofs = {}
350  self.message_target_component_ofs = {}
351  self.message_crcs = {}
352  self.message_names = {}
353  self.largest_payload = 0
354 
355  if not self.command_24bit:
356  # remove messages with IDs > 255
357  m2 = []
358  for m in self.message:
359  if m.id <= 255:
360  m2.append(m)
361  else:
362  print("Ignoring MAVLink2 message %s" % m.name)
363  self.message = m2
364 
365  for m in self.message:
366  if not self.command_24bit and m.id > 255:
367  continue
368 
369  m.wire_length = 0
370  m.wire_min_length = 0
371  m.fieldnames = []
372  m.fieldlengths = []
373  m.ordered_fieldnames = []
374  m.ordered_fieldtypes = []
375  m.fieldtypes = []
376  m.message_flags = 0
377  m.target_system_ofs = 0
378  m.target_component_ofs = 0
379 
380  if self.sort_fields:
381  # when we have extensions we only sort up to the first extended field
382  sort_end = m.base_fields()
383  m.ordered_fields = sorted(m.fields[:sort_end],
384  key=operator.attrgetter('type_length'),
385  reverse=True)
386  m.ordered_fields.extend(m.fields[sort_end:])
387  else:
388  m.ordered_fields = m.fields
389  for f in m.fields:
390  m.fieldnames.append(f.name)
391  L = f.array_length
392  if L == 0:
393  m.fieldlengths.append(1)
394  elif L > 1 and f.type == 'char':
395  m.fieldlengths.append(1)
396  else:
397  m.fieldlengths.append(L)
398  m.fieldtypes.append(f.type)
399  for i in range(len(m.ordered_fields)):
400  f = m.ordered_fields[i]
401  f.wire_offset = m.wire_length
402  m.wire_length += f.wire_length
403  if m.extensions_start is None or i < m.extensions_start:
404  m.wire_min_length = m.wire_length
405  m.ordered_fieldnames.append(f.name)
406  m.ordered_fieldtypes.append(f.type)
407  f.set_test_value()
408  if f.name.find('[') != -1:
409  raise MAVParseError("invalid field name with array descriptor %s" % f.name)
410  # having flags for target_system and target_component helps a lot for routing code
411  if is_target_system_field(m, f):
412  m.message_flags |= FLAG_HAVE_TARGET_SYSTEM
413  m.target_system_ofs = f.wire_offset
414  elif f.name == 'target_component':
415  m.message_flags |= FLAG_HAVE_TARGET_COMPONENT
416  m.target_component_ofs = f.wire_offset
417  m.num_fields = len(m.fieldnames)
418  if m.num_fields > 64:
419  raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % (
420  m.num_fields, 64))
421  m.crc_extra = message_checksum(m)
422 
423  key = m.id
424  self.message_crcs[key] = m.crc_extra
425  self.message_lengths[key] = m.wire_length
426  self.message_min_lengths[key] = m.wire_min_length
427  self.message_names[key] = m.name
428  self.message_flags[key] = m.message_flags
429  self.message_target_system_ofs[key] = m.target_system_ofs
430  self.message_target_component_ofs[key] = m.target_component_ofs
431 
432  if m.wire_length > self.largest_payload:
433  self.largest_payload = m.wire_length
434 
435  if m.wire_length+8 > 64:
436  print("Note: message %s is longer than 64 bytes long (%u bytes), which can cause fragmentation since many radio modems use 64 bytes as maximum air transfer unit." % (m.name, m.wire_length+8))
437 
438  def __str__(self):
439  return "MAVXML for %s from %s (%u message, %u enums)" % (
440  self.basename, self.filename, len(self.message), len(self.enum))
441 
442 
443 def message_checksum(msg):
444  '''calculate a 8-bit checksum of the key fields of a message, so we
445  can detect incompatible XML changes'''
446  from .mavcrc import x25crc
447  crc = x25crc()
448  crc.accumulate_str(msg.name + ' ')
449  # in order to allow for extensions the crc does not include
450  # any field extensions
451  crc_end = msg.base_fields()
452  for i in range(crc_end):
453  f = msg.ordered_fields[i]
454  crc.accumulate_str(f.type + ' ')
455  crc.accumulate_str(f.name + ' ')
456  if f.array_length:
457  crc.accumulate([f.array_length])
458  return (crc.crc&0xFF) ^ (crc.crc>>8)
459 
460 def merge_enums(xml):
461  '''merge enums between XML files'''
462  emap = {}
463  for x in xml:
464  newenums = []
465  for enum in x.enum:
466  if enum.name in emap:
467  emapitem = emap[enum.name]
468  # check for possible conflicting auto-assigned values after merge
469  if (emapitem.start_value <= enum.highest_value and emapitem.highest_value >= enum.start_value):
470  for entry in emapitem.entry:
471  # correct the value if necessary, but only if it was auto-assigned to begin with
472  if entry.value <= enum.highest_value and entry.autovalue is True:
473  entry.value = enum.highest_value + 1
474  enum.highest_value = entry.value
475  # merge the entries
476  emapitem.entry.extend(enum.entry)
477  if not emapitem.description:
478  emapitem.description = enum.description
479  print("Merged enum %s" % enum.name)
480  else:
481  newenums.append(enum)
482  emap[enum.name] = enum
483  x.enum = newenums
484  for e in emap:
485  # sort by value
486  emap[e].entry = sorted(emap[e].entry,
487  key=operator.attrgetter('value'),
488  reverse=False)
489  # add a ENUM_END
490  emap[e].entry.append(MAVEnumEntry("%s_ENUM_END" % emap[e].name,
491  emap[e].entry[-1].value+1, end_marker=True))
492 
493 def check_duplicates(xml):
494  '''check for duplicate message IDs'''
495 
496  merge_enums(xml)
497 
498  msgmap = {}
499  msg_name_map = {}
500  enummap = {}
501  for x in xml:
502  for m in x.message:
503  key = m.id
504  if key in msgmap:
505  print("ERROR: Duplicate message id %u for %s (%s:%u) also used by %s" % (
506  m.id,
507  m.name,
508  x.filename, m.linenumber,
509  msgmap[key]))
510  return True
511  fieldset = set()
512  for f in m.fields:
513  if f.name in fieldset:
514  print("ERROR: Duplicate field %s in message %s (%s:%u)" % (
515  f.name, m.name,
516  x.filename, m.linenumber))
517  return True
518  fieldset.add(f.name)
519  msgmap[key] = '%s (%s:%u)' % (m.name, x.filename, m.linenumber)
520  # Check for duplicate message names
521  if m.name in msg_name_map:
522  print("ERROR: Duplicate message name %s for id:%u (%s:%u) also used by %s" % (
523  m.name,
524  m.id,
525  x.filename, m.linenumber,
526  msg_name_map[m.name]))
527  return True
528  msg_name_map[m.name] = '%s (%s:%u)' % (m.id, x.filename, m.linenumber)
529  for enum in x.enum:
530  for entry in enum.entry:
531  if entry.autovalue is True and "common.xml" not in entry.origin_file:
532  print("Note: An enum value was auto-generated: %s = %u" % (entry.name, entry.value))
533  s1 = "%s.%s" % (enum.name, entry.name)
534  s2 = "%s.%s" % (enum.name, entry.value)
535  if s1 in enummap or s2 in enummap:
536  print("ERROR: Duplicate enum %s:\n\t%s = %s @ %s:%u\n\t%s" % (
537  "names" if s1 in enummap else "values",
538  s1, entry.value, entry.origin_file, entry.origin_line,
539  enummap.get(s1) or enummap.get(s2)))
540  return True
541  enummap[s1] = enummap[s2] = "%s.%s = %s @ %s:%u" % (enum.name, entry.name, entry.value, entry.origin_file, entry.origin_line)
542 
543  return False
544 
545 
546 
547 def total_msgs(xml):
548  '''count total number of msgs'''
549  count = 0
550  for x in xml:
551  count += len(x.message)
552  return count
553 
554 def mkdir_p(dir):
555  try:
556  os.makedirs(dir)
557  except OSError as exc:
558  if exc.errno != errno.EEXIST:
559  raise
560 
561 # check version consistent
562 # add test.xml
563 # finish test suite
564 # printf style error macro, if defined call errors

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