mavgen_python.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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)


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57