00001
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
00014 class MAVParseError(Exception):
00015 def __init__(self, message, inner_exception=None):
00016 self.message = message
00017 self.inner_exception = inner_exception
00018 self.exception_info = sys.exc_info()
00019 def __str__(self):
00020 return self.message
00021
00022 class MAVField(object):
00023 def __init__(self, name, type, print_format, xml, description='', enum=''):
00024 self.name = name
00025 self.name_upper = name.upper()
00026 self.description = description
00027 self.array_length = 0
00028 self.enum = enum
00029 self.omit_arg = False
00030 self.const_value = None
00031 self.print_format = print_format
00032 lengths = {
00033 'float' : 4,
00034 'double' : 8,
00035 'char' : 1,
00036 'int8_t' : 1,
00037 'uint8_t' : 1,
00038 'uint8_t_mavlink_version' : 1,
00039 'int16_t' : 2,
00040 'uint16_t' : 2,
00041 'int32_t' : 4,
00042 'uint32_t' : 4,
00043 'int64_t' : 8,
00044 'uint64_t' : 8,
00045 }
00046
00047 if type=='uint8_t_mavlink_version':
00048 type = 'uint8_t'
00049 self.omit_arg = True
00050 self.const_value = xml.version
00051
00052 aidx = type.find("[")
00053 if aidx != -1:
00054 assert type[-1:] == ']'
00055 self.array_length = int(type[aidx+1:-1])
00056 type = type[0:aidx]
00057 if type == 'array':
00058 type = 'int8_t'
00059 if type in lengths:
00060 self.type_length = lengths[type]
00061 self.type = type
00062 elif (type+"_t") in lengths:
00063 self.type_length = lengths[type+"_t"]
00064 self.type = type+'_t'
00065 else:
00066 raise MAVParseError("unknown type '%s'" % type)
00067 if self.array_length != 0:
00068 self.wire_length = self.array_length * self.type_length
00069 else:
00070 self.wire_length = self.type_length
00071 self.type_upper = self.type.upper()
00072
00073 def gen_test_value(self, i):
00074 '''generate a testsuite value for a MAVField'''
00075 if self.const_value:
00076 return self.const_value
00077 elif self.type == 'float':
00078 return 17.0 + self.wire_offset*7 + i
00079 elif self.type == 'double':
00080 return 123.0 + self.wire_offset*7 + i
00081 elif self.type == 'char':
00082 return chr(ord('A') + (self.wire_offset + i)%26)
00083 elif self.type in [ 'int8_t', 'uint8_t' ]:
00084 return (5 + self.wire_offset*67 + i) & 0xFF
00085 elif self.type in ['int16_t', 'uint16_t']:
00086 return (17235 + self.wire_offset*52 + i) & 0xFFFF
00087 elif self.type in ['int32_t', 'uint32_t']:
00088 return (963497464 + self.wire_offset*52 + i)&0xFFFFFFFF
00089 elif self.type in ['int64_t', 'uint64_t']:
00090 return 93372036854775807 + self.wire_offset*63 + i
00091 else:
00092 raise MAVError('unknown type %s' % self.type)
00093
00094 def set_test_value(self):
00095 '''set a testsuite value for a MAVField'''
00096 if self.array_length:
00097 self.test_value = []
00098 for i in range(self.array_length):
00099 self.test_value.append(self.gen_test_value(i))
00100 else:
00101 self.test_value = self.gen_test_value(0)
00102 if self.type == 'char' and self.array_length:
00103 v = ""
00104 for c in self.test_value:
00105 v += c
00106 self.test_value = v[:-1]
00107
00108
00109 class MAVType(object):
00110 def __init__(self, name, id, linenumber, description=''):
00111 self.name = name
00112 self.name_lower = name.lower()
00113 self.linenumber = linenumber
00114 self.id = int(id)
00115 self.description = description
00116 self.fields = []
00117 self.fieldnames = []
00118
00119 class MAVEnumParam(object):
00120 def __init__(self, index, description=''):
00121 self.index = index
00122 self.description = description
00123
00124 class MAVEnumEntry(object):
00125 def __init__(self, name, value, description='', end_marker=False):
00126 self.name = name
00127 self.value = value
00128 self.description = description
00129 self.param = []
00130 self.end_marker = end_marker
00131
00132 class MAVEnum(object):
00133 def __init__(self, name, linenumber, description=''):
00134 self.name = name
00135 self.description = description
00136 self.entry = []
00137 self.highest_value = 0
00138 self.linenumber = linenumber
00139
00140 class MAVXML(object):
00141 '''parse a mavlink XML file'''
00142 def __init__(self, filename, wire_protocol_version=PROTOCOL_0_9):
00143 self.filename = filename
00144 self.basename = os.path.basename(filename)
00145 if self.basename.lower().endswith(".xml"):
00146 self.basename = self.basename[:-4]
00147 self.basename_upper = self.basename.upper()
00148 self.message = []
00149 self.enum = []
00150 self.parse_time = time.asctime()
00151 self.version = 2
00152 self.include = []
00153 self.wire_protocol_version = wire_protocol_version
00154
00155 if wire_protocol_version == PROTOCOL_0_9:
00156 self.protocol_marker = ord('U')
00157 self.sort_fields = False
00158 self.little_endian = False
00159 self.crc_extra = False
00160 elif wire_protocol_version == PROTOCOL_1_0:
00161 self.protocol_marker = 0xFE
00162 self.sort_fields = True
00163 self.little_endian = True
00164 self.crc_extra = True
00165 else:
00166 print("Unknown wire protocol version")
00167 print("Available versions are: %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0))
00168 raise MAVParseError('Unknown MAVLink wire protocol version %s' % wire_protocol_version)
00169
00170 in_element_list = []
00171
00172 def check_attrs(attrs, check, where):
00173 for c in check:
00174 if not c in attrs:
00175 raise MAVParseError('expected missing %s "%s" attribute at %s:%u' % (
00176 where, c, filename, p.CurrentLineNumber))
00177
00178 def start_element(name, attrs):
00179 in_element_list.append(name)
00180 in_element = '.'.join(in_element_list)
00181
00182 if in_element == "mavlink.messages.message":
00183 check_attrs(attrs, ['name', 'id'], 'message')
00184 self.message.append(MAVType(attrs['name'], attrs['id'], p.CurrentLineNumber))
00185 elif in_element == "mavlink.messages.message.field":
00186 check_attrs(attrs, ['name', 'type'], 'field')
00187 if 'print_format' in attrs:
00188 print_format = attrs['print_format']
00189 else:
00190 print_format = None
00191 if 'enum' in attrs:
00192 enum = attrs['enum']
00193 else:
00194 enum = ''
00195 self.message[-1].fields.append(MAVField(attrs['name'], attrs['type'],
00196 print_format, self, enum=enum))
00197 elif in_element == "mavlink.enums.enum":
00198 check_attrs(attrs, ['name'], 'enum')
00199 self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber))
00200 elif in_element == "mavlink.enums.enum.entry":
00201 check_attrs(attrs, ['name'], 'enum entry')
00202 if 'value' in attrs:
00203 value = eval(attrs['value'])
00204 else:
00205 value = self.enum[-1].highest_value + 1
00206 if (value > self.enum[-1].highest_value):
00207 self.enum[-1].highest_value = value
00208 self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value))
00209 elif in_element == "mavlink.enums.enum.entry.param":
00210 check_attrs(attrs, ['index'], 'enum param')
00211 self.enum[-1].entry[-1].param.append(MAVEnumParam(attrs['index']))
00212
00213 def end_element(name):
00214 in_element_list.pop()
00215
00216 def char_data(data):
00217 in_element = '.'.join(in_element_list)
00218 if in_element == "mavlink.messages.message.description":
00219 self.message[-1].description += data
00220 elif in_element == "mavlink.messages.message.field":
00221 self.message[-1].fields[-1].description += data
00222 elif in_element == "mavlink.enums.enum.description":
00223 self.enum[-1].description += data
00224 elif in_element == "mavlink.enums.enum.entry.description":
00225 self.enum[-1].entry[-1].description += data
00226 elif in_element == "mavlink.enums.enum.entry.param":
00227 self.enum[-1].entry[-1].param[-1].description += data
00228 elif in_element == "mavlink.version":
00229 self.version = int(data)
00230 elif in_element == "mavlink.include":
00231 self.include.append(data)
00232
00233 f = open(filename, mode='rb')
00234 p = xml.parsers.expat.ParserCreate()
00235 p.StartElementHandler = start_element
00236 p.EndElementHandler = end_element
00237 p.CharacterDataHandler = char_data
00238 p.ParseFile(f)
00239 f.close()
00240
00241 self.message_lengths = [ 0 ] * 256
00242 self.message_crcs = [ 0 ] * 256
00243 self.message_names = [ None ] * 256
00244 self.largest_payload = 0
00245
00246 for m in self.message:
00247 m.wire_length = 0
00248 m.fieldnames = []
00249 m.fieldlengths = []
00250 m.ordered_fieldnames = []
00251 if self.sort_fields:
00252 m.ordered_fields = sorted(m.fields,
00253 key=operator.attrgetter('type_length'),
00254 reverse=True)
00255 else:
00256 m.ordered_fields = m.fields
00257 for f in m.fields:
00258 m.fieldnames.append(f.name)
00259 L = f.array_length
00260 if L == 0:
00261 m.fieldlengths.append(1)
00262 elif L > 1 and f.type == 'char':
00263 m.fieldlengths.append(1)
00264 else:
00265 m.fieldlengths.append(L)
00266 for f in m.ordered_fields:
00267 f.wire_offset = m.wire_length
00268 m.wire_length += f.wire_length
00269 m.ordered_fieldnames.append(f.name)
00270 f.set_test_value()
00271 if f.name.find('[') != -1:
00272 raise MAVParseError("invalid field name with array descriptor %s" % f.name)
00273 m.num_fields = len(m.fieldnames)
00274 if m.num_fields > 64:
00275 raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % (
00276 m.num_fields, 64))
00277 m.crc_extra = message_checksum(m)
00278 self.message_lengths[m.id] = m.wire_length
00279 self.message_names[m.id] = m.name
00280 self.message_crcs[m.id] = m.crc_extra
00281 if m.wire_length > self.largest_payload:
00282 self.largest_payload = m.wire_length
00283
00284 if m.wire_length+8 > 64:
00285 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))
00286
00287 def __str__(self):
00288 return "MAVXML for %s from %s (%u message, %u enums)" % (
00289 self.basename, self.filename, len(self.message), len(self.enum))
00290
00291
00292 def message_checksum(msg):
00293 '''calculate a 8-bit checksum of the key fields of a message, so we
00294 can detect incompatible XML changes'''
00295 from .mavcrc import x25crc
00296 crc = x25crc()
00297 crc.accumulate_str(msg.name + ' ')
00298 for f in msg.ordered_fields:
00299 crc.accumulate_str(f.type + ' ')
00300 crc.accumulate_str(f.name + ' ')
00301 if f.array_length:
00302 crc.accumulate([f.array_length])
00303 return (crc.crc&0xFF) ^ (crc.crc>>8)
00304
00305 def merge_enums(xml):
00306 '''merge enums between XML files'''
00307 emap = {}
00308 for x in xml:
00309 newenums = []
00310 for enum in x.enum:
00311 if enum.name in emap:
00312 emap[enum.name].entry.extend(enum.entry)
00313 if not emap[enum.name].description:
00314 emap[enum.name].description = enum.description
00315 print("Merged enum %s" % enum.name)
00316 else:
00317 newenums.append(enum)
00318 emap[enum.name] = enum
00319 x.enum = newenums
00320 for e in emap:
00321
00322 emap[e].entry = sorted(emap[e].entry,
00323 key=operator.attrgetter('value'),
00324 reverse=False)
00325
00326 emap[e].entry.append(MAVEnumEntry("%s_ENUM_END" % emap[e].name,
00327 emap[e].entry[-1].value+1, end_marker=True))
00328
00329 def check_duplicates(xml):
00330 '''check for duplicate message IDs'''
00331
00332 merge_enums(xml)
00333
00334 msgmap = {}
00335 enummap = {}
00336 for x in xml:
00337 for m in x.message:
00338 if m.id in msgmap:
00339 print("ERROR: Duplicate message id %u for %s (%s:%u) also used by %s" % (
00340 m.id, m.name,
00341 x.filename, m.linenumber,
00342 msgmap[m.id]))
00343 return True
00344 fieldset = set()
00345 for f in m.fields:
00346 if f.name in fieldset:
00347 print("ERROR: Duplicate field %s in message %s (%s:%u)" % (
00348 f.name, m.name,
00349 x.filename, m.linenumber))
00350 return True
00351 fieldset.add(f.name)
00352 msgmap[m.id] = '%s (%s:%u)' % (m.name, x.filename, m.linenumber)
00353 for enum in x.enum:
00354 for entry in enum.entry:
00355 s1 = "%s.%s" % (enum.name, entry.name)
00356 s2 = "%s.%s" % (enum.name, entry.value)
00357 if s1 in enummap or s2 in enummap:
00358 print("ERROR: Duplicate enums %s/%s at %s:%u and %s" % (
00359 s1, entry.value, x.filename, enum.linenumber,
00360 enummap.get(s1) or enummap.get(s2)))
00361 return True
00362 enummap[s1] = "%s:%u" % (x.filename, enum.linenumber)
00363 enummap[s2] = "%s:%u" % (x.filename, enum.linenumber)
00364
00365 return False
00366
00367
00368
00369 def total_msgs(xml):
00370 '''count total number of msgs'''
00371 count = 0
00372 for x in xml:
00373 count += len(x.message)
00374 return count
00375
00376 def mkdir_p(dir):
00377 try:
00378 os.makedirs(dir)
00379 except OSError as exc:
00380 if exc.errno == errno.EEXIST:
00381 pass
00382 else: raise
00383
00384
00385
00386
00387