mavparse.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 mavlink python parse functions
00004 
00005 Copyright Andrew Tridgell 2011
00006 Released under GNU GPL version 3 or later
00007 '''
00008 
00009 import xml.parsers.expat, os, errno, time, sys, operator, struct
00010 
00011 PROTOCOL_0_9 = "0.9"
00012 PROTOCOL_1_0 = "1.0"
00013 PROTOCOL_2_0 = "2.0"
00014 
00015 # message flags
00016 FLAG_HAVE_TARGET_SYSTEM    = 1
00017 FLAG_HAVE_TARGET_COMPONENT = 2
00018 
00019 class MAVParseError(Exception):
00020     def __init__(self, message, inner_exception=None):
00021         self.message = message
00022         self.inner_exception = inner_exception
00023         self.exception_info = sys.exc_info()
00024     def __str__(self):
00025         return self.message
00026 
00027 class MAVField(object):
00028     def __init__(self, name, type, print_format, xml, description='', enum=''):
00029         self.name = name
00030         self.name_upper = name.upper()
00031         self.description = description
00032         self.array_length = 0
00033         self.enum = enum
00034         self.omit_arg = False
00035         self.const_value = None
00036         self.print_format = print_format
00037         lengths = {
00038         'float'    : 4,
00039         'double'   : 8,
00040         'char'     : 1,
00041         'int8_t'   : 1,
00042         'uint8_t'  : 1,
00043         'uint8_t_mavlink_version'  : 1,
00044         'int16_t'  : 2,
00045         'uint16_t' : 2,
00046         'int32_t'  : 4,
00047         'uint32_t' : 4,
00048         'int64_t'  : 8,
00049         'uint64_t' : 8,
00050         }
00051 
00052         if type=='uint8_t_mavlink_version':
00053             type = 'uint8_t'
00054             self.omit_arg = True
00055             self.const_value = xml.version
00056 
00057         aidx = type.find("[")
00058         if aidx != -1:
00059             assert type[-1:] == ']'
00060             self.array_length = int(type[aidx+1:-1])
00061             type = type[0:aidx]
00062             if type == 'array':
00063                 type = 'int8_t'
00064         if type in lengths:
00065             self.type_length = lengths[type]
00066             self.type = type
00067         elif (type+"_t") in lengths:
00068             self.type_length = lengths[type+"_t"]
00069             self.type = type+'_t'
00070         else:
00071             raise MAVParseError("unknown type '%s'" % type)
00072         if self.array_length != 0:
00073             self.wire_length = self.array_length * self.type_length
00074         else:
00075             self.wire_length = self.type_length
00076         self.type_upper = self.type.upper()
00077 
00078     def gen_test_value(self, i):
00079         '''generate a testsuite value for a MAVField'''
00080         if self.const_value:
00081             return self.const_value
00082         elif self.type == 'float':
00083             return 17.0 + self.wire_offset*7 + i
00084         elif self.type == 'double':
00085             return 123.0 + self.wire_offset*7 + i
00086         elif self.type == 'char':
00087             return chr(ord('A') + (self.wire_offset + i)%26)
00088         elif self.type in [ 'int8_t', 'uint8_t' ]:
00089             return (5 + self.wire_offset*67 + i) & 0xFF
00090         elif self.type in ['int16_t', 'uint16_t']:
00091             return (17235 + self.wire_offset*52 + i) & 0xFFFF
00092         elif self.type in ['int32_t', 'uint32_t']:
00093             return (963497464 + self.wire_offset*52 + i)&0xFFFFFFFF
00094         elif self.type in ['int64_t', 'uint64_t']:
00095             return 93372036854775807 + self.wire_offset*63 + i
00096         else:
00097             raise MAVError('unknown type %s' % self.type)
00098 
00099     def set_test_value(self):
00100         '''set a testsuite value for a MAVField'''
00101         if self.array_length:
00102             self.test_value = []
00103             for i in range(self.array_length):
00104                 self.test_value.append(self.gen_test_value(i))
00105         else:
00106                 self.test_value = self.gen_test_value(0)
00107         if self.type == 'char' and self.array_length:
00108             v = ""
00109             for c in self.test_value:
00110                 v += c
00111             self.test_value = v[:-1]
00112 
00113 
00114 class MAVType(object):
00115     def __init__(self, name, id, linenumber, description=''):
00116         self.name = name
00117         self.name_lower = name.lower()
00118         self.linenumber = linenumber
00119         self.id = int(id)
00120         self.description = description
00121         self.fields = []
00122         self.fieldnames = []
00123         self.extensions_start = None
00124 
00125     def base_fields(self):
00126         '''return number of non-extended fields'''
00127         if self.extensions_start is None:
00128             return len(self.fields)
00129         return len(self.fields[:self.extensions_start])
00130 
00131 class MAVEnumParam(object):
00132     def __init__(self, index, description=''):
00133         self.index = index
00134         self.description = description
00135 
00136 class MAVEnumEntry(object):
00137     def __init__(self, name, value, description='', end_marker=False, autovalue=False, origin_file='', origin_line=0):
00138         self.name = name
00139         self.value = value
00140         self.description = description
00141         self.param = []
00142         self.end_marker = end_marker
00143         self.autovalue = autovalue  # True if value was *not* specified in XML
00144         self.origin_file = origin_file
00145         self.origin_line = origin_line
00146 
00147 class MAVEnum(object):
00148     def __init__(self, name, linenumber, description=''):
00149         self.name = name
00150         self.description = description
00151         self.entry = []
00152         self.start_value = None
00153         self.highest_value = 0
00154         self.linenumber = linenumber
00155 
00156 class MAVXML(object):
00157     '''parse a mavlink XML file'''
00158     def __init__(self, filename, wire_protocol_version=PROTOCOL_0_9):
00159         self.filename = filename
00160         self.basename = os.path.basename(filename)
00161         if self.basename.lower().endswith(".xml"):
00162             self.basename = self.basename[:-4]
00163         self.basename_upper = self.basename.upper()
00164         self.message = []
00165         self.enum = []
00166         # we use only the day for the parse_time, as otherwise
00167         # it causes a lot of unnecessary cache misses with ccache
00168         self.parse_time = time.strftime("%a %b %d %Y")
00169         self.version = 2
00170         self.include = []
00171         self.wire_protocol_version = wire_protocol_version
00172 
00173         # setup the protocol features for the requested protocol version
00174         if wire_protocol_version == PROTOCOL_0_9:
00175             self.protocol_marker = ord('U')
00176             self.sort_fields = False
00177             self.little_endian = False
00178             self.crc_extra = False
00179             self.crc_struct = False
00180             self.command_24bit = False
00181             self.allow_extensions = False
00182         elif wire_protocol_version == PROTOCOL_1_0:
00183             self.protocol_marker = 0xFE
00184             self.sort_fields = True
00185             self.little_endian = True
00186             self.crc_extra = True
00187             self.crc_struct = False
00188             self.command_24bit = False
00189             self.allow_extensions = False
00190         elif wire_protocol_version == PROTOCOL_2_0:
00191             self.protocol_marker = 0xFD
00192             self.sort_fields = True
00193             self.little_endian = True
00194             self.crc_extra = True
00195             self.crc_struct = True
00196             self.command_24bit = True
00197             self.allow_extensions = True
00198         else:
00199             print("Unknown wire protocol version")
00200             print("Available versions are: %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0, PROTOCOL_2_0))
00201             raise MAVParseError('Unknown MAVLink wire protocol version %s' % wire_protocol_version)
00202 
00203         in_element_list = []
00204 
00205         def check_attrs(attrs, check, where):
00206             for c in check:
00207                 if not c in attrs:
00208                     raise MAVParseError('expected missing %s "%s" attribute at %s:%u' % (
00209                         where, c, filename, p.CurrentLineNumber))
00210 
00211         def start_element(name, attrs):
00212             in_element_list.append(name)
00213             in_element = '.'.join(in_element_list)
00214             #print in_element
00215             if in_element == "mavlink.messages.message":
00216                 check_attrs(attrs, ['name', 'id'], 'message')
00217                 self.message.append(MAVType(attrs['name'], attrs['id'], p.CurrentLineNumber))
00218             elif in_element == "mavlink.messages.message.extensions":
00219                 self.message[-1].extensions_start = len(self.message[-1].fields)
00220             elif in_element == "mavlink.messages.message.field":
00221                 check_attrs(attrs, ['name', 'type'], 'field')
00222                 if 'print_format' in attrs:
00223                     print_format = attrs['print_format']
00224                 else:
00225                     print_format = None
00226                 if 'enum' in attrs:
00227                     enum = attrs['enum']
00228                 else:
00229                     enum = ''
00230                 new_field = MAVField(attrs['name'], attrs['type'], print_format, self, enum=enum)
00231                 if self.message[-1].extensions_start is None or self.allow_extensions:
00232                     self.message[-1].fields.append(new_field)
00233             elif in_element == "mavlink.enums.enum":
00234                 check_attrs(attrs, ['name'], 'enum')
00235                 self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber))
00236             elif in_element == "mavlink.enums.enum.entry":
00237                 check_attrs(attrs, ['name'], 'enum entry')
00238                 # determine value and if it was automatically assigned (for possible merging later)
00239                 if 'value' in attrs:
00240                     value = eval(attrs['value'])
00241                     autovalue = False
00242                 else:
00243                     value = self.enum[-1].highest_value + 1
00244                     autovalue = True
00245                 # check lowest value
00246                 if (self.enum[-1].start_value == None or value < self.enum[-1].start_value):
00247                     self.enum[-1].start_value = value
00248                 # check highest value
00249                 if (value > self.enum[-1].highest_value):
00250                     self.enum[-1].highest_value = value
00251                 # append the new entry
00252                 self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value, '', False, autovalue, self.filename, p.CurrentLineNumber))
00253             elif in_element == "mavlink.enums.enum.entry.param":
00254                 check_attrs(attrs, ['index'], 'enum param')
00255                 self.enum[-1].entry[-1].param.append(MAVEnumParam(attrs['index']))
00256 
00257         def end_element(name):
00258             in_element_list.pop()
00259 
00260         def char_data(data):
00261             in_element = '.'.join(in_element_list)
00262             if in_element == "mavlink.messages.message.description":
00263                 self.message[-1].description += data
00264             elif in_element == "mavlink.messages.message.field":
00265                 if self.message[-1].extensions_start is None or self.allow_extensions:
00266                     self.message[-1].fields[-1].description += data
00267             elif in_element == "mavlink.enums.enum.description":
00268                 self.enum[-1].description += data
00269             elif in_element == "mavlink.enums.enum.entry.description":
00270                 self.enum[-1].entry[-1].description += data
00271             elif in_element == "mavlink.enums.enum.entry.param":
00272                 self.enum[-1].entry[-1].param[-1].description += data
00273             elif in_element == "mavlink.version":
00274                 self.version = int(data)
00275             elif in_element == "mavlink.include":
00276                 self.include.append(data)
00277 
00278         f = open(filename, mode='rb')
00279         p = xml.parsers.expat.ParserCreate()
00280         p.StartElementHandler = start_element
00281         p.EndElementHandler = end_element
00282         p.CharacterDataHandler = char_data
00283         p.ParseFile(f)
00284         f.close()
00285 
00286         self.message_lengths = {}
00287         self.message_min_lengths = {}
00288         self.message_flags = {}
00289         self.message_target_system_ofs = {}
00290         self.message_target_component_ofs = {}
00291         self.message_crcs = {}
00292         self.message_names = {}
00293         self.largest_payload = 0
00294 
00295         if not self.command_24bit:
00296             # remove messages with IDs > 255
00297             m2 = []
00298             for m in self.message:
00299                 if m.id <= 255:
00300                     m2.append(m)
00301                 else:
00302                     print("Ignoring MAVLink2 message %s" % m.name)
00303             self.message = m2
00304 
00305         for m in self.message:
00306             if not self.command_24bit and m.id > 255:
00307                 continue
00308 
00309             m.wire_length = 0
00310             m.wire_min_length = 0
00311             m.fieldnames = []
00312             m.fieldlengths = []
00313             m.ordered_fieldnames = []
00314             m.message_flags = 0
00315             m.target_system_ofs = 0
00316             m.target_component_ofs = 0
00317             
00318             if self.sort_fields:
00319                 # when we have extensions we only sort up to the first extended field
00320                 sort_end = m.base_fields()
00321                 m.ordered_fields = sorted(m.fields[:sort_end],
00322                                                    key=operator.attrgetter('type_length'),
00323                                                    reverse=True)
00324                 m.ordered_fields.extend(m.fields[sort_end:])
00325             else:
00326                 m.ordered_fields = m.fields
00327             for f in m.fields:
00328                 m.fieldnames.append(f.name)
00329                 L = f.array_length
00330                 if L == 0:
00331                     m.fieldlengths.append(1)
00332                 elif L > 1 and f.type == 'char':
00333                     m.fieldlengths.append(1)
00334                 else:
00335                     m.fieldlengths.append(L)
00336             for i in range(len(m.ordered_fields)):
00337                 f = m.ordered_fields[i]
00338                 f.wire_offset = m.wire_length
00339                 m.wire_length += f.wire_length
00340                 if m.extensions_start is None or i < m.extensions_start:
00341                     m.wire_min_length = m.wire_length
00342                 m.ordered_fieldnames.append(f.name)
00343                 f.set_test_value()
00344                 if f.name.find('[') != -1:
00345                     raise MAVParseError("invalid field name with array descriptor %s" % f.name)
00346                 # having flags for target_system and target_component helps a lot for routing code
00347                 if f.name == 'target_system':
00348                     m.message_flags |= FLAG_HAVE_TARGET_SYSTEM
00349                     m.target_system_ofs = f.wire_offset
00350                 elif f.name == 'target_component':
00351                     m.message_flags |= FLAG_HAVE_TARGET_COMPONENT
00352                     m.target_component_ofs = f.wire_offset
00353             m.num_fields = len(m.fieldnames)
00354             if m.num_fields > 64:
00355                 raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % (
00356                     m.num_fields, 64))
00357             m.crc_extra = message_checksum(m)
00358 
00359             key = m.id
00360             self.message_crcs[key] = m.crc_extra
00361             self.message_lengths[key] = m.wire_length
00362             self.message_min_lengths[key] = m.wire_min_length
00363             self.message_names[key] = m.name
00364             self.message_flags[key] = m.message_flags
00365             self.message_target_system_ofs[key] = m.target_system_ofs
00366             self.message_target_component_ofs[key] = m.target_component_ofs
00367 
00368             if m.wire_length > self.largest_payload:
00369                 self.largest_payload = m.wire_length
00370 
00371             if m.wire_length+8 > 64:
00372                 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))
00373 
00374     def __str__(self):
00375         return "MAVXML for %s from %s (%u message, %u enums)" % (
00376             self.basename, self.filename, len(self.message), len(self.enum))
00377 
00378 
00379 def message_checksum(msg):
00380     '''calculate a 8-bit checksum of the key fields of a message, so we
00381        can detect incompatible XML changes'''
00382     from .mavcrc import x25crc
00383     crc = x25crc()
00384     crc.accumulate_str(msg.name + ' ')
00385     # in order to allow for extensions the crc does not include
00386     # any field extensions
00387     crc_end = msg.base_fields()
00388     for i in range(crc_end):
00389         f = msg.ordered_fields[i]
00390         crc.accumulate_str(f.type + ' ')
00391         crc.accumulate_str(f.name + ' ')
00392         if f.array_length:
00393             crc.accumulate([f.array_length])
00394     return (crc.crc&0xFF) ^ (crc.crc>>8)
00395 
00396 def merge_enums(xml):
00397     '''merge enums between XML files'''
00398     emap = {}
00399     for x in xml:
00400         newenums = []
00401         for enum in x.enum:
00402             if enum.name in emap:
00403                 emapitem = emap[enum.name]
00404                 # check for possible conflicting auto-assigned values after merge
00405                 if (emapitem.start_value <= enum.highest_value and emapitem.highest_value >= enum.start_value):
00406                     for entry in emapitem.entry:
00407                         # correct the value if necessary, but only if it was auto-assigned to begin with
00408                         if entry.value <= enum.highest_value and entry.autovalue == True:
00409                             entry.value = enum.highest_value + 1
00410                             enum.highest_value = entry.value
00411                 # merge the entries
00412                 emapitem.entry.extend(enum.entry)
00413                 if not emapitem.description:
00414                     emapitem.description = enum.description
00415                 print("Merged enum %s" % enum.name)
00416             else:
00417                 newenums.append(enum)
00418                 emap[enum.name] = enum
00419         x.enum = newenums
00420     for e in emap:
00421         # sort by value
00422         emap[e].entry = sorted(emap[e].entry,
00423                                key=operator.attrgetter('value'),
00424                                reverse=False)
00425         # add a ENUM_END
00426         emap[e].entry.append(MAVEnumEntry("%s_ENUM_END" % emap[e].name,
00427                                             emap[e].entry[-1].value+1, end_marker=True))
00428 
00429 def check_duplicates(xml):
00430     '''check for duplicate message IDs'''
00431 
00432     merge_enums(xml)
00433 
00434     msgmap = {}
00435     enummap = {}
00436     for x in xml:
00437         for m in x.message:
00438             key = m.id
00439             if key in msgmap:
00440                 print("ERROR: Duplicate message id %u for %s (%s:%u) also used by %s" % (
00441                     m.id,
00442                     m.name,
00443                     x.filename, m.linenumber,
00444                     msgmap[key]))
00445                 return True
00446             fieldset = set()
00447             for f in m.fields:
00448                 if f.name in fieldset:
00449                     print("ERROR: Duplicate field %s in message %s (%s:%u)" % (
00450                         f.name, m.name,
00451                         x.filename, m.linenumber))
00452                     return True
00453                 fieldset.add(f.name)
00454             msgmap[key] = '%s (%s:%u)' % (m.name, x.filename, m.linenumber)
00455         for enum in x.enum:
00456             for entry in enum.entry:
00457                 if entry.autovalue == True and "common.xml" not in entry.origin_file:
00458                     print("Note: An enum value was auto-generated: %s = %u" % (entry.name, entry.value))
00459                 s1 = "%s.%s" % (enum.name, entry.name)
00460                 s2 = "%s.%s" % (enum.name, entry.value)
00461                 if s1 in enummap or s2 in enummap:
00462                     print("ERROR: Duplicate enum %s:\n\t%s = %s @ %s:%u\n\t%s" % (
00463                         "names" if s1 in enummap else "values",
00464                         s1, entry.value, entry.origin_file, entry.origin_line,
00465                         enummap.get(s1) or enummap.get(s2)))
00466                     return True
00467                 enummap[s1] = enummap[s2] = "%s.%s = %s @ %s:%u" % (enum.name, entry.name, entry.value, entry.origin_file, entry.origin_line)
00468 
00469     return False
00470 
00471 
00472 
00473 def total_msgs(xml):
00474     '''count total number of msgs'''
00475     count = 0
00476     for x in xml:
00477         count += len(x.message)
00478     return count
00479 
00480 def mkdir_p(dir):
00481     try:
00482         os.makedirs(dir)
00483     except OSError as exc:
00484         if exc.errno == errno.EEXIST:
00485             pass
00486         else: raise
00487 
00488 # check version consistent
00489 # add test.xml
00490 # finish test suite
00491 # printf style error macro, if defined call errors


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43