mavgen_javascript.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 parse a MAVLink protocol XML file and generate a Node.js javascript module implementation
00004 
00005 Based on original work 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 from shutil import copyfile
00012 
00013 t = mavtemplate.MAVTemplate()
00014 
00015 def generate_preamble(outf, msgs, args, xml):
00016     print("Generating preamble")
00017     t.write(outf, """
00018 /*
00019 MAVLink protocol implementation for node.js (auto-generated by mavgen_javascript.py)
00020 
00021 Generated from: ${FILELIST}
00022 
00023 Note: this file has been auto-generated. DO NOT EDIT
00024 */
00025 
00026 jspack = require("jspack").jspack,
00027     _ = require("underscore"),
00028     events = require("events"),
00029     util = require("util");
00030 
00031 // Add a convenience method to Buffer
00032 Buffer.prototype.toByteArray = function () {
00033   return Array.prototype.slice.call(this, 0)
00034 }
00035 
00036 mavlink = function(){};
00037 
00038 // Implement the X25CRC function (present in the Python version through the mavutil.py package)
00039 mavlink.x25Crc = function(buffer, crc) {
00040 
00041     var bytes = buffer;
00042     var crc = crc || 0xffff;
00043     _.each(bytes, function(e) {
00044         var tmp = e ^ (crc & 0xff);
00045         tmp = (tmp ^ (tmp << 4)) & 0xff;
00046         crc = (crc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4);
00047         crc = crc & 0xffff;
00048     });
00049     return crc;
00050 
00051 }
00052 
00053 mavlink.WIRE_PROTOCOL_VERSION = "${WIRE_PROTOCOL_VERSION}";
00054 
00055 mavlink.MAVLINK_TYPE_CHAR     = 0
00056 mavlink.MAVLINK_TYPE_UINT8_T  = 1
00057 mavlink.MAVLINK_TYPE_INT8_T   = 2
00058 mavlink.MAVLINK_TYPE_UINT16_T = 3
00059 mavlink.MAVLINK_TYPE_INT16_T  = 4
00060 mavlink.MAVLINK_TYPE_UINT32_T = 5
00061 mavlink.MAVLINK_TYPE_INT32_T  = 6
00062 mavlink.MAVLINK_TYPE_UINT64_T = 7
00063 mavlink.MAVLINK_TYPE_INT64_T  = 8
00064 mavlink.MAVLINK_TYPE_FLOAT    = 9
00065 mavlink.MAVLINK_TYPE_DOUBLE   = 10
00066 
00067 // Mavlink headers incorporate sequence, source system (platform) and source component. 
00068 mavlink.header = function(msgId, mlen, seq, srcSystem, srcComponent) {
00069 
00070     this.mlen = ( typeof mlen === 'undefined' ) ? 0 : mlen;
00071     this.seq = ( typeof seq === 'undefined' ) ? 0 : seq;
00072     this.srcSystem = ( typeof srcSystem === 'undefined' ) ? 0 : srcSystem;
00073     this.srcComponent = ( typeof srcComponent === 'undefined' ) ? 0 : srcComponent;
00074     this.msgId = msgId
00075 
00076 }
00077 
00078 mavlink.header.prototype.pack = function() {
00079     return jspack.Pack('BBBBBB', [${PROTOCOL_MARKER}, this.mlen, this.seq, this.srcSystem, this.srcComponent, this.msgId]);
00080 }
00081 
00082 // Base class declaration: mavlink.message will be the parent class for each
00083 // concrete implementation in mavlink.messages.
00084 mavlink.message = function() {};
00085 
00086 // Convenience setter to facilitate turning the unpacked array of data into member properties
00087 mavlink.message.prototype.set = function(args) {
00088     _.each(this.fieldnames, function(e, i) {
00089         this[e] = args[i];
00090     }, this);
00091 };
00092 
00093 // This pack function builds the header and produces a complete MAVLink message,
00094 // including header and message CRC.
00095 mavlink.message.prototype.pack = function(mav, crc_extra, payload) {
00096 
00097     this.payload = payload;
00098     this.header = new mavlink.header(this.id, payload.length, mav.seq, mav.srcSystem, mav.srcComponent);    
00099     this.msgbuf = this.header.pack().concat(payload);
00100     var crc = mavlink.x25Crc(this.msgbuf.slice(1));
00101 
00102     // For now, assume always using crc_extra = True.  TODO: check/fix this.
00103     crc = mavlink.x25Crc([crc_extra], crc);
00104     this.msgbuf = this.msgbuf.concat(jspack.Pack('<H', [crc] ) );
00105     return this.msgbuf;
00106 
00107 }
00108 
00109 """, {'FILELIST' : ",".join(args),
00110       'PROTOCOL_MARKER' : xml.protocol_marker,
00111       'crc_extra' : xml.crc_extra,
00112       'WIRE_PROTOCOL_VERSION' : xml.wire_protocol_version })
00113 
00114 def generate_enums(outf, enums):
00115     print("Generating enums")
00116     outf.write("\n// enums\n")
00117     wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="                        // ")
00118     for e in enums:
00119         outf.write("\n// %s\n" % e.name)
00120         for entry in e.entry:
00121             outf.write("mavlink.%s = %u // %s\n" % (entry.name, entry.value, wrapper.fill(entry.description)))
00122 
00123 def generate_message_ids(outf, msgs):
00124     print("Generating message IDs")
00125     outf.write("\n// message IDs\n")
00126     outf.write("mavlink.MAVLINK_MSG_ID_BAD_DATA = -1\n")
00127     for m in msgs:
00128         outf.write("mavlink.MAVLINK_MSG_ID_%s = %u\n" % (m.name.upper(), m.id))
00129 
00130 def generate_classes(outf, msgs):
00131     """
00132     Generate the implementations of the classes representing MAVLink messages.
00133 
00134     """
00135     print("Generating class definitions")
00136     wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="")
00137     outf.write("\nmavlink.messages = {};\n\n");
00138 
00139     def field_descriptions(fields):
00140         ret = ""
00141         for f in fields:
00142             ret += "                %-18s        : %s (%s)\n" % (f.name, f.description.strip(), f.type)
00143         return ret
00144 
00145     for m in msgs:
00146 
00147         comment = "%s\n\n%s" % (wrapper.fill(m.description.strip()), field_descriptions(m.fields))
00148 
00149         selffieldnames = 'self, '
00150         for f in m.fields:
00151             # if f.omit_arg:
00152             #    selffieldnames += '%s=%s, ' % (f.name, f.const_value)
00153             #else:
00154             # -- Omitting the code above because it is rarely used (only once?) and would need some special handling
00155             # in javascript.  Specifically, inside the method definition, it needs to check for a value then assign
00156             # a default.
00157             selffieldnames += '%s, ' % f.name
00158         selffieldnames = selffieldnames[:-2]
00159 
00160         sub = {'NAMELOWER'      : m.name.lower(),
00161                'SELFFIELDNAMES' : selffieldnames,
00162                'COMMENT'        : comment,
00163                'FIELDNAMES'     : ", ".join(m.fieldnames)}
00164 
00165         t.write(outf, """
00166 /* 
00167 ${COMMENT}
00168 */
00169 """, sub)
00170 
00171         # function signature + declaration
00172         outf.write("mavlink.messages.%s = function(" % (m.name.lower()))
00173         if len(m.fields) != 0:
00174                 outf.write(", ".join(m.fieldnames))
00175         outf.write(") {")
00176 
00177         # body: set message type properties    
00178         outf.write("""
00179 
00180     this.format = '%s';
00181     this.id = mavlink.MAVLINK_MSG_ID_%s;
00182     this.order_map = %s;
00183     this.crc_extra = %u;
00184     this.name = '%s';
00185 
00186 """ % (m.fmtstr, m.name.upper(), m.order_map, m.crc_extra, m.name.upper()))
00187         
00188         # body: set own properties
00189         if len(m.fieldnames) != 0:
00190                 outf.write("    this.fieldnames = ['%s'];\n" % "', '".join(m.fieldnames))
00191         outf.write("""
00192 
00193     this.set(arguments);
00194 
00195 }
00196         """)
00197 
00198         # inherit methods from the base message class
00199         outf.write("""
00200 mavlink.messages.%s.prototype = new mavlink.message;
00201 """ % m.name.lower())
00202 
00203         # Implement the pack() function for this message
00204         outf.write("""
00205 mavlink.messages.%s.prototype.pack = function(mav) {
00206     return mavlink.message.prototype.pack.call(this, mav, this.crc_extra, jspack.Pack(this.format""" % m.name.lower())
00207         if len(m.fields) != 0:
00208                 outf.write(", [ this." + ", this.".join(m.ordered_fieldnames) + ']')
00209         outf.write("));\n}\n\n")
00210 
00211 def mavfmt(field):
00212     '''work out the struct format for a type'''
00213     map = {
00214         'float'    : 'f',
00215         'double'   : 'd',
00216         'char'     : 'c',
00217         'int8_t'   : 'b',
00218         'uint8_t'  : 'B',
00219         'uint8_t_mavlink_version'  : 'B',
00220         'int16_t'  : 'h',
00221         'uint16_t' : 'H',
00222         'int32_t'  : 'i',
00223         'uint32_t' : 'I',
00224         'int64_t'  : 'q',
00225         'uint64_t' : 'Q',
00226         }
00227 
00228     if field.array_length:
00229         if field.type in ['char', 'int8_t', 'uint8_t']:
00230             return str(field.array_length)+'s'
00231         return str(field.array_length)+map[field.type]
00232     return map[field.type]
00233 
00234 def generate_mavlink_class(outf, msgs, xml):
00235     print("Generating MAVLink class")
00236 
00237     # Write mapper to enable decoding based on the integer message type
00238     outf.write("\n\nmavlink.map = {\n");
00239     for m in msgs:
00240         outf.write("        %s: { format: '%s', type: mavlink.messages.%s, order_map: %s, crc_extra: %u },\n" % (
00241             m.id, m.fmtstr, m.name.lower(), m.order_map, m.crc_extra))
00242     outf.write("}\n\n")
00243     
00244     t.write(outf, """
00245 
00246 // Special mavlink message to capture malformed data packets for debugging
00247 mavlink.messages.bad_data = function(data, reason) {
00248     this.id = mavlink.MAVLINK_MSG_ID_BAD_DATA;
00249     this.data = data;
00250     this.reason = reason;
00251     this.msgbuf = data;
00252 }
00253 
00254 /* MAVLink protocol handling class */
00255 MAVLink = function(logger, srcSystem, srcComponent) {
00256 
00257     this.logger = logger;
00258 
00259     this.seq = 0;
00260     this.buf = new Buffer(0);
00261     this.bufInError = new Buffer(0);
00262    
00263     this.srcSystem = (typeof srcSystem === 'undefined') ? 0 : srcSystem;
00264     this.srcComponent =  (typeof srcComponent === 'undefined') ? 0 : srcComponent;
00265 
00266     // The first packet we expect is a valid header, 6 bytes.
00267     this.expected_length = 6;
00268 
00269     this.have_prefix_error = false;
00270 
00271     this.protocol_marker = 254;
00272     this.little_endian = true;
00273 
00274     this.crc_extra = true;
00275     this.sort_fields = true;
00276     this.total_packets_sent = 0;
00277     this.total_bytes_sent = 0;
00278     this.total_packets_received = 0;
00279     this.total_bytes_received = 0;
00280     this.total_receive_errors = 0;
00281     this.startup_time = Date.now();
00282     
00283 }
00284 
00285 // Implements EventEmitter
00286 util.inherits(MAVLink, events.EventEmitter);
00287 
00288 // If the logger exists, this function will add a message to it.
00289 // Assumes the logger is a winston object.
00290 MAVLink.prototype.log = function(message) {
00291     if(this.logger) {
00292         this.logger.info(message);
00293     }
00294 }
00295 
00296 MAVLink.prototype.log = function(level, message) {
00297     if(this.logger) {
00298         this.logger.log(level, message);
00299     }
00300 }
00301 
00302 MAVLink.prototype.send = function(mavmsg) {
00303     buf = mavmsg.pack(this);
00304     this.file.write(buf);
00305     this.seq = (this.seq + 1) % 256;
00306     this.total_packets_sent +=1;
00307     this.total_bytes_sent += buf.length;
00308 }
00309 
00310 // return number of bytes needed for next parsing stage
00311 MAVLink.prototype.bytes_needed = function() {
00312     ret = this.expected_length - this.buf.length;
00313     return ( ret <= 0 ) ? 1 : ret;
00314 }
00315 
00316 // add data to the local buffer
00317 MAVLink.prototype.pushBuffer = function(data) {
00318     if(data) {
00319         this.buf = Buffer.concat([this.buf, data]);
00320         this.total_bytes_received += data.length;
00321     }
00322 }
00323 
00324 // Decode prefix.  Elides the prefix.
00325 MAVLink.prototype.parsePrefix = function() {
00326 
00327     // Test for a message prefix.
00328     if( this.buf.length >= 1 && this.buf[0] != 254 ) {
00329 
00330         // Strip the offending initial byte and throw an error.
00331         var badPrefix = this.buf[0];
00332         this.bufInError = this.buf.slice(0,1);
00333         this.buf = this.buf.slice(1);
00334         this.expected_length = 6;
00335 
00336         // TODO: enable subsequent prefix error suppression if robust_parsing is implemented
00337         //if(!this.have_prefix_error) {
00338         //    this.have_prefix_error = true;
00339             throw new Error("Bad prefix ("+badPrefix+")");
00340         //}
00341 
00342     }
00343     //else if( this.buf.length >= 1 && this.buf[0] == 254 ) {
00344     //    this.have_prefix_error = false;
00345     //}
00346 
00347 }
00348 
00349 // Determine the length.  Leaves buffer untouched.
00350 MAVLink.prototype.parseLength = function() {
00351     
00352     if( this.buf.length >= 2 ) {
00353         var unpacked = jspack.Unpack('BB', this.buf.slice(0, 2));
00354         this.expected_length = unpacked[1] + 8; // length of message + header + CRC
00355     }
00356 
00357 }
00358 
00359 // input some data bytes, possibly returning a new message
00360 MAVLink.prototype.parseChar = function(c) {
00361 
00362     var m = null;
00363 
00364     try {
00365 
00366         this.pushBuffer(c);
00367         this.parsePrefix();
00368         this.parseLength();
00369         m = this.parsePayload();
00370 
00371     } catch(e) {
00372 
00373         this.log('error', e.message);
00374         this.total_receive_errors += 1;
00375         m = new mavlink.messages.bad_data(this.bufInError, e.message);
00376         this.bufInError = new Buffer(0);
00377         
00378     }
00379 
00380     if(null != m) {
00381         this.emit(m.name, m);
00382         this.emit('message', m);
00383     }
00384 
00385     return m;
00386 
00387 }
00388 
00389 MAVLink.prototype.parsePayload = function() {
00390 
00391     var m = null;
00392 
00393     // If we have enough bytes to try and read it, read it.
00394     if( this.expected_length >= 8 && this.buf.length >= this.expected_length ) {
00395 
00396         // Slice off the expected packet length, reset expectation to be to find a header.
00397         var mbuf = this.buf.slice(0, this.expected_length);
00398         // TODO: slicing off the buffer should depend on the error produced by the decode() function
00399         // - if a message we find a well formed message, cut-off the expected_length
00400         // - if the message is not well formed (correct prefix by accident), cut-off 1 char only
00401         this.buf = this.buf.slice(this.expected_length);
00402         this.expected_length = 6;
00403 
00404         // w.info("Attempting to parse packet, message candidate buffer is ["+mbuf.toByteArray()+"]");
00405 
00406         try {
00407             m = this.decode(mbuf);
00408             this.total_packets_received += 1;
00409         }
00410         catch(e) {
00411             // Set buffer in question and re-throw to generic error handling
00412             this.bufInError = mbuf;
00413             throw e;
00414         }
00415     }
00416 
00417     return m;
00418 
00419 }
00420 
00421 // input some data bytes, possibly returning an array of new messages
00422 MAVLink.prototype.parseBuffer = function(s) {
00423     
00424     // Get a message, if one is available in the stream.
00425     var m = this.parseChar(s);
00426 
00427     // No messages available, bail.
00428     if ( null === m ) {
00429         return null;
00430     }
00431     
00432     // While more valid messages can be read from the existing buffer, add
00433     // them to the array of new messages and return them.
00434     var ret = [m];
00435     while(true) {
00436         m = this.parseChar();
00437         if ( null === m ) {
00438             // No more messages left.
00439             return ret;
00440         }
00441         ret.push(m);
00442     }
00443     return ret;
00444 
00445 }
00446 
00447 /* decode a buffer as a MAVLink message */
00448 MAVLink.prototype.decode = function(msgbuf) {
00449 
00450     var magic, mlen, seq, srcSystem, srcComponent, unpacked, msgId;
00451 
00452     // decode the header
00453     try {
00454         unpacked = jspack.Unpack('cBBBBB', msgbuf.slice(0, 6));
00455         magic = unpacked[0];
00456         mlen = unpacked[1];
00457         seq = unpacked[2];
00458         srcSystem = unpacked[3];
00459         srcComponent = unpacked[4];
00460         msgId = unpacked[5];
00461     }
00462     catch(e) {
00463         throw new Error('Unable to unpack MAVLink header: ' + e.message);
00464     }
00465 
00466     if (magic.charCodeAt(0) != 254) {
00467         throw new Error("Invalid MAVLink prefix ("+magic.charCodeAt(0)+")");
00468     }
00469 
00470     if( mlen != msgbuf.length - 8 ) {
00471         throw new Error("Invalid MAVLink message length.  Got " + (msgbuf.length - 8) + " expected " + mlen + ", msgId=" + msgId);
00472     }
00473 
00474     if( false === _.has(mavlink.map, msgId) ) {
00475         throw new Error("Unknown MAVLink message ID (" + msgId + ")");
00476     }
00477 
00478     // decode the payload
00479     // refs: (fmt, type, order_map, crc_extra) = mavlink.map[msgId]
00480     var decoder = mavlink.map[msgId];
00481 
00482     // decode the checksum
00483     try {
00484         var receivedChecksum = jspack.Unpack('<H', msgbuf.slice(msgbuf.length - 2));
00485     } catch (e) {
00486         throw new Error("Unable to unpack MAVLink CRC: " + e.message);
00487     }
00488 
00489     var messageChecksum = mavlink.x25Crc(msgbuf.slice(1, msgbuf.length - 2));
00490 
00491     // Assuming using crc_extra = True.  See the message.prototype.pack() function.
00492     messageChecksum = mavlink.x25Crc([decoder.crc_extra], messageChecksum);
00493     
00494     if ( receivedChecksum != messageChecksum ) {
00495         throw new Error('invalid MAVLink CRC in msgID ' +msgId+ ', got 0x' + receivedChecksum + ' checksum, calculated payload checkum as 0x'+messageChecksum );
00496     }
00497 
00498     // Decode the payload and reorder the fields to match the order map.
00499     try {
00500         var t = jspack.Unpack(decoder.format, msgbuf.slice(6, msgbuf.length));
00501     }
00502     catch (e) {
00503         throw new Error('Unable to unpack MAVLink payload type='+decoder.type+' format='+decoder.format+' payloadLength='+ msgbuf.slice(6, -2).length +': '+ e.message);
00504     }
00505 
00506     // Reorder the fields to match the order map
00507     var args = [];
00508     _.each(t, function(e, i, l) {
00509         args[i] = t[decoder.order_map[i]]
00510     });
00511 
00512     // construct the message object
00513     try {
00514         var m = new decoder.type(args);
00515         m.set.call(m, args);
00516     }
00517     catch (e) {
00518         throw new Error('Unable to instantiate MAVLink message of type '+decoder.type+' : ' + e.message);
00519     }
00520     m.msgbuf = msgbuf;
00521     m.payload = msgbuf.slice(6);
00522     m.crc = receivedChecksum;
00523     m.header = new mavlink.header(msgId, mlen, seq, srcSystem, srcComponent);
00524     this.log(m);
00525     return m;
00526 }
00527 
00528 """, xml)
00529 
00530 def generate_footer(outf):
00531     t.write(outf, """
00532 
00533 // Expose this code as a module
00534 module.exports = mavlink;
00535 
00536 """)
00537 
00538 def generate(basename, xml):
00539     '''generate complete javascript implementation'''
00540 
00541     if basename.endswith('.js'):
00542         filename = basename
00543     else:
00544         filename = basename + '.js'
00545 
00546     msgs = []
00547     enums = []
00548     filelist = []
00549     for x in xml:
00550         msgs.extend(x.message)
00551         enums.extend(x.enum)
00552         filelist.append(os.path.basename(x.filename))
00553 
00554     for m in msgs:
00555         if xml[0].little_endian:
00556             m.fmtstr = '<'
00557         else:
00558             m.fmtstr = '>'
00559         for f in m.ordered_fields:
00560             m.fmtstr += mavfmt(f)
00561         m.order_map = [ 0 ] * len(m.fieldnames)
00562         for i in range(0, len(m.fieldnames)):
00563             m.order_map[i] = m.ordered_fieldnames.index(m.fieldnames[i])
00564 
00565     print("Generating %s" % filename)
00566     outf = open(filename, "w")
00567     generate_preamble(outf, msgs, filelist, xml[0])
00568     generate_enums(outf, enums)
00569     generate_message_ids(outf, msgs)
00570     generate_classes(outf, msgs)
00571     generate_mavlink_class(outf, msgs, xml[0])
00572     generate_footer(outf)
00573     outf.close()
00574     print("Generated %s OK" % filename)


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