00001
00002 '''
00003 parse a MAVLink protocol XML file and generate a python implementation
00004
00005 Copyright Andrew Tridgell 2011
00006 Released under GNU GPL version 3 or later
00007 '''
00008
00009 import sys, textwrap, os
00010 from . import mavparse, mavtemplate
00011
00012 t = mavtemplate.MAVTemplate()
00013
00014 def generate_preamble(outf, msgs, basename, args, xml):
00015 print("Generating preamble")
00016 t.write(outf, """
00017 '''
00018 MAVLink protocol implementation (auto-generated by mavgen.py)
00019
00020 Generated from: ${FILELIST}
00021
00022 Note: this file has been auto-generated. DO NOT EDIT
00023 '''
00024
00025 import struct, array, time, json, os, sys, platform
00026
00027 from ...generator.mavcrc import x25crc
00028 import hashlib
00029
00030 WIRE_PROTOCOL_VERSION = '${WIRE_PROTOCOL_VERSION}'
00031 DIALECT = '${DIALECT}'
00032
00033 PROTOCOL_MARKER_V1 = 0xFE
00034 PROTOCOL_MARKER_V2 = 0xFD
00035 HEADER_LEN_V1 = 6
00036 HEADER_LEN_V2 = 10
00037
00038 MAVLINK_SIGNATURE_BLOCK_LEN = 13
00039
00040 MAVLINK_IFLAG_SIGNED = 0x01
00041
00042 native_supported = platform.system() != 'Windows' # Not yet supported on other dialects
00043 native_force = 'MAVNATIVE_FORCE' in os.environ # Will force use of native code regardless of what client app wants
00044 native_testing = 'MAVNATIVE_TESTING' in os.environ # Will force both native and legacy code to be used and their results compared
00045
00046 if native_supported and float(WIRE_PROTOCOL_VERSION) <= 1:
00047 try:
00048 import mavnative
00049 except ImportError:
00050 print('ERROR LOADING MAVNATIVE - falling back to python implementation')
00051 native_supported = False
00052 else:
00053 # mavnative isn't supported for MAVLink2 yet
00054 native_supported = False
00055
00056 # some base types from mavlink_types.h
00057 MAVLINK_TYPE_CHAR = 0
00058 MAVLINK_TYPE_UINT8_T = 1
00059 MAVLINK_TYPE_INT8_T = 2
00060 MAVLINK_TYPE_UINT16_T = 3
00061 MAVLINK_TYPE_INT16_T = 4
00062 MAVLINK_TYPE_UINT32_T = 5
00063 MAVLINK_TYPE_INT32_T = 6
00064 MAVLINK_TYPE_UINT64_T = 7
00065 MAVLINK_TYPE_INT64_T = 8
00066 MAVLINK_TYPE_FLOAT = 9
00067 MAVLINK_TYPE_DOUBLE = 10
00068
00069
00070 class MAVLink_header(object):
00071 '''MAVLink message header'''
00072 def __init__(self, msgId, incompat_flags=0, compat_flags=0, mlen=0, seq=0, srcSystem=0, srcComponent=0):
00073 self.mlen = mlen
00074 self.seq = seq
00075 self.srcSystem = srcSystem
00076 self.srcComponent = srcComponent
00077 self.msgId = msgId
00078 self.incompat_flags = incompat_flags
00079 self.compat_flags = compat_flags
00080
00081 def pack(self):
00082 if WIRE_PROTOCOL_VERSION == '2.0':
00083 return struct.pack('<BBBBBBBHB', ${PROTOCOL_MARKER}, self.mlen,
00084 self.incompat_flags, self.compat_flags,
00085 self.seq, self.srcSystem, self.srcComponent,
00086 self.msgId&0xFFFF, self.msgId>>16)
00087 return struct.pack('<BBBBBB', PROTOCOL_MARKER_V1, self.mlen, self.seq,
00088 self.srcSystem, self.srcComponent, self.msgId)
00089
00090 class MAVLink_message(object):
00091 '''base MAVLink message class'''
00092 def __init__(self, msgId, name):
00093 self._header = MAVLink_header(msgId)
00094 self._payload = None
00095 self._msgbuf = None
00096 self._crc = None
00097 self._fieldnames = []
00098 self._type = name
00099 self._signed = False
00100 self._link_id = None
00101
00102 def get_msgbuf(self):
00103 if isinstance(self._msgbuf, bytearray):
00104 return self._msgbuf
00105 return bytearray(self._msgbuf)
00106
00107 def get_header(self):
00108 return self._header
00109
00110 def get_payload(self):
00111 return self._payload
00112
00113 def get_crc(self):
00114 return self._crc
00115
00116 def get_fieldnames(self):
00117 return self._fieldnames
00118
00119 def get_type(self):
00120 return self._type
00121
00122 def get_msgId(self):
00123 return self._header.msgId
00124
00125 def get_srcSystem(self):
00126 return self._header.srcSystem
00127
00128 def get_srcComponent(self):
00129 return self._header.srcComponent
00130
00131 def get_seq(self):
00132 return self._header.seq
00133
00134 def get_signed(self):
00135 return self._signed
00136
00137 def get_link_id(self):
00138 return self._link_id
00139
00140 def __str__(self):
00141 ret = '%s {' % self._type
00142 for a in self._fieldnames:
00143 v = getattr(self, a)
00144 ret += '%s : %s, ' % (a, v)
00145 ret = ret[0:-2] + '}'
00146 return ret
00147
00148 def __ne__(self, other):
00149 return not self.__eq__(other)
00150
00151 def __eq__(self, other):
00152 if other == None:
00153 return False
00154
00155 if self.get_type() != other.get_type():
00156 return False
00157
00158 # We do not compare CRC because native code doesn't provide it
00159 #if self.get_crc() != other.get_crc():
00160 # return False
00161
00162 if self.get_seq() != other.get_seq():
00163 return False
00164
00165 if self.get_srcSystem() != other.get_srcSystem():
00166 return False
00167
00168 if self.get_srcComponent() != other.get_srcComponent():
00169 return False
00170
00171 for a in self._fieldnames:
00172 if getattr(self, a) != getattr(other, a):
00173 return False
00174
00175 return True
00176
00177 def to_dict(self):
00178 d = dict({})
00179 d['mavpackettype'] = self._type
00180 for a in self._fieldnames:
00181 d[a] = getattr(self, a)
00182 return d
00183
00184 def to_json(self):
00185 return json.dumps(self.to_dict())
00186
00187 def sign_packet(self, mav):
00188 h = hashlib.new('sha256')
00189 self._msgbuf += struct.pack('<BQ', mav.signing.link_id, mav.signing.timestamp)[:7]
00190 h.update(mav.signing.secret_key)
00191 h.update(self._msgbuf)
00192 sig = h.digest()[:6]
00193 self._msgbuf += sig
00194 mav.signing.timestamp += 1
00195
00196 def pack(self, mav, crc_extra, payload):
00197 self._payload = payload
00198 incompat_flags = 0
00199 if mav.signing.sign_outgoing:
00200 incompat_flags |= MAVLINK_IFLAG_SIGNED
00201 self._header = MAVLink_header(self._header.msgId,
00202 incompat_flags=incompat_flags, compat_flags=0,
00203 mlen=len(payload), seq=mav.seq,
00204 srcSystem=mav.srcSystem, srcComponent=mav.srcComponent)
00205 self._msgbuf = self._header.pack() + payload
00206 crc = x25crc(self._msgbuf[1:])
00207 if ${crc_extra}: # using CRC extra
00208 crc.accumulate_str(struct.pack('B', crc_extra))
00209 self._crc = crc.crc
00210 self._msgbuf += struct.pack('<H', self._crc)
00211 if mav.signing.sign_outgoing:
00212 self.sign_packet(mav)
00213 return self._msgbuf
00214
00215 """, {'FILELIST' : ",".join(args),
00216 'PROTOCOL_MARKER' : xml.protocol_marker,
00217 'DIALECT' : os.path.splitext(os.path.basename(basename))[0],
00218 'crc_extra' : xml.crc_extra,
00219 'WIRE_PROTOCOL_VERSION' : xml.wire_protocol_version })
00220
00221 def generate_enums(outf, enums):
00222 print("Generating enums")
00223 outf.write('''
00224 # enums
00225
00226 class EnumEntry(object):
00227 def __init__(self, name, description):
00228 self.name = name
00229 self.description = description
00230 self.param = {}
00231
00232 enums = {}
00233 ''')
00234 wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent=" # ")
00235 for e in enums:
00236 outf.write("\n# %s\n" % e.name)
00237 outf.write("enums['%s'] = {}\n" % e.name)
00238 for entry in e.entry:
00239 outf.write("%s = %u # %s\n" % (entry.name, entry.value, wrapper.fill(entry.description)))
00240 outf.write("enums['%s'][%d] = EnumEntry('%s', '''%s''')\n" % (e.name,
00241 int(entry.value), entry.name,
00242 entry.description))
00243 for param in entry.param:
00244 outf.write("enums['%s'][%d].param[%d] = '''%s'''\n" % (e.name,
00245 int(entry.value),
00246 int(param.index),
00247 param.description))
00248
00249 def generate_message_ids(outf, msgs):
00250 print("Generating message IDs")
00251 outf.write("\n# message IDs\n")
00252 outf.write("MAVLINK_MSG_ID_BAD_DATA = -1\n")
00253 for m in msgs:
00254 outf.write("MAVLINK_MSG_ID_%s = %u\n" % (m.name.upper(), m.id))
00255
00256 def generate_classes(outf, msgs):
00257 print("Generating class definitions")
00258 wrapper = textwrap.TextWrapper(initial_indent=" ", subsequent_indent=" ")
00259 for m in msgs:
00260 classname = "MAVLink_%s_message" % m.name.lower()
00261 fieldname_str = ", ".join(map(lambda s: "'%s'" % s, m.fieldnames))
00262 ordered_fieldname_str = ", ".join(map(lambda s: "'%s'" % s, m.ordered_fieldnames))
00263
00264 outf.write("""
00265 class %s(MAVLink_message):
00266 '''
00267 %s
00268 '''
00269 id = MAVLINK_MSG_ID_%s
00270 name = '%s'
00271 fieldnames = [%s]
00272 ordered_fieldnames = [ %s ]
00273 format = '%s'
00274 native_format = bytearray('%s', 'ascii')
00275 orders = %s
00276 lengths = %s
00277 array_lengths = %s
00278 crc_extra = %s
00279
00280 def __init__(self""" % (classname, wrapper.fill(m.description.strip()),
00281 m.name.upper(),
00282 m.name.upper(),
00283 fieldname_str,
00284 ordered_fieldname_str,
00285 m.fmtstr,
00286 m.native_fmtstr,
00287 m.order_map,
00288 m.len_map,
00289 m.array_len_map,
00290 m.crc_extra))
00291 if len(m.fields) != 0:
00292 outf.write(", " + ", ".join(m.fieldnames))
00293 outf.write("):\n")
00294 outf.write(" MAVLink_message.__init__(self, %s.id, %s.name)\n" % (classname, classname))
00295 outf.write(" self._fieldnames = %s.fieldnames\n" % (classname))
00296 for f in m.fields:
00297 outf.write(" self.%s = %s\n" % (f.name, f.name))
00298 outf.write("""
00299 def pack(self, mav):
00300 return MAVLink_message.pack(self, mav, %u, struct.pack('%s'""" % (m.crc_extra, m.fmtstr))
00301 for field in m.ordered_fields:
00302 if (field.type != "char" and field.array_length > 1):
00303 for i in range(field.array_length):
00304 outf.write(", self.{0:s}[{1:d}]".format(field.name,i))
00305 else:
00306 outf.write(", self.{0:s}".format(field.name))
00307 outf.write("))\n")
00308
00309
00310 def native_mavfmt(field):
00311 '''work out the struct format for a type (in a form expected by mavnative)'''
00312 map = {
00313 'float' : 'f',
00314 'double' : 'd',
00315 'char' : 'c',
00316 'int8_t' : 'b',
00317 'uint8_t' : 'B',
00318 'uint8_t_mavlink_version' : 'v',
00319 'int16_t' : 'h',
00320 'uint16_t' : 'H',
00321 'int32_t' : 'i',
00322 'uint32_t' : 'I',
00323 'int64_t' : 'q',
00324 'uint64_t' : 'Q',
00325 }
00326 return map[field.type]
00327
00328 def mavfmt(field):
00329 '''work out the struct format for a type'''
00330 map = {
00331 'float' : 'f',
00332 'double' : 'd',
00333 'char' : 'c',
00334 'int8_t' : 'b',
00335 'uint8_t' : 'B',
00336 'uint8_t_mavlink_version' : 'B',
00337 'int16_t' : 'h',
00338 'uint16_t' : 'H',
00339 'int32_t' : 'i',
00340 'uint32_t' : 'I',
00341 'int64_t' : 'q',
00342 'uint64_t' : 'Q',
00343 }
00344
00345 if field.array_length:
00346 if field.type == 'char':
00347 return str(field.array_length)+'s'
00348 return str(field.array_length)+map[field.type]
00349 return map[field.type]
00350
00351 def generate_mavlink_class(outf, msgs, xml):
00352 print("Generating MAVLink class")
00353
00354 outf.write("\n\nmavlink_map = {\n");
00355 for m in msgs:
00356 outf.write(" MAVLINK_MSG_ID_%s : MAVLink_%s_message,\n" % (
00357 m.name.upper(), m.name.lower()))
00358 outf.write("}\n\n")
00359
00360 t.write(outf, """
00361 class MAVError(Exception):
00362 '''MAVLink error class'''
00363 def __init__(self, msg):
00364 Exception.__init__(self, msg)
00365 self.message = msg
00366
00367 class MAVString(str):
00368 '''NUL terminated string'''
00369 def __init__(self, s):
00370 str.__init__(self)
00371 def __str__(self):
00372 i = self.find(chr(0))
00373 if i == -1:
00374 return self[:]
00375 return self[0:i]
00376
00377 class MAVLink_bad_data(MAVLink_message):
00378 '''
00379 a piece of bad data in a mavlink stream
00380 '''
00381 def __init__(self, data, reason):
00382 MAVLink_message.__init__(self, MAVLINK_MSG_ID_BAD_DATA, 'BAD_DATA')
00383 self._fieldnames = ['data', 'reason']
00384 self.data = data
00385 self.reason = reason
00386 self._msgbuf = data
00387
00388 def __str__(self):
00389 '''Override the __str__ function from MAVLink_messages because non-printable characters are common in to be the reason for this message to exist.'''
00390 return '%s {%s, data:%s}' % (self._type, self.reason, [('%x' % ord(i) if isinstance(i, str) else '%x' % i) for i in self.data])
00391
00392 class MAVLinkSigning(object):
00393 '''MAVLink signing state class'''
00394 def __init__(self):
00395 self.secret_key = None
00396 self.timestamp = 0
00397 self.link_id = 0
00398 self.sign_outgoing = False
00399 self.allow_unsigned_callback = None
00400 self.stream_timestamps = {}
00401
00402 class MAVLink(object):
00403 '''MAVLink protocol handling class'''
00404 def __init__(self, file, srcSystem=0, srcComponent=0, use_native=False):
00405 self.seq = 0
00406 self.file = file
00407 self.srcSystem = srcSystem
00408 self.srcComponent = srcComponent
00409 self.callback = None
00410 self.callback_args = None
00411 self.callback_kwargs = None
00412 self.send_callback = None
00413 self.send_callback_args = None
00414 self.send_callback_kwargs = None
00415 self.buf = bytearray()
00416 self.buf_index = 0
00417 self.expected_length = HEADER_LEN_V1+2
00418 self.have_prefix_error = False
00419 self.robust_parsing = False
00420 self.protocol_marker = ${protocol_marker}
00421 self.little_endian = ${little_endian}
00422 self.crc_extra = ${crc_extra}
00423 self.sort_fields = ${sort_fields}
00424 self.total_packets_sent = 0
00425 self.total_bytes_sent = 0
00426 self.total_packets_received = 0
00427 self.total_bytes_received = 0
00428 self.total_receive_errors = 0
00429 self.startup_time = time.time()
00430 self.signing = MAVLinkSigning()
00431 if native_supported and (use_native or native_testing or native_force):
00432 print("NOTE: mavnative is currently beta-test code")
00433 self.native = mavnative.NativeConnection(MAVLink_message, mavlink_map)
00434 else:
00435 self.native = None
00436 if native_testing:
00437 self.test_buf = bytearray()
00438
00439 def set_callback(self, callback, *args, **kwargs):
00440 self.callback = callback
00441 self.callback_args = args
00442 self.callback_kwargs = kwargs
00443
00444 def set_send_callback(self, callback, *args, **kwargs):
00445 self.send_callback = callback
00446 self.send_callback_args = args
00447 self.send_callback_kwargs = kwargs
00448
00449 def send(self, mavmsg):
00450 '''send a MAVLink message'''
00451 buf = mavmsg.pack(self)
00452 self.file.write(buf)
00453 self.seq = (self.seq + 1) % 256
00454 self.total_packets_sent += 1
00455 self.total_bytes_sent += len(buf)
00456 if self.send_callback:
00457 self.send_callback(mavmsg, *self.send_callback_args, **self.send_callback_kwargs)
00458
00459 def buf_len(self):
00460 return len(self.buf) - self.buf_index
00461
00462 def bytes_needed(self):
00463 '''return number of bytes needed for next parsing stage'''
00464 if self.native:
00465 ret = self.native.expected_length - self.buf_len()
00466 else:
00467 ret = self.expected_length - self.buf_len()
00468
00469 if ret <= 0:
00470 return 1
00471 return ret
00472
00473 def __parse_char_native(self, c):
00474 '''this method exists only to see in profiling results'''
00475 m = self.native.parse_chars(c)
00476 return m
00477
00478 def __callbacks(self, msg):
00479 '''this method exists only to make profiling results easier to read'''
00480 if self.callback:
00481 self.callback(msg, *self.callback_args, **self.callback_kwargs)
00482
00483 def parse_char(self, c):
00484 '''input some data bytes, possibly returning a new message'''
00485 self.buf.extend(c)
00486
00487 self.total_bytes_received += len(c)
00488
00489 if self.native:
00490 if native_testing:
00491 self.test_buf.extend(c)
00492 m = self.__parse_char_native(self.test_buf)
00493 m2 = self.__parse_char_legacy()
00494 if m2 != m:
00495 print("Native: %s\\nLegacy: %s\\n" % (m, m2))
00496 raise Exception('Native vs. Legacy mismatch')
00497 else:
00498 m = self.__parse_char_native(self.buf)
00499 else:
00500 m = self.__parse_char_legacy()
00501
00502 if m != None:
00503 self.total_packets_received += 1
00504 self.__callbacks(m)
00505 else:
00506 # XXX The idea here is if we've read something and there's nothing left in
00507 # the buffer, reset it to 0 which frees the memory
00508 if self.buf_len() == 0 and self.buf_index != 0:
00509 self.buf = bytearray()
00510 self.buf_index = 0
00511
00512 return m
00513
00514 def __parse_char_legacy(self):
00515 '''input some data bytes, possibly returning a new message (uses no native code)'''
00516 header_len = HEADER_LEN_V1
00517 if self.buf_len() >= 1 and self.buf[self.buf_index] == PROTOCOL_MARKER_V2:
00518 header_len = HEADER_LEN_V2
00519
00520 if self.buf_len() >= 1 and self.buf[self.buf_index] != PROTOCOL_MARKER_V1 and self.buf[self.buf_index] != PROTOCOL_MARKER_V2:
00521 magic = self.buf[self.buf_index]
00522 self.buf_index += 1
00523 if self.robust_parsing:
00524 m = MAVLink_bad_data(chr(magic), 'Bad prefix')
00525 self.expected_length = header_len+2
00526 self.total_receive_errors += 1
00527 return m
00528 if self.have_prefix_error:
00529 return None
00530 self.have_prefix_error = True
00531 self.total_receive_errors += 1
00532 raise MAVError("invalid MAVLink prefix '%s'" % magic)
00533 self.have_prefix_error = False
00534 if self.buf_len() >= 3:
00535 sbuf = self.buf[self.buf_index:3+self.buf_index]
00536 if sys.version_info[0] < 3:
00537 sbuf = str(sbuf)
00538 (magic, self.expected_length, incompat_flags) = struct.unpack('BBB', sbuf)
00539 if magic == PROTOCOL_MARKER_V2 and (incompat_flags & MAVLINK_IFLAG_SIGNED):
00540 self.expected_length += MAVLINK_SIGNATURE_BLOCK_LEN
00541 self.expected_length += header_len + 2
00542 if self.expected_length >= (header_len+2) and self.buf_len() >= self.expected_length:
00543 mbuf = array.array('B', self.buf[self.buf_index:self.buf_index+self.expected_length])
00544 self.buf_index += self.expected_length
00545 self.expected_length = header_len+2
00546 if self.robust_parsing:
00547 try:
00548 if magic == PROTOCOL_MARKER_V2 and (incompat_flags & ~MAVLINK_IFLAG_SIGNED) != 0:
00549 raise MAVError('invalid incompat_flags 0x%x 0x%x %u' % (incompat_flags, magic, self.expected_length))
00550 m = self.decode(mbuf)
00551 except MAVError as reason:
00552 m = MAVLink_bad_data(mbuf, reason.message)
00553 self.total_receive_errors += 1
00554 else:
00555 if magic == PROTOCOL_MARKER_V2 and (incompat_flags & ~MAVLINK_IFLAG_SIGNED) != 0:
00556 raise MAVError('invalid incompat_flags 0x%x 0x%x %u' % (incompat_flags, magic, self.expected_length))
00557 m = self.decode(mbuf)
00558 return m
00559 return None
00560
00561 def parse_buffer(self, s):
00562 '''input some data bytes, possibly returning a list of new messages'''
00563 m = self.parse_char(s)
00564 if m is None:
00565 return None
00566 ret = [m]
00567 while True:
00568 m = self.parse_char("")
00569 if m is None:
00570 return ret
00571 ret.append(m)
00572 return ret
00573
00574 def check_signature(self, msgbuf, srcSystem, srcComponent):
00575 '''check signature on incoming message'''
00576 if isinstance(msgbuf, array.array):
00577 msgbuf = msgbuf.tostring()
00578 timestamp_buf = msgbuf[-12:-6]
00579 link_id = msgbuf[-13]
00580 (tlow, thigh) = struct.unpack('<IH', timestamp_buf)
00581 timestamp = tlow + (thigh<<32)
00582
00583 # see if the timestamp is acceptable
00584 stream_key = (link_id,srcSystem,srcComponent)
00585 if stream_key in self.signing.stream_timestamps:
00586 if timestamp <= self.signing.stream_timestamps[stream_key]:
00587 # reject old timestamp
00588 # print('old timestamp')
00589 return False
00590 else:
00591 # a new stream has appeared. Accept the timestamp if it is at most
00592 # one minute behind our current timestamp
00593 if timestamp + 6000*1000 < self.signing.timestamp:
00594 # print('bad new stream ', timestamp/(100.0*1000*60*60*24*365), self.signing.timestamp/(100.0*1000*60*60*24*365))
00595 return False
00596 self.signing.stream_timestamps[stream_key] = timestamp
00597 # print('new stream')
00598
00599 h = hashlib.new('sha256')
00600 h.update(self.signing.secret_key)
00601 h.update(msgbuf[:-6])
00602 sig1 = str(h.digest())[:6]
00603 sig2 = str(msgbuf)[-6:]
00604 if sig1 != sig2:
00605 # print('sig mismatch')
00606 return False
00607
00608 # the timestamp we next send with is the max of the received timestamp and
00609 # our current timestamp
00610 self.signing.timestamp = max(self.signing.timestamp, timestamp)
00611 return True
00612
00613 def decode(self, msgbuf):
00614 '''decode a buffer as a MAVLink message'''
00615 # decode the header
00616 if msgbuf[0] != PROTOCOL_MARKER_V1:
00617 headerlen = 10
00618 allow_smaller = True
00619 try:
00620 magic, mlen, incompat_flags, compat_flags, seq, srcSystem, srcComponent, msgIdlow, msgIdhigh = struct.unpack('<cBBBBBBHB', msgbuf[:headerlen])
00621 except struct.error as emsg:
00622 raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
00623 msgId = msgIdlow | (msgIdhigh<<16)
00624 mapkey = msgId
00625 else:
00626 headerlen = 6
00627 allow_smaller = False
00628 try:
00629 magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('<cBBBBB', msgbuf[:headerlen])
00630 incompat_flags = 0
00631 compat_flags = 0
00632 except struct.error as emsg:
00633 raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
00634 mapkey = msgId
00635 if (incompat_flags & MAVLINK_IFLAG_SIGNED) != 0:
00636 signature_len = MAVLINK_SIGNATURE_BLOCK_LEN
00637 else:
00638 signature_len = 0
00639
00640 if ord(magic) != PROTOCOL_MARKER_V1 and ord(magic) != PROTOCOL_MARKER_V2:
00641 raise MAVError("invalid MAVLink prefix '%s'" % magic)
00642 if mlen != len(msgbuf)-(headerlen+2+signature_len):
00643 raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u headerlen=%u' % (len(msgbuf)-(headerlen+2+signature_len), mlen, msgId, headerlen))
00644
00645 if not mapkey in mavlink_map:
00646 raise MAVError('unknown MAVLink message ID %s' % str(mapkey))
00647
00648 # decode the payload
00649 type = mavlink_map[mapkey]
00650 fmt = type.format
00651 order_map = type.orders
00652 len_map = type.lengths
00653 crc_extra = type.crc_extra
00654
00655 # decode the checksum
00656 try:
00657 crc, = struct.unpack('<H', msgbuf[-(2+signature_len):][:2])
00658 except struct.error as emsg:
00659 raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg)
00660 crcbuf = msgbuf[1:-(2+signature_len)]
00661 if ${crc_extra}: # using CRC extra
00662 crcbuf.append(crc_extra)
00663 crc2 = x25crc(crcbuf)
00664 if crc != crc2.crc:
00665 raise MAVError('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc))
00666
00667 sig_ok = False
00668 if self.signing.secret_key is not None:
00669 accept_signature = False
00670 if signature_len == MAVLINK_SIGNATURE_BLOCK_LEN:
00671 sig_ok = self.check_signature(msgbuf, srcSystem, srcComponent)
00672 accept_signature = sig_ok
00673 if not accept_signature and self.signing.allow_unsigned_callback is not None:
00674 accept_signature = self.signing.allow_unsigned_callback(self, msgId)
00675 elif self.signing.allow_unsigned_callback is not None:
00676 accept_signature = self.signing.allow_unsigned_callback(self, msgId)
00677 if not accept_signature:
00678 raise MAVError('Invalid signature')
00679
00680 csize = struct.calcsize(fmt)
00681 mbuf = msgbuf[headerlen:-(2+signature_len)]
00682 if len(mbuf) < csize and allow_smaller:
00683 # zero pad to give right size
00684 mbuf.extend([0]*(csize - len(mbuf)))
00685 if len(mbuf) < csize:
00686 raise MAVError('Bad message of type %s length %u needs %s' % (
00687 type, len(mbuf), csize))
00688 mbuf = mbuf[:csize]
00689 try:
00690 t = struct.unpack(fmt, mbuf)
00691 except struct.error as emsg:
00692 raise MAVError('Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s' % (
00693 type, fmt, len(mbuf), emsg))
00694
00695 tlist = list(t)
00696 # handle sorted fields
00697 if ${sort_fields}:
00698 t = tlist[:]
00699 if sum(len_map) == len(len_map):
00700 # message has no arrays in it
00701 for i in range(0, len(tlist)):
00702 tlist[i] = t[order_map[i]]
00703 else:
00704 # message has some arrays
00705 tlist = []
00706 for i in range(0, len(order_map)):
00707 order = order_map[i]
00708 L = len_map[order]
00709 tip = sum(len_map[:order])
00710 field = t[tip]
00711 if L == 1 or isinstance(field, str):
00712 tlist.append(field)
00713 else:
00714 tlist.append(t[tip:(tip + L)])
00715
00716 # terminate any strings
00717 for i in range(0, len(tlist)):
00718 if isinstance(tlist[i], str):
00719 tlist[i] = str(MAVString(tlist[i]))
00720 t = tuple(tlist)
00721 # construct the message object
00722 try:
00723 m = type(*t)
00724 except Exception as emsg:
00725 raise MAVError('Unable to instantiate MAVLink message of type %s : %s' % (type, emsg))
00726 m._signed = sig_ok
00727 if m._signed:
00728 m._link_id = msgbuf[-13]
00729 m._msgbuf = msgbuf
00730 m._payload = msgbuf[6:-(2+signature_len)]
00731 m._crc = crc
00732 m._header = MAVLink_header(msgId, incompat_flags, compat_flags, mlen, seq, srcSystem, srcComponent)
00733 return m
00734 """, xml)
00735
00736 def generate_methods(outf, msgs):
00737 print("Generating methods")
00738
00739 def field_descriptions(fields):
00740 ret = ""
00741 for f in fields:
00742 ret += " %-18s : %s (%s)\n" % (f.name, f.description.strip(), f.type)
00743 return ret
00744
00745 wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent=" ")
00746
00747 for m in msgs:
00748 comment = "%s\n\n%s" % (wrapper.fill(m.description.strip()), field_descriptions(m.fields))
00749
00750 selffieldnames = 'self, '
00751 for f in m.fields:
00752 if f.omit_arg:
00753 selffieldnames += '%s=%s, ' % (f.name, f.const_value)
00754 else:
00755 selffieldnames += '%s, ' % f.name
00756 selffieldnames = selffieldnames[:-2]
00757
00758 sub = {'NAMELOWER' : m.name.lower(),
00759 'SELFFIELDNAMES' : selffieldnames,
00760 'COMMENT' : comment,
00761 'FIELDNAMES' : ", ".join(m.fieldnames)}
00762
00763 t.write(outf, """
00764 def ${NAMELOWER}_encode(${SELFFIELDNAMES}):
00765 '''
00766 ${COMMENT}
00767 '''
00768 return MAVLink_${NAMELOWER}_message(${FIELDNAMES})
00769
00770 """, sub)
00771
00772 t.write(outf, """
00773 def ${NAMELOWER}_send(${SELFFIELDNAMES}):
00774 '''
00775 ${COMMENT}
00776 '''
00777 return self.send(self.${NAMELOWER}_encode(${FIELDNAMES}))
00778
00779 """, sub)
00780
00781
00782 def generate(basename, xml):
00783 '''generate complete python implemenation'''
00784 if basename.endswith('.py'):
00785 filename = basename
00786 else:
00787 filename = basename + '.py'
00788
00789 msgs = []
00790 enums = []
00791 filelist = []
00792 for x in xml:
00793 msgs.extend(x.message)
00794 enums.extend(x.enum)
00795 filelist.append(os.path.basename(x.filename))
00796
00797 for m in msgs:
00798 if xml[0].little_endian:
00799 m.fmtstr = '<'
00800 else:
00801 m.fmtstr = '>'
00802 m.native_fmtstr = m.fmtstr
00803 for f in m.ordered_fields:
00804 m.fmtstr += mavfmt(f)
00805 m.native_fmtstr += native_mavfmt(f)
00806 m.order_map = [ 0 ] * len(m.fieldnames)
00807 m.len_map = [ 0 ] * len(m.fieldnames)
00808 m.array_len_map = [ 0 ] * len(m.fieldnames)
00809 for i in range(0, len(m.fieldnames)):
00810 m.order_map[i] = m.ordered_fieldnames.index(m.fieldnames[i])
00811 m.array_len_map[i] = m.ordered_fields[i].array_length
00812 for i in range(0, len(m.fieldnames)):
00813 n = m.order_map[i]
00814 m.len_map[n] = m.fieldlengths[i]
00815
00816 print("Generating %s" % filename)
00817 outf = open(filename, "w")
00818 generate_preamble(outf, msgs, basename, filelist, xml[0])
00819 generate_enums(outf, enums)
00820 generate_message_ids(outf, msgs)
00821 generate_classes(outf, msgs)
00822 generate_mavlink_class(outf, msgs, xml[0])
00823 generate_methods(outf, msgs)
00824 outf.close()
00825 print("Generated %s OK" % filename)