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 
163 class MAVEnumEntry(object):
164  def __init__(self, name, value, description='', end_marker=False, autovalue=False, origin_file='', origin_line=0):
165  self.name = name
166  self.value = value
167  self.description = description
168  self.param = []
169  self.end_marker = end_marker
170  self.autovalue = autovalue # True if value was *not* specified in XML
171  self.origin_file = origin_file
172  self.origin_line = origin_line
173 
174 class MAVEnum(object):
175  def __init__(self, name, linenumber, description=''):
176  self.name = name
177  self.description = description
178  self.entry = []
179  self.start_value = None
180  self.highest_value = 0
181  self.linenumber = linenumber
182 
183 class MAVXML(object):
184  '''parse a mavlink XML file'''
185  def __init__(self, filename, wire_protocol_version=PROTOCOL_0_9):
186  self.filename = filename
187  self.basename = os.path.basename(filename)
188  if self.basename.lower().endswith(".xml"):
189  self.basename = self.basename[:-4]
190  self.basename_upper = self.basename.upper()
191  self.message = []
192  self.enum = []
193  # we use only the day for the parse_time, as otherwise
194  # it causes a lot of unnecessary cache misses with ccache
195  self.parse_time = time.strftime("%a %b %d %Y")
196  self.version = 2
197  self.include = []
198  self.wire_protocol_version = wire_protocol_version
199 
200  # setup the protocol features for the requested protocol version
201  if wire_protocol_version == PROTOCOL_0_9:
202  self.protocol_marker = ord('U')
203  self.sort_fields = False
204  self.little_endian = False
205  self.crc_extra = False
206  self.crc_struct = False
207  self.command_24bit = False
208  self.allow_extensions = False
209  elif wire_protocol_version == PROTOCOL_1_0:
210  self.protocol_marker = 0xFE
211  self.sort_fields = True
212  self.little_endian = True
213  self.crc_extra = True
214  self.crc_struct = False
215  self.command_24bit = False
216  self.allow_extensions = False
217  elif wire_protocol_version == PROTOCOL_2_0:
218  self.protocol_marker = 0xFD
219  self.sort_fields = True
220  self.little_endian = True
221  self.crc_extra = True
222  self.crc_struct = True
223  self.command_24bit = True
224  self.allow_extensions = True
225  else:
226  print("Unknown wire protocol version")
227  print("Available versions are: %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0, PROTOCOL_2_0))
228  raise MAVParseError('Unknown MAVLink wire protocol version %s' % wire_protocol_version)
229 
230  in_element_list = []
231 
232  def check_attrs(attrs, check, where):
233  for c in check:
234  if c not in attrs:
235  raise MAVParseError('expected missing %s "%s" attribute at %s:%u' % (
236  where, c, filename, p.CurrentLineNumber))
237 
238  def start_element(name, attrs):
239  in_element_list.append(name)
240  in_element = '.'.join(in_element_list)
241  #print in_element
242  if in_element == "mavlink.messages.message":
243  check_attrs(attrs, ['name', 'id'], 'message')
244  self.message.append(MAVType(attrs['name'], attrs['id'], p.CurrentLineNumber))
245  elif in_element == "mavlink.messages.message.extensions":
246  self.message[-1].extensions_start = len(self.message[-1].fields)
247  elif in_element == "mavlink.messages.message.field":
248  check_attrs(attrs, ['name', 'type'], 'field')
249  print_format = attrs.get('print_format', None)
250  enum = attrs.get('enum', '')
251  display = attrs.get('display', '')
252  units = attrs.get('units', '')
253  if units:
254  units = '[' + units + ']'
255  new_field = MAVField(attrs['name'], attrs['type'], print_format, self, enum=enum, display=display, units=units)
256  if self.message[-1].extensions_start is None or self.allow_extensions:
257  self.message[-1].fields.append(new_field)
258  elif in_element == "mavlink.enums.enum":
259  check_attrs(attrs, ['name'], 'enum')
260  self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber))
261  elif in_element == "mavlink.enums.enum.entry":
262  check_attrs(attrs, ['name'], 'enum entry')
263  # determine value and if it was automatically assigned (for possible merging later)
264  if 'value' in attrs:
265  value = eval(attrs['value'])
266  autovalue = False
267  else:
268  value = self.enum[-1].highest_value + 1
269  autovalue = True
270  # check lowest value
271  if (self.enum[-1].start_value is None or value < self.enum[-1].start_value):
272  self.enum[-1].start_value = value
273  # check highest value
274  if (value > self.enum[-1].highest_value):
275  self.enum[-1].highest_value = value
276  # append the new entry
277  self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value, '', False, autovalue, self.filename, p.CurrentLineNumber))
278  elif in_element == "mavlink.enums.enum.entry.param":
279  check_attrs(attrs, ['index'], 'enum param')
280  self.enum[-1].entry[-1].param.append(
281  MAVEnumParam(attrs['index'],
282  label=attrs.get('label', ''), units=attrs.get('units', ''),
283  enum=attrs.get('enum', ''), increment=attrs.get('increment', ''),
284  minValue=attrs.get('minValue', ''),
285  maxValue=attrs.get('maxValue', ''), default=attrs.get('default', '0'),
286  reserved=attrs.get('reserved', False) ))
287 
288  def is_target_system_field(m, f):
289  if f.name == 'target_system':
290  return True
291  if m.name == "MANUAL_CONTROL" and f.name == "target":
292  return True
293  return False
294 
295  def end_element(name):
296  in_element_list.pop()
297 
298  def char_data(data):
299  in_element = '.'.join(in_element_list)
300  if in_element == "mavlink.messages.message.description":
301  self.message[-1].description += data
302  elif in_element == "mavlink.messages.message.field":
303  if self.message[-1].extensions_start is None or self.allow_extensions:
304  self.message[-1].fields[-1].description += data
305  elif in_element == "mavlink.enums.enum.description":
306  self.enum[-1].description += data
307  elif in_element == "mavlink.enums.enum.entry.description":
308  self.enum[-1].entry[-1].description += data
309  elif in_element == "mavlink.enums.enum.entry.param":
310  self.enum[-1].entry[-1].param[-1].set_description(data.strip())
311  elif in_element == "mavlink.version":
312  self.version = int(data)
313  elif in_element == "mavlink.include":
314  self.include.append(data)
315 
316  f = open(filename, mode='rb')
317  p = xml.parsers.expat.ParserCreate()
318  p.StartElementHandler = start_element
319  p.EndElementHandler = end_element
320  p.CharacterDataHandler = char_data
321  p.ParseFile(f)
322  f.close()
323 
324 
325  #Post process to add reserved params (for docs)
326  for current_enum in self.enum:
327  if not 'MAV_CMD' in current_enum.name:
328  continue
329  print(current_enum.name)
330  for enum_entry in current_enum.entry:
331  print(enum_entry.name)
332  if len(enum_entry.param) == 7:
333  continue
334  params_dict=dict()
335  for param_index in range (1,8):
336  params_dict[param_index] = MAVEnumParam(param_index, label='', units='', enum='', increment='',
337  minValue='', maxValue='', default='0', reserved='True')
338 
339  for a_param in enum_entry.param:
340  params_dict[int(a_param.index)] = a_param
341  enum_entry.param=params_dict.values()
342 
343 
344 
345  self.message_lengths = {}
346  self.message_min_lengths = {}
347  self.message_flags = {}
348  self.message_target_system_ofs = {}
349  self.message_target_component_ofs = {}
350  self.message_crcs = {}
351  self.message_names = {}
352  self.largest_payload = 0
353 
354  if not self.command_24bit:
355  # remove messages with IDs > 255
356  m2 = []
357  for m in self.message:
358  if m.id <= 255:
359  m2.append(m)
360  else:
361  print("Ignoring MAVLink2 message %s" % m.name)
362  self.message = m2
363 
364  for m in self.message:
365  if not self.command_24bit and m.id > 255:
366  continue
367 
368  m.wire_length = 0
369  m.wire_min_length = 0
370  m.fieldnames = []
371  m.fieldlengths = []
372  m.ordered_fieldnames = []
373  m.ordered_fieldtypes = []
374  m.fieldtypes = []
375  m.message_flags = 0
376  m.target_system_ofs = 0
377  m.target_component_ofs = 0
378 
379  if self.sort_fields:
380  # when we have extensions we only sort up to the first extended field
381  sort_end = m.base_fields()
382  m.ordered_fields = sorted(m.fields[:sort_end],
383  key=operator.attrgetter('type_length'),
384  reverse=True)
385  m.ordered_fields.extend(m.fields[sort_end:])
386  else:
387  m.ordered_fields = m.fields
388  for f in m.fields:
389  m.fieldnames.append(f.name)
390  L = f.array_length
391  if L == 0:
392  m.fieldlengths.append(1)
393  elif L > 1 and f.type == 'char':
394  m.fieldlengths.append(1)
395  else:
396  m.fieldlengths.append(L)
397  m.fieldtypes.append(f.type)
398  for i in range(len(m.ordered_fields)):
399  f = m.ordered_fields[i]
400  f.wire_offset = m.wire_length
401  m.wire_length += f.wire_length
402  if m.extensions_start is None or i < m.extensions_start:
403  m.wire_min_length = m.wire_length
404  m.ordered_fieldnames.append(f.name)
405  m.ordered_fieldtypes.append(f.type)
406  f.set_test_value()
407  if f.name.find('[') != -1:
408  raise MAVParseError("invalid field name with array descriptor %s" % f.name)
409  # having flags for target_system and target_component helps a lot for routing code
410  if is_target_system_field(m, f):
411  m.message_flags |= FLAG_HAVE_TARGET_SYSTEM
412  m.target_system_ofs = f.wire_offset
413  elif f.name == 'target_component':
414  m.message_flags |= FLAG_HAVE_TARGET_COMPONENT
415  m.target_component_ofs = f.wire_offset
416  m.num_fields = len(m.fieldnames)
417  if m.num_fields > 64:
418  raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % (
419  m.num_fields, 64))
420  m.crc_extra = message_checksum(m)
421 
422  key = m.id
423  self.message_crcs[key] = m.crc_extra
424  self.message_lengths[key] = m.wire_length
425  self.message_min_lengths[key] = m.wire_min_length
426  self.message_names[key] = m.name
427  self.message_flags[key] = m.message_flags
428  self.message_target_system_ofs[key] = m.target_system_ofs
429  self.message_target_component_ofs[key] = m.target_component_ofs
430 
431  if m.wire_length > self.largest_payload:
432  self.largest_payload = m.wire_length
433 
434  if m.wire_length+8 > 64:
435  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))
436 
437  def __str__(self):
438  return "MAVXML for %s from %s (%u message, %u enums)" % (
439  self.basename, self.filename, len(self.message), len(self.enum))
440 
441 
442 def message_checksum(msg):
443  '''calculate a 8-bit checksum of the key fields of a message, so we
444  can detect incompatible XML changes'''
445  from .mavcrc import x25crc
446  crc = x25crc()
447  crc.accumulate_str(msg.name + ' ')
448  # in order to allow for extensions the crc does not include
449  # any field extensions
450  crc_end = msg.base_fields()
451  for i in range(crc_end):
452  f = msg.ordered_fields[i]
453  crc.accumulate_str(f.type + ' ')
454  crc.accumulate_str(f.name + ' ')
455  if f.array_length:
456  crc.accumulate([f.array_length])
457  return (crc.crc&0xFF) ^ (crc.crc>>8)
458 
459 def merge_enums(xml):
460  '''merge enums between XML files'''
461  emap = {}
462  for x in xml:
463  newenums = []
464  for enum in x.enum:
465  if enum.name in emap:
466  emapitem = emap[enum.name]
467  # check for possible conflicting auto-assigned values after merge
468  if (emapitem.start_value <= enum.highest_value and emapitem.highest_value >= enum.start_value):
469  for entry in emapitem.entry:
470  # correct the value if necessary, but only if it was auto-assigned to begin with
471  if entry.value <= enum.highest_value and entry.autovalue is True:
472  entry.value = enum.highest_value + 1
473  enum.highest_value = entry.value
474  # merge the entries
475  emapitem.entry.extend(enum.entry)
476  if not emapitem.description:
477  emapitem.description = enum.description
478  print("Merged enum %s" % enum.name)
479  else:
480  newenums.append(enum)
481  emap[enum.name] = enum
482  x.enum = newenums
483  for e in emap:
484  # sort by value
485  emap[e].entry = sorted(emap[e].entry,
486  key=operator.attrgetter('value'),
487  reverse=False)
488  # add a ENUM_END
489  emap[e].entry.append(MAVEnumEntry("%s_ENUM_END" % emap[e].name,
490  emap[e].entry[-1].value+1, end_marker=True))
491 
492 def check_duplicates(xml):
493  '''check for duplicate message IDs'''
494 
495  merge_enums(xml)
496 
497  msgmap = {}
498  msg_name_map = {}
499  enummap = {}
500  for x in xml:
501  for m in x.message:
502  key = m.id
503  if key in msgmap:
504  print("ERROR: Duplicate message id %u for %s (%s:%u) also used by %s" % (
505  m.id,
506  m.name,
507  x.filename, m.linenumber,
508  msgmap[key]))
509  return True
510  fieldset = set()
511  for f in m.fields:
512  if f.name in fieldset:
513  print("ERROR: Duplicate field %s in message %s (%s:%u)" % (
514  f.name, m.name,
515  x.filename, m.linenumber))
516  return True
517  fieldset.add(f.name)
518  msgmap[key] = '%s (%s:%u)' % (m.name, x.filename, m.linenumber)
519  # Check for duplicate message names
520  if m.name in msg_name_map:
521  print("ERROR: Duplicate message name %s for id:%u (%s:%u) also used by %s" % (
522  m.name,
523  m.id,
524  x.filename, m.linenumber,
525  msg_name_map[m.name]))
526  return True
527  msg_name_map[m.name] = '%s (%s:%u)' % (m.id, x.filename, m.linenumber)
528  for enum in x.enum:
529  for entry in enum.entry:
530  if entry.autovalue is True and "common.xml" not in entry.origin_file:
531  print("Note: An enum value was auto-generated: %s = %u" % (entry.name, entry.value))
532  s1 = "%s.%s" % (enum.name, entry.name)
533  s2 = "%s.%s" % (enum.name, entry.value)
534  if s1 in enummap or s2 in enummap:
535  print("ERROR: Duplicate enum %s:\n\t%s = %s @ %s:%u\n\t%s" % (
536  "names" if s1 in enummap else "values",
537  s1, entry.value, entry.origin_file, entry.origin_line,
538  enummap.get(s1) or enummap.get(s2)))
539  return True
540  enummap[s1] = enummap[s2] = "%s.%s = %s @ %s:%u" % (enum.name, entry.name, entry.value, entry.origin_file, entry.origin_line)
541 
542  return False
543 
544 
545 
546 def total_msgs(xml):
547  '''count total number of msgs'''
548  count = 0
549  for x in xml:
550  count += len(x.message)
551  return count
552 
553 def mkdir_p(dir):
554  try:
555  os.makedirs(dir)
556  except OSError as exc:
557  if exc.errno != errno.EEXIST:
558  raise
559 
560 # check version consistent
561 # add test.xml
562 # finish test suite
563 # printf style error macro, if defined call errors

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