#!/usr/bin/env python ''' parse a MAVLink protocol XML file and generate a Node.js javascript module implementation Based on original work Copyright Andrew Tridgell 2011 Released under GNU GPL version 3 or later ''' from __future__ import print_function from builtins import range import os import textwrap from . import mavtemplate t = mavtemplate.MAVTemplate() def get_mavhead(xml): return ("mavlink20" if xml.protocol_marker == 253 else "mavlink10") def get_mavprocessor(xml): return ("MAVLink20Processor" if xml.protocol_marker == 253 else "MAVLink10Processor") def generate_preamble(outf, msgs, args, xml): print("Generating preamble") t.write(outf, """ /* MAVLink protocol implementation for node.js (auto-generated by mavgen_javascript.py) Generated from: ${FILELIST} Note: this file has been auto-generated. DO NOT EDIT */ jspack = require("jspack").jspack, _ = require("underscore"), events = require("events"), util = require("util"); // Add a convenience method to Buffer Buffer.prototype.toByteArray = function () { return Array.prototype.slice.call(this, 0) } ${MAVHEAD} = function(){}; // Implement the CRC-16/MCRF4XX function (present in the Python version through the mavutil.py package) ${MAVHEAD}.x25Crc = function(buffer, crcIN) { var bytes = buffer; var crcOUT = crcIN === undefined ? 0xffff : crcIN; _.each(bytes, function(e) { var tmp = e ^ (crcOUT & 0xff); tmp = (tmp ^ (tmp << 4)) & 0xff; crcOUT = (crcOUT >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); crcOUT = crcOUT & 0xffff; }); return crcOUT; } ${MAVHEAD}.WIRE_PROTOCOL_VERSION = "${WIRE_PROTOCOL_VERSION}"; ${MAVHEAD}.HEADER_LEN = ${HEADERLEN}; ${MAVHEAD}.MAVLINK_TYPE_CHAR = 0 ${MAVHEAD}.MAVLINK_TYPE_UINT8_T = 1 ${MAVHEAD}.MAVLINK_TYPE_INT8_T = 2 ${MAVHEAD}.MAVLINK_TYPE_UINT16_T = 3 ${MAVHEAD}.MAVLINK_TYPE_INT16_T = 4 ${MAVHEAD}.MAVLINK_TYPE_UINT32_T = 5 ${MAVHEAD}.MAVLINK_TYPE_INT32_T = 6 ${MAVHEAD}.MAVLINK_TYPE_UINT64_T = 7 ${MAVHEAD}.MAVLINK_TYPE_INT64_T = 8 ${MAVHEAD}.MAVLINK_TYPE_FLOAT = 9 ${MAVHEAD}.MAVLINK_TYPE_DOUBLE = 10 ${MAVHEAD}.MAVLINK_IFLAG_SIGNED = 0x01 // Mavlink headers incorporate sequence, source system (platform) and source component. ${MAVHEAD}.header = function(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags=0, compat_flags=0,) { this.mlen = ( typeof mlen === 'undefined' ) ? 0 : mlen; this.seq = ( typeof seq === 'undefined' ) ? 0 : seq; this.srcSystem = ( typeof srcSystem === 'undefined' ) ? 0 : srcSystem; this.srcComponent = ( typeof srcComponent === 'undefined' ) ? 0 : srcComponent; this.msgId = msgId this.incompat_flags = incompat_flags this.compat_flags = compat_flags } """, {'FILELIST' : ",".join(args), 'PROTOCOL_MARKER' : xml.protocol_marker, 'crc_extra' : xml.crc_extra, 'WIRE_PROTOCOL_VERSION' : ("2.0" if xml.protocol_marker == 253 else "1.0"), 'MAVHEAD': get_mavhead(xml), 'HEADERLEN': ("10" if xml.protocol_marker == 253 else "6")}) # Mavlink2 if (xml.protocol_marker == 253): t.write(outf, """ ${MAVHEAD}.header.prototype.pack = function() { return jspack.Pack('BBBBBBBHB', [${PROTOCOL_MARKER}, this.mlen, this.incompat_flags, this.compat_flags, this.seq, this.srcSystem, this.srcComponent, ((this.msgId & 0xFF) << 8) | ((this.msgId >> 8) & 0xFF), this.msgId>>16]); } """, {'PROTOCOL_MARKER' : xml.protocol_marker, 'MAVHEAD': get_mavhead(xml)}) # Mavlink1 else: t.write(outf, """ ${MAVHEAD}.header.prototype.pack = function() { return jspack.Pack('BBBBBB', [${PROTOCOL_MARKER}, this.mlen, this.seq, this.srcSystem, this.srcComponent, this.msgId]); } """, {'PROTOCOL_MARKER' : xml.protocol_marker, 'MAVHEAD': get_mavhead(xml)}) t.write(outf, """ // Base class declaration: mavlink.message will be the parent class for each // concrete implementation in mavlink.messages. ${MAVHEAD}.message = function() {}; // Convenience setter to facilitate turning the unpacked array of data into member properties ${MAVHEAD}.message.prototype.set = function(args) { _.each(this.fieldnames, function(e, i) { this[e] = args[i]; }, this); }; // This pack function builds the header and produces a complete MAVLink message, // including header and message CRC. ${MAVHEAD}.message.prototype.pack = function(mav, crc_extra, payload) { this.payload = payload; var plen = this.payload.length; """, {'MAVHEAD': get_mavhead(xml)}) # Mavlink2 only if (xml.protocol_marker == 253): t.write(outf, """ //in MAVLink2 we can strip trailing zeros off payloads. This allows for simple // variable length arrays and smaller packets while (plen > 1 && this.payload[plen-1] == 0) { plen = plen - 1; } this.payload = this.payload.slice(0, plen); """) t.write(outf, """ var incompat_flags = 0; this.header = new ${MAVHEAD}.header(this.id, this.payload.length, mav.seq, mav.srcSystem, mav.srcComponent, incompat_flags, 0,); this.msgbuf = this.header.pack().concat(this.payload); var crc = ${MAVHEAD}.x25Crc(this.msgbuf.slice(1)); // For now, assume always using crc_extra = True. TODO: check/fix this. crc = ${MAVHEAD}.x25Crc([crc_extra], crc); this.msgbuf = this.msgbuf.concat(jspack.Pack('= 1 && this.buf[0] != this.protocol_marker ) { // Strip the offending initial byte and throw an error. var badPrefix = this.buf[0]; this.bufInError = this.buf.slice(0,1); this.buf = this.buf.slice(1); this.expected_length = ${MAVHEAD}.HEADER_LEN; // TODO: enable subsequent prefix error suppression if robust_parsing is implemented //if(!this.have_prefix_error) { // this.have_prefix_error = true; throw new Error("Bad prefix ("+badPrefix+")"); //} } //else if( this.buf.length >= 1 && this.buf[0] == this.protocol_marker ) { // this.have_prefix_error = false; //} } // Determine the length. Leaves buffer untouched. ${MAVPROCESSOR}.prototype.parseLength = function() { if( this.buf.length >= 2 ) { var unpacked = jspack.Unpack('BB', this.buf.slice(0, 2)); this.expected_length = unpacked[1] + ${MAVHEAD}.HEADER_LEN + 2 // length of message + header + CRC } } // input some data bytes, possibly returning a new message ${MAVPROCESSOR}.prototype.parseChar = function(c) { var m = null; try { this.pushBuffer(c); this.parsePrefix(); this.parseLength(); m = this.parsePayload(); } catch(e) { this.log('error', e.message); this.total_receive_errors += 1; m = new ${MAVHEAD}.messages.bad_data(this.bufInError, e.message); this.bufInError = new Buffer.from([]); } if(null != m) { this.emit(m.name, m); this.emit('message', m); } return m; } ${MAVPROCESSOR}.prototype.parsePayload = function() { var m = null; // If we have enough bytes to try and read it, read it. if( this.expected_length >= 8 && this.buf.length >= this.expected_length ) { // Slice off the expected packet length, reset expectation to be to find a header. var mbuf = this.buf.slice(0, this.expected_length); // TODO: slicing off the buffer should depend on the error produced by the decode() function // - if a message we find a well formed message, cut-off the expected_length // - if the message is not well formed (correct prefix by accident), cut-off 1 char only this.buf = this.buf.slice(this.expected_length); this.expected_length = 6; // w.info("Attempting to parse packet, message candidate buffer is ["+mbuf.toByteArray()+"]"); try { m = this.decode(mbuf); this.total_packets_received += 1; } catch(e) { // Set buffer in question and re-throw to generic error handling this.bufInError = mbuf; throw e; } } return m; } // input some data bytes, possibly returning an array of new messages ${MAVPROCESSOR}.prototype.parseBuffer = function(s) { // Get a message, if one is available in the stream. var m = this.parseChar(s); // No messages available, bail. if ( null === m ) { return null; } // While more valid messages can be read from the existing buffer, add // them to the array of new messages and return them. var ret = [m]; while(true) { m = this.parseChar(); if ( null === m ) { // No more messages left. return ret; } ret.push(m); } } /* decode a buffer as a MAVLink message */ ${MAVPROCESSOR}.prototype.decode = function(msgbuf) { var magic, incompat_flags, compat_flags, mlen, seq, srcSystem, srcComponent, unpacked, msgId; // decode the header try { """, {'MAVPROCESSOR': get_mavprocessor(xml), 'MAVHEAD': get_mavhead(xml), 'PROTOCOL_MARKER': xml.protocol_marker}) # Mavlink2 only if (xml.protocol_marker == 253): t.write(outf, """ unpacked = jspack.Unpack('cBBBBBBHB', msgbuf.slice(0, 10)); magic = unpacked[0]; mlen = unpacked[1]; incompat_flags = unpacked[2]; compat_flags = unpacked[3]; seq = unpacked[4]; srcSystem = unpacked[5]; srcComponent = unpacked[6]; var msgIDlow = ((unpacked[7] & 0xFF) << 8) | ((unpacked[7] >> 8) & 0xFF); var msgIDhigh = unpacked[8]; msgId = msgIDlow | (msgIDhigh<<16); """, {'MAVHEAD': get_mavhead(xml)}) # Mavlink1 else: t.write(outf, """ unpacked = jspack.Unpack('cBBBBB', msgbuf.slice(0, 6)); magic = unpacked[0]; mlen = unpacked[1]; seq = unpacked[2]; srcSystem = unpacked[3]; srcComponent = unpacked[4]; msgId = unpacked[5]; """, {'MAVHEAD': get_mavhead(xml)}) t.write(outf, """ } catch(e) { throw new Error('Unable to unpack MAVLink header: ' + e.message); } if (magic.charCodeAt(0) != this.protocol_marker) { throw new Error("Invalid MAVLink prefix ("+magic.charCodeAt(0)+")"); } if( mlen != msgbuf.length - (${MAVHEAD}.HEADER_LEN + 2)) { throw new Error("Invalid MAVLink message length. Got " + (msgbuf.length - (${MAVHEAD}.HEADER_LEN + 2)) + " expected " + mlen + ", msgId=" + msgId); } if( false === _.has(${MAVHEAD}.map, msgId) ) { throw new Error("Unknown MAVLink message ID (" + msgId + ")"); } // decode the payload // refs: (fmt, type, order_map, crc_extra) = ${MAVHEAD}.map[msgId] var decoder = ${MAVHEAD}.map[msgId]; // decode the checksum try { var receivedChecksum = jspack.Unpack(' payload.length) { payload = Buffer.concat([payload, Buffer.alloc(paylen - payload.length)]); } """) t.write(outf, """ // Decode the payload and reorder the fields to match the order map. try { var t = jspack.Unpack(decoder.format, payload); } catch (e) { throw new Error('Unable to unpack MAVLink payload type='+decoder.type+' format='+decoder.format+' payloadLength='+ payload +': '+ e.message); } // Need to check if the message contains arrays var args = {}; const elementsInMsg = decoder.order_map.length; const actualElementsInMsg = JSON.parse(JSON.stringify(t)).length; if (elementsInMsg == actualElementsInMsg) { // Reorder the fields to match the order map _.each(t, function(e, i, l) { args[i] = t[decoder.order_map[i]] }); } else { // This message contains arrays var typeIndex = 1; var orderIndex = 0; var memberIndex = 0; var tempArgs = {}; // Walk through the fields for(var i = 0, size = decoder.format.length-1; i <= size; ++i) { var order = decoder.order_map[orderIndex]; var currentType = decoder.format[typeIndex]; if (isNaN(parseInt(currentType))) { // This field is not an array check the type and add it to the args tempArgs[orderIndex] = t[memberIndex]; memberIndex++; } else { // This field is part of an array, need to find the length of the array var arraySize = '' var newArray = [] while (!isNaN(decoder.format[typeIndex])) { arraySize = arraySize + decoder.format[typeIndex]; typeIndex++; } // Now that we know how long the array is, create an array with the values for(var j = 0, size = parseInt(arraySize); j < size; ++j){ newArray.push(t[j+orderIndex]); memberIndex++; } // Add the array to the args object arraySize = arraySize + decoder.format[typeIndex]; currentType = arraySize; tempArgs[orderIndex] = newArray; } orderIndex++; typeIndex++; } // Finally reorder the fields to match the order map _.each(t, function(e, i, l) { args[i] = tempArgs[decoder.order_map[i]] }); } // construct the message object try { var m = new decoder.type(args); m.set.call(m, args); } catch (e) { throw new Error('Unable to instantiate MAVLink message of type '+decoder.type+' : ' + e.message); } m.msgbuf = msgbuf; m.payload = payload m.crc = receivedChecksum; m.header = new ${MAVHEAD}.header(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags, compat_flags); this.log(m); return m; } """, {'MAVHEAD': get_mavhead(xml), 'MAVPROCESSOR': get_mavprocessor(xml), 'PROTOCOL_MARKER' : xml.protocol_marker}) def generate_footer(outf, xml): t.write(outf, """ // Expose this code as a module module.exports = {${MAVHEAD}, ${MAVPROCESSOR}}; """, {'MAVHEAD': get_mavhead(xml), 'MAVPROCESSOR': get_mavprocessor(xml)}) def generate(basename, xml): '''generate complete javascript implementation''' if basename.endswith('.js'): filename = basename else: filename = basename + '.js' msgs = [] enums = [] filelist = [] for x in xml: msgs.extend(x.message) enums.extend(x.enum) filelist.append(os.path.basename(x.filename)) for m in msgs: if xml[0].little_endian: m.fmtstr = '<' else: m.fmtstr = '>' for f in m.ordered_fields: m.fmtstr += mavfmt(f) m.order_map = [ 0 ] * len(m.fieldnames) for i in range(0, len(m.fieldnames)): m.order_map[i] = m.ordered_fieldnames.index(m.fieldnames[i]) print("Generating %s" % filename) outf = open(filename, "w") generate_preamble(outf, msgs, filelist, xml[0]) generate_enums(outf, enums, xml[0]) generate_message_ids(outf, msgs, xml[0]) generate_classes(outf, msgs, xml[0]) generate_mavlink_class(outf, msgs, xml[0]) generate_footer(outf, xml[0]) outf.close() print("Generated %s OK" % filename)