font upload enabled, updated libraries
font upload enabled, updated libraries

file:a/Font.ino -> file:b/Font.ino
--- a/Font.ino
+++ b/Font.ino
@@ -1,4 +1,4 @@
-/*
+
 void uploadFont()
 {
     uint16_t byte_count = 0;
@@ -17,7 +17,7 @@
 //    osd.closePanel();
 
 
-    Serial.printf_P("Ready for Font\n");
+    Serial.printf_P(PSTR("Ready for Font\n"));
 
     while(font_count < 256) { 
         int8_t incomingByte = Serial.read();
@@ -84,13 +84,12 @@
             osd.write_NVM(font_count, character_bitmap);    
             byte_count = 0;
             font_count++;
-            Serial.printf_P("Char Done\n");
+            Serial.printf_P(PSTR("Char Done\n"));
         }
     }
 
     //  character_bitmap[]
 }
 
+

 
-*/

-

--- a/MAVLink.ino
+++ b/MAVLink.ino
@@ -137,11 +137,12 @@
                 crlf_count = 0;
             }
             if (crlf_count == 3) {
-                //uploadFont();
+                uploadFont();
             }
         }
         
         if (data == START_STOP) {
+          lastMAVBeat = millis();
           mavlink_active = 1;
           dataState = STATE_DATA_IN_FRAME;
           bufferIndex = 0;

--- a/OSD_Panels.ino
+++ b/OSD_Panels.ino
@@ -716,7 +716,7 @@
           }
  if (rotation > 5) rotation = 0;
 #ifdef FRSKY
-  if (warning[4] == 1 || warning[0] == 0) {
+  if (warning[4] == 1 && rotation == 5) {
         osd.printf("%s",warning_string);  
   }
 #else
@@ -884,8 +884,11 @@
         osd.printf(" %c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
     else osd.printf("%c%5.2f%c%c", 0xbc, (double)osd_vbat_A, 0x0d, osd_battery_pic_A);
     */
-//    osd.printf("%c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
+#ifdef FRSKY    
+    osd.printf("%c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
+#else    
     osd.printf("%5.2f%c", (double)osd_vbat_A, 0x0d);
+#endif    
     osd.closePanel();
 }
 

--- a/libraries/AP_Common/AP_Param.cpp
+++ b/libraries/AP_Common/AP_Param.cpp
@@ -874,16 +874,16 @@
 

         switch (type) {

         case AP_PARAM_INT8:

-            Serial.printf_P("%s: %d\n", s, (int)((AP_Int8 *)ap)->get());

+            Serial.printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int8 *)ap)->get());

             break;

         case AP_PARAM_INT16:

-            Serial.printf_P("%s: %d\n", s, (int)((AP_Int16 *)ap)->get());

+            Serial.printf_P(PSTR("%s: %d\n"), s, (int)((AP_Int16 *)ap)->get());

             break;

         case AP_PARAM_INT32:

-            Serial.printf_P("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get());

+            Serial.printf_P(PSTR("%s: %ld\n"), s, (long)((AP_Int32 *)ap)->get());

             break;

         case AP_PARAM_FLOAT:

-            Serial.printf_P("%s: %f\n", s, ((AP_Float *)ap)->get());

+            Serial.printf_P(PSTR("%s: %f\n"), s, ((AP_Float *)ap)->get());

             break;

         default:

             break;


--- a/libraries/FastSerial/BetterStream.cpp
+++ b/libraries/FastSerial/BetterStream.cpp
@@ -12,22 +12,22 @@
 // Enhancements to the Arduino Stream class.

 //

 

-#include "limits.h"

+#include <limits.h>

 #include "BetterStream.h"

 

 // Stream extensions////////////////////////////////////////////////////////////

 

 void

-BetterStream::print_P(const prog_char *s)

+BetterStream::print_P(const prog_char_t *s)

 {

         char    c;

 

-        while ('\0' != (c = pgm_read_byte(s++)))

+        while ('\0' != (c = pgm_read_byte((const prog_char *)s++)))

                 write(c);

 }

 

 void

-BetterStream::println_P(const char *s)

+BetterStream::println_P(const prog_char_t *s)

 {

         print_P(s);

         println();

@@ -44,7 +44,7 @@
 }

 

 void

-BetterStream::printf_P(const char *fmt, ...)

+BetterStream::_printf_P(const prog_char *fmt, ...)

 {

         va_list ap;

 

@@ -58,4 +58,5 @@
 {

         // by default claim that there is always space in transmit buffer

         return(INT_MAX);

-}
+}

+

--- a/libraries/FastSerial/BetterStream.h
+++ b/libraries/FastSerial/BetterStream.h
@@ -13,6 +13,7 @@
 

 #include <Stream.h>

 #include <avr/pgmspace.h>

+#include "../AP_Common/AP_Common.h"

 

 class BetterStream : public Stream {

 public:

@@ -20,14 +21,16 @@
         }

 

         // Stream extensions

-        void            print_P(const char *);

-        void            println_P(const char *);

+        void            print_P(const prog_char_t *);

+        void            println_P(const prog_char_t *);

         void            printf(const char *, ...)

                 __attribute__ ((format(__printf__, 2, 3)));

-        void            printf_P(const char *, ...)

+        void            _printf_P(const prog_char *, ...);

                 __attribute__ ((format(__printf__, 2, 3)));

 

         virtual int     txspace(void);

+

+#define printf_P(fmt, ...) _printf_P((const prog_char *)fmt, ## __VA_ARGS__)

 

 private:

         void            _vprintf(unsigned char, const char *, va_list)


--- a/libraries/FastSerial/FastSerial.cpp
+++ b/libraries/FastSerial/FastSerial.cpp
@@ -1,10 +1,10 @@
-// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+// -*-  tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

 //

 // Interrupt-driven serial transmit/receive library.

 //

 //      Copyright (c) 2010 Michael Smith. All rights reserved.

 //

-// Receive and baudrate calculations derived from the Arduino 

+// Receive and baudrate calculations derived from the Arduino

 // HardwareSerial driver:

 //

 //      Copyright (c) 2006 Nicholas Zambetti.  All right reserved.

@@ -31,7 +31,12 @@
 

 //#include "../AP_Common/AP_Common.h"

 #include "FastSerial.h"

-#include "WProgram.h"

+

+#if defined(ARDUINO) && ARDUINO >= 100

+	#include "Arduino.h"

+#else

+	#include "WProgram.h"

+#endif

 

 #if   defined(UDR3)

 # define FS_MAX_PORTS   4

@@ -43,128 +48,106 @@
 # define FS_MAX_PORTS   1

 #endif

 

-FastSerial::Buffer	__FastSerial__rxBuffer[FS_MAX_PORTS];

-FastSerial::Buffer	__FastSerial__txBuffer[FS_MAX_PORTS];

-

-// Default buffer sizes

-#define RX_BUFFER_SIZE  128

-#define TX_BUFFER_SIZE  64

-#define BUFFER_MAX      512

+FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS];

+FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS];

+uint8_t FastSerial::_serialInitialized = 0;

 

 // Constructor /////////////////////////////////////////////////////////////////

 

-FastSerial::FastSerial(const uint8_t portNumber,

-                       volatile uint8_t *ubrrh,

-                       volatile uint8_t *ubrrl,

-                       volatile uint8_t *ucsra,

-                       volatile uint8_t *ucsrb,

-                       const uint8_t u2x,

-                       const uint8_t portEnableBits,

-                       const uint8_t portTxBits)

-{

-        _ubrrh = ubrrh;

-        _ubrrl = ubrrl;

-        _ucsra = ucsra;

-        _ucsrb = ucsrb;

-        _u2x   = u2x;

-        _portEnableBits = portEnableBits;

-        _portTxBits     = portTxBits;

-

-        // init buffers

-        _rxBuffer = &__FastSerial__rxBuffer[portNumber];

-        _txBuffer->head = _txBuffer->tail = 0;

-        _txBuffer = &__FastSerial__txBuffer[portNumber];

-        _rxBuffer->head = _rxBuffer->tail = 0;

+FastSerial::FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl,

+					   volatile uint8_t *ucsra, volatile uint8_t *ucsrb, const uint8_t u2x,

+					   const uint8_t portEnableBits, const uint8_t portTxBits) :

+					   _ubrrh(ubrrh),

+					   _ubrrl(ubrrl),

+					   _ucsra(ucsra),

+					   _ucsrb(ucsrb),

+					   _u2x(u2x),

+					   _portEnableBits(portEnableBits),

+					   _portTxBits(portTxBits),

+					   _rxBuffer(&__FastSerial__rxBuffer[portNumber]),

+					   _txBuffer(&__FastSerial__txBuffer[portNumber])

+{

+	setInitialized(portNumber);

+	begin(57600);

 }

 

 // Public Methods //////////////////////////////////////////////////////////////

 

 void FastSerial::begin(long baud)

 {

-        unsigned int    rxb, txb;

-

-        // If we are re-configuring an already-open port, preserve the

-        // existing buffer sizes.

-        if (_open) {

-                rxb = _rxBuffer->mask + 1;

-                txb = _txBuffer->mask + 1;

-        } else {

-                rxb = RX_BUFFER_SIZE;

-                txb = TX_BUFFER_SIZE;

-        }

-

-        begin(baud, RX_BUFFER_SIZE, TX_BUFFER_SIZE);

+	begin(baud, 0, 0);

 }

 

 void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace)

 {

-        uint16_t        ubrr;

-        bool            use_u2x = false;

-        int             ureg, u2;

-        long            breg, b2, dreg, d2;

-       

-        // if we are currently open, close and restart

-        if (_open)

-                end();

-

-        // allocate buffers

-        if (!_allocBuffer(_rxBuffer, rxSpace ? : RX_BUFFER_SIZE) ||

-            !_allocBuffer(_txBuffer, txSpace ? : TX_BUFFER_SIZE)) {

-                end();

-                return;                 // couldn't allocate buffers - fatal

-        }

-        _open = true;

-

-        // U2X mode is needed for bitrates higher than (F_CPU / 16)

-        if (baud > F_CPU / 16) {

-                use_u2x = true;

-                ubrr = F_CPU / (8 * baud) - 1;

-        } else {

-                // Determine whether u2x mode would give a closer

-                // approximation of the desired speed.

-    

-                // ubrr for non-2x mode, corresponding baudrate and delta

-                ureg = F_CPU / 16 / baud - 1;

-                breg = F_CPU / 16 / (ureg + 1);

-                dreg = abs(baud - breg);

-                

-                // ubrr for 2x mode, corresponding bitrate and delta

-                u2   = F_CPU / 8 / baud - 1;

-                b2   = F_CPU / 8 / (u2 + 1); 

-                d2   = abs(baud - b2);

-

-                // Pick the setting that gives the smallest rate

-                // error, preferring non-u2x mode if the delta is

-                // identical.

-                if (dreg <= d2) {

-                        ubrr = ureg;

-                } else {

-                        ubrr = u2;

-                        use_u2x = true;

-                }                

-        }

-  

-        *_ucsra = use_u2x ? _BV(_u2x) : 0;

-        *_ubrrh = ubrr >> 8;

-        *_ubrrl = ubrr;

-        *_ucsrb |= _portEnableBits;

+	uint16_t ubrr;

+	bool use_u2x = true;

+

+	// if we are currently open...

+	if (_open) {

+		// If the caller wants to preserve the buffer sizing, work out what

+		// it currently is...

+		if (0 == rxSpace)

+			rxSpace = _rxBuffer->mask + 1;

+		if (0 == txSpace)

+			txSpace = _txBuffer->mask + 1;

+

+		// close the port in its current configuration, clears _open

+		end();

+	}

+

+	// allocate buffers

+	if (!_allocBuffer(_rxBuffer, rxSpace ? : _default_rx_buffer_size) || !_allocBuffer(_txBuffer, txSpace ?	: _default_tx_buffer_size)) {

+		end();

+		return; // couldn't allocate buffers - fatal

+	}

+

+	// reset buffer pointers

+	_txBuffer->head = _txBuffer->tail = 0;

+	_rxBuffer->head = _rxBuffer->tail = 0;

+

+	// mark the port as open

+	_open = true;

+

+	// If the user has supplied a new baud rate, compute the new UBRR value.

+	if (baud > 0) {

+#if F_CPU == 16000000UL

+		// hardcoded exception for compatibility with the bootloader shipped

+		// with the Duemilanove and previous boards and the firmware on the 8U2

+		// on the Uno and Mega 2560.

+		if (baud == 57600)

+			use_u2x = false;

+#endif

+

+		if (use_u2x) {

+			*_ucsra = 1 << _u2x;

+			ubrr = (F_CPU / 4 / baud - 1) / 2;

+		} else {

+			*_ucsra = 0;

+			ubrr = (F_CPU / 8 / baud - 1) / 2;

+		}

+

+		*_ubrrh = ubrr >> 8;

+		*_ubrrl = ubrr;

+	}

+

+	*_ucsrb |= _portEnableBits;

 }

 

 void FastSerial::end()

 {

-        *_ucsrb &= ~(_portEnableBits | _portTxBits);

-

-        _freeBuffer(_rxBuffer);

-        _freeBuffer(_txBuffer);

-        _open = false;

-}

-

-int

-FastSerial::available(void)

-{

-        if (!_open)

-                return(-1);

-        return((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask);

+	*_ucsrb &= ~(_portEnableBits | _portTxBits);

+

+	_freeBuffer(_rxBuffer);

+	_freeBuffer(_txBuffer);

+	_open = false;

+}

+

+int FastSerial::available(void)

+{

+	if (!_open)

+		return (-1);

+	return ((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask);

 }

 

 int FastSerial::txspace(void)

@@ -174,109 +157,149 @@
 	return ((_txBuffer->mask+1) - ((_txBuffer->head - _txBuffer->tail) & _txBuffer->mask));

 }

 

-int

-FastSerial::read(void)

-{

-        uint8_t         c;

-

-        // if the head and tail are equal, the buffer is empty

-        if (!_open || (_rxBuffer->head == _rxBuffer->tail))

-                return(-1);

-

-        // pull character from tail

-        c = _rxBuffer->bytes[_rxBuffer->tail];

-        _rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask;

-

-        return(c);

-}

-

-int

-FastSerial::peek(void)

-{

-

-        // if the head and tail are equal, the buffer is empty

-        if (!_open || (_rxBuffer->head == _rxBuffer->tail))

-                return(-1);

-

-        // pull character from tail

-        return(_rxBuffer->bytes[_rxBuffer->tail]);

-}

-

-

-void

-FastSerial::flush(void)

-{

-        // don't reverse this or there may be problems if the RX interrupt

-        // occurs after reading the value of _rxBuffer->head but before writing

-        // the value to _rxBuffer->tail; the previous value of head

-        // may be written to tail, making it appear as if the buffer

-        // don't reverse this or there may be problems if the RX interrupt

-        // occurs after reading the value of head but before writing

-        // the value to tail; the previous value of rx_buffer_head

-        // may be written to tail, making it appear as if the buffer

-        // were full, not empty.

-        _rxBuffer->head = _rxBuffer->tail;

-

-        // don't reverse this or there may be problems if the TX interrupt

-        // occurs after reading the value of _txBuffer->tail but before writing

-        // the value to _txBuffer->head.

-        _txBuffer->tail = _rxBuffer->head;

-}

-

-size_t

-FastSerial::write(uint8_t c)

-{

-        int16_t         i;

-

-        if (!_open)                     // drop bytes if not open

-                return c;

-

-        // wait for room in the tx buffer

-        i = (_txBuffer->head + 1) & _txBuffer->mask;

-        while (i == _txBuffer->tail)

-                ;

-

-        // add byte to the buffer

-        _txBuffer->bytes[_txBuffer->head] = c;

-        _txBuffer->head = i;

-

-        // enable the data-ready interrupt, as it may be off if the buffer is empty

-        *_ucsrb |= _portTxBits;

-}

+int FastSerial::read(void)

+{

+	uint8_t c;

+

+	// if the head and tail are equal, the buffer is empty

+	if (!_open || (_rxBuffer->head == _rxBuffer->tail))

+		return (-1);

+

+	// pull character from tail

+	c = _rxBuffer->bytes[_rxBuffer->tail];

+	_rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask;

+

+	return (c);

+}

+

+int FastSerial::peek(void)

+{

+

+	// if the head and tail are equal, the buffer is empty

+	if (!_open || (_rxBuffer->head == _rxBuffer->tail))

+		return (-1);

+

+	// pull character from tail

+	return (_rxBuffer->bytes[_rxBuffer->tail]);

+}

+

+void FastSerial::flush(void)

+{

+	// don't reverse this or there may be problems if the RX interrupt

+	// occurs after reading the value of _rxBuffer->head but before writing

+	// the value to _rxBuffer->tail; the previous value of head

+	// may be written to tail, making it appear as if the buffer

+	// don't reverse this or there may be problems if the RX interrupt

+	// occurs after reading the value of head but before writing

+	// the value to tail; the previous value of rx_buffer_head

+	// may be written to tail, making it appear as if the buffer

+	// were full, not empty.

+	_rxBuffer->head = _rxBuffer->tail;

+

+	// don't reverse this or there may be problems if the TX interrupt

+	// occurs after reading the value of _txBuffer->tail but before writing

+	// the value to _txBuffer->head.

+	_txBuffer->tail = _txBuffer->head;

+}

+

+#if defined(ARDUINO) && ARDUINO >= 100

+size_t FastSerial::write(uint8_t c)

+{

+	uint16_t i;

+

+	if (!_open) // drop bytes if not open

+		return 0;

+

+	// wait for room in the tx buffer

+	i = (_txBuffer->head + 1) & _txBuffer->mask;

+

+	// if the port is set into non-blocking mode, then drop the byte

+	// if there isn't enough room for it in the transmit buffer

+	if (_nonblocking_writes && i == _txBuffer->tail) {

+		return 0;

+	}

+

+	while (i == _txBuffer->tail)

+		;

+

+	// add byte to the buffer

+	_txBuffer->bytes[_txBuffer->head] = c;

+	_txBuffer->head = i;

+

+	// enable the data-ready interrupt, as it may be off if the buffer is empty

+	*_ucsrb |= _portTxBits;

+

+	// return number of bytes written (always 1)

+	return 1;

+}

+#else

+void FastSerial::write(uint8_t c)

+{

+	uint16_t i;

+

+	if (!_open) // drop bytes if not open

+		return;

+

+	// wait for room in the tx buffer

+	i = (_txBuffer->head + 1) & _txBuffer->mask;

+	while (i == _txBuffer->tail)

+		;

+

+	// add byte to the buffer

+	_txBuffer->bytes[_txBuffer->head] = c;

+	_txBuffer->head = i;

+

+	// enable the data-ready interrupt, as it may be off if the buffer is empty

+	*_ucsrb |= _portTxBits;

+}

+#endif

 

 // Buffer management ///////////////////////////////////////////////////////////

 

-bool

-FastSerial::_allocBuffer(Buffer *buffer, unsigned int size)

-{

-        uint8_t shift;

-

-        // init buffer state

-        buffer->head = buffer->tail = 0;

-

-        // Compute the power of 2 greater or equal to the requested buffer size

-        // and then a mask to simplify wrapping operations.  Using __builtin_clz

-        // would seem to make sense, but it uses a 256(!) byte table.

-        // Note that we ignore requests for more than BUFFER_MAX space.

-        for (shift = 1; (1U << shift) < min(BUFFER_MAX, size); shift++)

-                ;

-        buffer->mask = (1 << shift) - 1;

-

-        // allocate memory for the buffer - if this fails, we fail

-        buffer->bytes = (uint8_t *)malloc(buffer->mask + 1);

-

-        return(buffer->bytes != NULL);

-}

-

-void

-FastSerial::_freeBuffer(Buffer *buffer)

-{

-        buffer->head = buffer->tail = 0;

-        buffer->mask = 0;

-        if (NULL != buffer->bytes) {

-                free(buffer->bytes);

-                buffer->bytes = NULL;

-        }

+bool FastSerial::_allocBuffer(Buffer *buffer, unsigned int size)

+{

+	uint16_t	mask;

+	uint8_t		shift;

+

+	// init buffer state

+	buffer->head = buffer->tail = 0;

+

+	// Compute the power of 2 greater or equal to the requested buffer size

+	// and then a mask to simplify wrapping operations.  Using __builtin_clz

+	// would seem to make sense, but it uses a 256(!) byte table.

+	// Note that we ignore requests for more than BUFFER_MAX space.

+	for (shift = 1; (1U << shift) < min(_max_buffer_size, size); shift++)

+		;

+	mask = (1 << shift) - 1;

+

+	// If the descriptor already has a buffer allocated we need to take

+	// care of it.

+	if (buffer->bytes) {

+

+		// If the allocated buffer is already the correct size then

+		// we have nothing to do

+		if (buffer->mask == mask)

+			return true;

+

+		// Dispose of the old buffer.

+		free(buffer->bytes);

+	}

+	buffer->mask = mask;

+

+	// allocate memory for the buffer - if this fails, we fail.

+	buffer->bytes = (uint8_t *) malloc(buffer->mask + 1);

+

+	return (buffer->bytes != NULL);

+}

+

+void FastSerial::_freeBuffer(Buffer *buffer)

+{

+	buffer->head = buffer->tail = 0;

+	buffer->mask = 0;

+	if (NULL != buffer->bytes) {

+		free(buffer->bytes);

+		buffer->bytes = NULL;

+	}

 }

 

 

--- a/libraries/FastSerial/FastSerial.h
+++ b/libraries/FastSerial/FastSerial.h
@@ -1,10 +1,10 @@
-// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

 //

 // Interrupt-driven serial transmit/receive library.

 //

 //      Copyright (c) 2010 Michael Smith. All rights reserved.

 //

-// Receive and baudrate calculations derived from the Arduino 

+// Receive and baudrate calculations derived from the Arduino

 // HardwareSerial driver:

 //

 //      Copyright (c) 2006 Nicholas Zambetti.  All right reserved.

@@ -35,7 +35,7 @@
 // Note that this library does not pre-declare drivers for serial

 // ports; the user must explicitly create drivers for the ports they

 // wish to use.  This is less friendly than the stock Arduino driver,

-// but it saves 24 bytes of RAM for every unused port and frees up

+// but it saves a few bytes of RAM for every unused port and frees up

 // the vector for another driver (e.g. MSPIM on USARTs).

 //

 

@@ -55,98 +55,186 @@
 

 #include "BetterStream.h"

 

-//

-// Because Arduino libraries aren't really libraries, but we want to

-// only define interrupt handlers for serial ports that are actually

-// used, we have to force our users to define them using a macro.

-//

-// Due to the way interrupt vectors are specified, we have to have

-// a separate macro for every port.  Ugh.

-//

-// The macros are:

-//

-// FastSerialPort0(<port name>)         creates <port name> referencing serial port 0

-// FastSerialPort1(<port name>)         creates <port name> referencing serial port 1

-// FastSerialPort2(<port name>)         creates <port name> referencing serial port 2

-// FastSerialPort3(<port name>)         creates <port name> referencing serial port 3

-//

-// Note that macros are only defined for ports that exist on the target device.

-//

-

-//

-// Forward declarations for clients that want to assume that the

-// default Serial* objects exist.

-//

-// Note that the application is responsible for ensuring that these

-// actually get defined, otherwise Arduino will suck in the

-// HardwareSerial library and linking will fail.

-//

+

+/// @file	FastSerial.h

+/// @brief	An enhanced version of the Arduino HardwareSerial class

+///			implementing interrupt-driven transmission and flexible

+///			buffer management.

+///

+/// Because Arduino libraries aren't really libraries, but we want to

+/// only define interrupt handlers for serial ports that are actually

+/// used, we have to force our users to define them using a macro.

+///

+/// FastSerialPort(<port name>, <port number>)

+///

+/// <port name> is the name of the object that will be created by the

+/// macro.  <port number> is the 0-based number of the port that will

+/// be managed by the object.

+///

+/// Previously ports were defined with a different macro for each port,

+/// and these macros are retained for compatibility:

+///

+/// FastSerialPort0(<port name>)         creates <port name> referencing serial port 0

+/// FastSerialPort1(<port name>)         creates <port name> referencing serial port 1

+/// FastSerialPort2(<port name>)         creates <port name> referencing serial port 2

+/// FastSerialPort3(<port name>)         creates <port name> referencing serial port 3

+///

+/// Note that compatibility macros are only defined for ports that

+/// exist on the target device.

+///

+

+///	@name	Compatibility

+///

+/// Forward declarations for clients that want to assume that the

+/// default Serial* objects exist.

+///

+/// Note that the application is responsible for ensuring that these

+/// actually get defined, otherwise Arduino will suck in the

+/// HardwareSerial library and linking will fail.

+//@{

 extern class FastSerial Serial;

 extern class FastSerial Serial1;

 extern class FastSerial Serial2;

 extern class FastSerial Serial3;

-

-class FastSerial : public BetterStream {

+//@}

+

+/// The FastSerial class definition

+///

+class FastSerial: public BetterStream {

 public:

-        FastSerial(const uint8_t portNumber,

-                   volatile uint8_t *ubrrh,

-                   volatile uint8_t *ubrrl,

-                   volatile uint8_t *ucsra,

-                   volatile uint8_t *ucsrb,

-                   const uint8_t u2x,

-                   const uint8_t portEnableBits,

-                   const uint8_t portTxBits);

-

-        // Serial API

-        virtual void    begin(long baud);

-        virtual void    begin(long baud, unsigned int rxSpace, unsigned int txSpace);

-        virtual void    end(void);

-        virtual int     available(void);

-        virtual int     txspace(void);

-        virtual int     read(void);

-        virtual int     peek(void);

-        virtual void    flush(void);

-        virtual size_t    write(uint8_t c);

-        using BetterStream::write;

-

-        // public so the interrupt handlers can see it

-        struct Buffer {

-                volatile uint16_t head, tail;

-                uint16_t        mask;

-                uint8_t         *bytes;

-        };

+

+	/// Constructor

+	FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, volatile uint8_t *ucsra,

+			   volatile uint8_t *ucsrb, const uint8_t u2x, const uint8_t portEnableBits, const uint8_t portTxBits);

+

+	/// @name 	Serial API

+	//@{

+	virtual void begin(long baud);

+	virtual void end(void);

+	virtual int available(void);

+	virtual int txspace(void);

+	virtual int read(void);

+	virtual int peek(void);

+	virtual void flush(void);

+#if defined(ARDUINO) && ARDUINO >= 100

+	virtual size_t write(uint8_t c);

+#else

+	virtual void write(uint8_t c);

+#endif

+	using BetterStream::write;

+	//@}

+

+	/// Extended port open method

+	///

+	/// Allows for both opening with specified buffer sizes, and re-opening

+	/// to adjust a subset of the port's settings.

+	///

+	/// @note	Buffer sizes greater than ::_max_buffer_size will be rounded

+	///			down.

+	///

+	/// @param	baud		Selects the speed that the port will be

+	///						configured to.  If zero, the port speed is left

+	///						unchanged.

+	/// @param rxSpace		Sets the receive buffer size for the port.  If zero

+	///						then the buffer size is left unchanged if the port

+	///						is open, or set to ::_default_rx_buffer_size if it is

+	///						currently closed.

+	/// @param txSpace		Sets the transmit buffer size for the port.  If zero

+	///						then the buffer size is left unchanged if the port

+	///						is open, or set to ::_default_tx_buffer_size if it

+	///						is currently closed.

+	///

+	virtual void begin(long baud, unsigned int rxSpace, unsigned int txSpace);

+

+	/// Transmit/receive buffer descriptor.

+	///

+	/// Public so the interrupt handlers can see it

+	struct Buffer {

+		volatile uint16_t head, tail;	///< head and tail pointers

+		uint16_t mask;					///< buffer size mask for pointer wrap

+		uint8_t *bytes;					///< pointer to allocated buffer

+	};

+

+	/// Tell if the serial port has been initialized

+	static bool getInitialized(uint8_t port) {

+		return (1<<port) & _serialInitialized;

+	}

+

+	// ask for writes to be blocking or non-blocking

+	void set_blocking_writes(bool blocking) {

+		_nonblocking_writes = !blocking;

+	}

 

 private:

-        // register accessors

-        volatile uint8_t *_ubrrh;

-        volatile uint8_t *_ubrrl;

-        volatile uint8_t *_ucsra;

-        volatile uint8_t *_ucsrb;

-

-        // register magic numbers

-        uint8_t         _portEnableBits;        // rx, tx and rx interrupt enables

-        uint8_t         _portTxBits;            // tx data and completion interrupt enables

-        uint8_t         _u2x;

-

-        // ring buffers

-        Buffer          *_rxBuffer;

-        Buffer          *_txBuffer;

-        bool            _open;

-

-        static bool     _allocBuffer(Buffer *buffer, unsigned int size);

-        static void     _freeBuffer(Buffer *buffer);

+

+	/// Bit mask for initialized ports

+	static uint8_t _serialInitialized;

+

+	/// Set if the serial port has been initialized

+	static void setInitialized(uint8_t port) {

+		_serialInitialized |= (1<<port);

+	}

+

+	// register accessors

+	volatile uint8_t * const _ubrrh;

+	volatile uint8_t * const _ubrrl;

+	volatile uint8_t * const _ucsra;

+	volatile uint8_t * const _ucsrb;

+

+	// register magic numbers

+	const uint8_t	_u2x;

+	const uint8_t	_portEnableBits;		///< rx, tx and rx interrupt enables

+	const uint8_t	_portTxBits;			///< tx data and completion interrupt enables

+

+

+	// ring buffers

+	Buffer			* const _rxBuffer;

+	Buffer			* const _txBuffer;

+	bool 			_open;

+

+	// whether writes to the port should block waiting

+	// for enough space to appear

+	bool			_nonblocking_writes;

+

+	/// Allocates a buffer of the given size

+	///

+	/// @param	buffer		The buffer descriptor for which the buffer will

+	///						will be allocated.

+	/// @param	size		The desired buffer size.

+	/// @returns			True if the buffer was allocated successfully.

+	///

+	static bool _allocBuffer(Buffer *buffer, unsigned int size);

+

+	/// Frees the allocated buffer in a descriptor

+	///

+	/// @param	buffer		The descriptor whose buffer should be freed.

+	///

+	static void _freeBuffer(Buffer *buffer);

+

+	/// default receive buffer size

+	static const unsigned int	_default_rx_buffer_size = 128;

+

+	/// default transmit buffer size

+	static const unsigned int	_default_tx_buffer_size = 16;

+

+	/// maxium tx/rx buffer size

+	/// @note if we could bring the max size down to 256, the mask and head/tail

+	///       pointers in the buffer could become uint8_t.

+	///

+	static const unsigned int	_max_buffer_size = 512;

 };

 

 // Used by the per-port interrupt vectors

-extern FastSerial::Buffer	__FastSerial__rxBuffer[];

-extern FastSerial::Buffer	__FastSerial__txBuffer[];

-

-// Generic Rx/Tx vectors for a serial port - needs to know magic numbers

+extern FastSerial::Buffer __FastSerial__rxBuffer[];

+extern FastSerial::Buffer __FastSerial__txBuffer[];

+

+/// Generic Rx/Tx vectors for a serial port - needs to know magic numbers

+///

 #define FastSerialHandler(_PORT, _RXVECTOR, _TXVECTOR, _UDR, _UCSRB, _TXBITS) \

 ISR(_RXVECTOR, ISR_BLOCK)                                               \

 {                                                                       \

         uint8_t c;                                                      \

-        int16_t i;                                                      \

+        uint16_t i;                                                      \

                                                                         \

         /* read the byte as quickly as possible */                      \

         c = _UDR;                                                       \

@@ -211,9 +299,9 @@
 # endif

 #endif

 

-//

-// Macro defining a FastSerial port instance.

-//

+///

+/// Macro defining a FastSerial port instance.

+///

 #define FastSerialPort(_name, _num)                                     \

 	FastSerial _name(_num,                                          \

                          &UBRR##_num##H,                                \

@@ -230,14 +318,14 @@
                           UCSR##_num##B,                                \

                           _BV(UDRIE##_num))

 

-//

-// Compatibility macros for previous FastSerial versions.

-//

-// Note that these are not conditionally defined, as the errors

-// generated when using these macros for a board that does not support

-// the port are better than the errors generated for a macro that's not

-// defined at all.

-//

+///

+/// Compatibility macros for previous FastSerial versions.

+///

+/// Note that these are not conditionally defined, as the errors

+/// generated when using these macros for a board that does not support

+/// the port are better than the errors generated for a macro that's not

+/// defined at all.

+///

 #define FastSerialPort0(_portName)     FastSerialPort(_portName, 0)

 #define FastSerialPort1(_portName)     FastSerialPort(_portName, 1)

 #define FastSerialPort2(_portName)     FastSerialPort(_portName, 2)


--- a/libraries/FastSerial/examples/FastSerial/FastSerial.pde
+++ b/libraries/FastSerial/examples/FastSerial/FastSerial.pde
@@ -17,8 +17,9 @@
 #undef PROGMEM 

 #define PROGMEM __attribute__(( section(".progmem.data") )) 

 

-#undef PSTR 

-#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 

+# undef PSTR

+# define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); \

+                (prog_char_t *)&__c[0];}))

 

 //

 // Create a FastSerial driver that looks just like the stock Arduino

@@ -38,7 +39,7 @@
         //

         // Set the speed for our replacement serial port.

         //

-	Serial.begin(38400);

+	Serial.begin(115200);

 

         //

         // Test printing things


--- a/libraries/FastSerial/ftoa_engine.S
+++ b/libraries/FastSerial/ftoa_engine.S
@@ -1,532 +1,532 @@
-/* Copyright (c) 2005, Dmitry Xmelkov
-   All rights reserved.
-
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in
-     the documentation and/or other materials provided with the
-     distribution.
-   * Neither the name of the copyright holders nor the names of
-     contributors may be used to endorse or promote products derived
-     from this software without specific prior written permission.
-
-  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-  POSSIBILITY OF SUCH DAMAGE. */
-
-/* $Id: ftoa_engine.S,v 1.3 2009/04/01 23:11:00 arcanum Exp $ */
-
-#ifndef	__DOXYGEN__
-
-#include "macros.inc"
-#include "ftoa_engine.h"
-
-#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
-#  define AVR_ENH_LPM	1
-#else
-#  define AVR_ENH_LPM	0
-#endif
-
-/*
-   int __ftoa_engine (double val, char *buf,
-                      unsigned char prec, unsigned char maxdgs)
- Input:
-    val    - value to convert
-    buf    - output buffer address
-    prec   - precision: number of decimal digits is 'prec + 1'
-    maxdgs - (0 if unused) precision restriction for "%f" specification
-
- Output:
-    return     - decimal exponent of first digit
-    buf[0]     - flags (FTOA_***)
-    buf[1],... - decimal digits
-    Number of digits:
-	maxdgs == 0 ? prec+1 :
-	(buf[0] & FTOA_CARRY) == 0 || buf[1] != '1' ?
-	    aver(1, maxdgs+exp, prec+1) :
-	    aver(1, masdgs+exp-1, prec+1)
-
- Notes:
-    * Output string is not 0-terminated. For possibility of user's buffer
-    usage in any case.
-    * If used, 'maxdgs' is a number of digits for value with zero exponent.
-*/
-
-    /* Input */
-#define maxdgs	r16
-#define	prec	r18
-#define	buf_lo	r20
-#define	buf_hi	r21
-#define	val_lo	r22
-#define	val_hi	r23
-#define	val_hlo	r24
-#define	val_hhi	r25
-
-    /* Float value parse	*/
-#define	flag	r19
-
-    /* Multiplication of mantisses	*/
-#define	exp_sv	r17
-#define	mlt_1	r19	/* lowest result byte	*/
-#define mlt_2	r14
-#define	mlt_3	r15
-#define	mlt_4	r20
-#define	mlt_5	r21
-#define	mlt_6	r28
-#define	mlt_7	r29
-
-    /* Conversion to string	*/
-#define	pwr_2	r1	/* lowest byte of 'powr10' element	*/
-#define	pwr_3	r17
-#define	pwr_4	r19
-#define	pwr_5	r22
-#define	pwr_6	r25
-#define	pwr_7	r0
-#define	digit	r23
-#define	exp10	r24
-
-    /* Fixed */
-#define	zero	r1
-
-/*    ASSEMBLY_CLIB_SECTION */
-	
-    .global	__ftoa_engine
-    .type	__ftoa_engine, "function"
-__ftoa_engine:
-
-/* --------------------------------------------------------------------
-   Float value parse.
-*/
-  ; limit 'prec'
-	cpi	prec, 8
-	brlo	1f
-	ldi	prec, 7
-1:
-  ; init.
-	clr	flag
-	X_movw	XL, buf_lo
-  ; val_hhi := exponent, sign test and remove
-#if  FTOA_MINUS != 1
-#  error  FTOA_MINUS must be 1:  add with carry used
-#endif
-	lsl	val_hhi
-	adc	flag, zero		; FTOA_MINUS
-	sbrc	val_hlo, 7
-	ori	val_hhi, 1
-  ; zero test
-	adiw	val_hlo, 0
-	cpc	val_lo, zero
-	cpc	val_hi, zero
-	brne	3f
-  ; return 0
-	ori	flag, FTOA_ZERO
-	subi	prec, -2
-2:	st	X+, flag
-	ldi	flag, '0'
-	dec	prec
-	brne	2b
-	ret				; r24,r25 == 0
-3:
-  ; infinity, NaN ?
-#if  FTOA_NAN != 2 * FTOA_INF
-#  error  Must: FTOA_NAN == 2*FTOA_INF: 'rjmp' is absent
-#endif
-	cpi	val_hhi, 0xff
-	brlo	6f
-	cpi	val_hlo, 0x80
-	cpc	val_hi, zero
-	cpc	val_lo, zero
-	breq	5f
-	subi	flag, -FTOA_INF		; FTOA_NAN
-5:	subi	flag, -FTOA_INF
-6:
-  ; write flags byte
-	st	X+, flag
-  ; hidden bit
-	cpi	val_hhi, 1
-	brlo	7f			; if subnormal value
-	ori	val_hlo, 0x80
-7:	adc	val_hhi, zero
-  ; pushes
-	push	r29
-	push	r28
-	push	r17
-	push	r16
-	push	r15
-	push	r14
-
-/* --------------------------------------------------------------------
-   Multiplication of mantisses (val and table).
-   At the begin:
-	val_hlo .. val_lo  - input value mantisse
-	val_hhi            - input value exponent
-	X                  - second byte address (string begin)
-   At the end:
-	mlt_7 .. mlt_2     - multiplication result
-	exp10              - decimal exponent
-*/
-
-  ; save
-	mov	exp_sv, val_hhi
-  ; Z := & base10[exp / 8]	(sizeof(base10[0]) == 5)
-	andi	val_hhi, ~7
-	lsr	val_hhi			; (exp/8) * 4
-	mov	ZL, val_hhi
-	lsr	val_hhi
-	lsr	val_hhi			; exp/8
-	add	ZL, val_hhi		; (exp/8) * 5
-	clr	ZH
-	subi	ZL, lo8(-(.L_base10))
-	sbci	ZH, hi8(-(.L_base10))
-  ; highest mantissa byte  (mult. shifting prepare)
-	clr	val_hhi
-  ; result initializ.
-	clr	mlt_1
-	clr	mlt_2
-	clr	mlt_3
-	X_movw	mlt_4, mlt_2
-	X_movw	mlt_6, mlt_2
-
-  ; multiply to 1-st table byte
-#if  AVR_ENH_LPM
-	lpm	r0, Z+
-#else
-	lpm
-	adiw	ZL, 1
-#endif
-	sec			; for loop end control
-	ror	r0
-  ; addition
-10:	brcc	11f
-	add	mlt_1, val_lo
-	adc	mlt_2, val_hi
-	adc	mlt_3, val_hlo
-	adc	mlt_4, val_hhi
-	adc	mlt_5, zero
-  ; arg shift
-11:	lsl	val_lo
-	rol	val_hi
-	rol	val_hlo
-	rol	val_hhi
-  ; next bit
-	lsr	r0
-	brne	10b
-
-  ; second table byte
-#if  AVR_ENH_LPM
-	lpm	r0, Z+		; C flag is stay 1
-#else
-	lpm
-	adiw	ZL, 1
-	sec
-#endif
-	ror	r0
-  ; addition
-12:	brcc	13f
-	add	mlt_2, val_hi		; val_hi is the least byte now
-	adc	mlt_3, val_hlo
-	adc	mlt_4, val_hhi
-	adc	mlt_5, val_lo
-	adc	mlt_6, zero
-  ; arg shift
-13:	lsl	val_hi
-	rol	val_hlo
-	rol	val_hhi
-	rol	val_lo
-  ; next bit
-	lsr	r0
-	brne	12b
-
-  ; 3-t table byte
-#if  AVR_ENH_LPM
-	lpm	r0, Z+		; C flag is stay 1
-#else
-	lpm
-	adiw	ZL, 1
-	sec
-#endif
-	ror	r0
-  ; addition
-14:	brcc	15f
-	add	mlt_3, val_hlo		; val_hlo is the least byte now
-	adc	mlt_4, val_hhi
-	adc	mlt_5, val_lo
-	adc	mlt_6, val_hi
-	adc	mlt_7, zero
-  ; arg shift
-15:	lsl	val_hlo
-	rol	val_hhi
-	rol	val_lo
-	rol	val_hi
-  ; next bit
-	lsr	r0
-	brne	14b
-
-  ; 4-t table byte
-#if  AVR_ENH_LPM
-	lpm	r0, Z+		; C flag is stay 1
-#else
-	lpm
-#endif
-	ror	r0
-  ; addition
-16:	brcc	17f
-	add	mlt_4, val_hhi		; val_hhi is the least byte now
-	adc	mlt_5, val_lo
-	adc	mlt_6, val_hi
-	adc	mlt_7, val_hlo
-  ; arg shift
-17:	lsl	val_hhi
-	rol	val_lo
-	rol	val_hi
-	rol	val_hlo
-  ; next bit
-	lsr	r0
-	brne	16b
-
-  ; decimal exponent
-#if  AVR_ENH_LPM
-	lpm	exp10, Z
-#else
-	adiw	ZL, 1
-	lpm
-	mov	exp10, r0
-#endif
-
-  ; result shift:  mlt_7..2 >>= (~exp & 7)
-	com	exp_sv
-	andi	exp_sv, 7
-	breq	19f
-18:	lsr	mlt_7
-	ror	mlt_6
-	ror	mlt_5
-	ror	mlt_4
-	ror	mlt_3
-	ror	mlt_2
-	dec	exp_sv
-	brne	18b
-19:
-
-/* --------------------------------------------------------------------
-   Conversion to string.
-
-   Registers usage:
-      mlt_7 .. mlt_2	- new mantissa (multiplication result)
-      pwr_7 .. pwr_2	- 'powr10' table element
-      Z			- 'powr10' table pointer
-      X			- output string pointer
-      maxdgs		- number of digits
-      prec		- number of digits stays to output
-      exp10		- decimal exponent
-      digit		- conversion process
-
-   At the end:
-      X			- end of buffer (nonfilled byte)
-      exp10		- corrected dec. exponent
-      mlt_7 .. mlt_2	- remainder
-      pwr_7 .. pwr_2	- last powr10[] element
-
-   Notes:
-     * It is possible to leave out powr10'x table with subnormal value.
-      Result: accuracy degrease on the rounding phase.  No matter: high
-      precision with subnormals is not needed. (Now 0x00000001 is converted
-      exactly on prec = 5, i.e. 6 digits.)
-*/
-
-  ; to find first digit
-	ldi	ZL, lo8(.L_powr10)
-	ldi	ZH, hi8(.L_powr10)
-	set
-  ; 'pwr10' element reading
-.L_digit:
-	X_lpm	pwr_2, Z+
-	X_lpm	pwr_3, Z+
-	X_lpm	pwr_4, Z+
-	X_lpm	pwr_5, Z+
-	X_lpm	pwr_6, Z+
-	X_lpm	pwr_7, Z+
-  ; 'digit' init.
-	ldi	digit, '0' - 1
-  ; subtraction loop
-20:	inc	digit
-	sub	mlt_2, pwr_2
-	sbc	mlt_3, pwr_3
-	sbc	mlt_4, pwr_4
-	sbc	mlt_5, pwr_5
-	sbc	mlt_6, pwr_6
-	sbc	mlt_7, pwr_7
-	brsh	20b
-  ; restore mult
-	add	mlt_2, pwr_2
-	adc	mlt_3, pwr_3
-	adc	mlt_4, pwr_4
-	adc	mlt_5, pwr_5
-	adc	mlt_6, pwr_6
-	adc	mlt_7, pwr_7
-  ; analisys
-	brtc	25f
-	cpi	digit, '0'
-	brne	21f		; this is the first digit finded
-	dec	exp10
-	rjmp	.L_digit
-  ; now is the first digit
-21:	clt
-  ; number of digits
-	subi	maxdgs, 1
-	brlo	23f			; maxdgs was 0
-	add	maxdgs, exp10
-	brpl	22f
-	clr	maxdgs
-22:	cp	maxdgs, prec
-	brsh	23f
-	mov	prec, maxdgs
-23:	inc	prec
-	mov	maxdgs, prec	
-  ; operate digit
-25:	cpi	digit, '0' + 10
-	brlo	27f
-  ; overflow, digit > '9'
-	ldi	digit, '9'
-26:	st	X+, digit
-	dec	prec
-	brne	26b
-	rjmp	.L_up
-  ; write digit
-27:	st	X+, digit
-	dec	prec
-	brne	.L_digit
-
-/* --------------------------------------------------------------------
-    Rounding.
-*/
-.L_round:
-  ; pwr10 /= 2
-	lsr	pwr_7
-	ror	pwr_6
-	ror	pwr_5
-	ror	pwr_4
-	ror	pwr_3
-	ror	pwr_2
-  ; mult -= pwr10  (half of last 'pwr10' value)
-	sub	mlt_2, pwr_2
-	sbc	mlt_3, pwr_3
-	sbc	mlt_4, pwr_4
-	sbc	mlt_5, pwr_5
-	sbc	mlt_6, pwr_6
-	sbc	mlt_7, pwr_7
-  ; rounding direction?
-	brlo	.L_rest
-  ; round to up
-.L_up:
-	inc	prec
-	ld	digit, -X
-	inc	digit
-	cpi	digit, '9' + 1
-	brlo	31f
-	ldi	digit, '0'
-31:	st	X, digit
-	cpse	prec, maxdgs
-	brsh	.L_up
-  ; it was a carry to master digit
-	ld	digit, -X		; flags
-	ori	digit, FTOA_CARRY	; 'C' is not changed
-	st	X+, digit
-	brlo	.L_rest			; above comparison
-  ; overflow
-	inc	exp10
-	ldi	digit, '1'
-32:	st	X+, digit
-	ldi	digit, '0'
-	dec	prec
-	brne	32b
-  ; restore
-.L_rest:
-	clr	zero
-	pop	r14
-	pop	r15
-	pop	r16
-	pop	r17
-	pop	r28
-	pop	r29
-  ; return
-	clr	r25
-	sbrc	exp10, 7		; high byte
-	com	r25
-	ret
-
-    .size  __ftoa_engine, . - __ftoa_engine
-
-/* --------------------------------------------------------------------
-    Tables.  '.L_powr10' is placed first -- for subnormals stability.
-*/
-    .section .progmem.data,"a",@progbits
-
-    .type .L_powr10, "object"
-.L_powr10:
-	.byte	0, 64, 122, 16, 243, 90	; 100000000000000
-	.byte	0, 160, 114, 78, 24, 9	; 10000000000000
-	.byte	0, 16, 165, 212, 232, 0	; 1000000000000
-	.byte	0, 232, 118, 72, 23, 0	; 100000000000
-	.byte	0, 228, 11, 84, 2, 0	; 10000000000
-	.byte	0, 202, 154, 59, 0, 0	; 1000000000
-	.byte	0, 225, 245, 5, 0, 0	; 100000000
-	.byte	128, 150, 152, 0, 0, 0	; 10000000
-	.byte	64, 66, 15, 0, 0, 0	; 1000000
-	.byte	160, 134, 1, 0, 0, 0	; 100000
-	.byte	16, 39, 0, 0, 0, 0	; 10000
-	.byte	232, 3, 0, 0, 0, 0	; 1000
-	.byte	100, 0, 0, 0, 0, 0	; 100
-	.byte	10, 0, 0, 0, 0, 0	; 10
-	.byte	1, 0, 0, 0, 0, 0	; 1
-    .size .L_powr10, . - .L_powr10
-
-    .type	.L_base10, "object"
-.L_base10:
-	.byte	44, 118, 216, 136, -36	; 2295887404
-	.byte	103, 79, 8, 35, -33	; 587747175
-	.byte	193, 223, 174, 89, -31	; 1504632769
-	.byte	177, 183, 150, 229, -29	; 3851859889
-	.byte	228, 83, 198, 58, -26	; 986076132
-	.byte	81, 153, 118, 150, -24	; 2524354897
-	.byte	230, 194, 132, 38, -21	; 646234854
-	.byte	137, 140, 155, 98, -19	; 1654361225
-	.byte	64, 124, 111, 252, -17	; 4235164736
-	.byte	188, 156, 159, 64, -14	; 1084202172
-	.byte	186, 165, 111, 165, -12	; 2775557562
-	.byte	144, 5, 90, 42, -9	; 710542736
-	.byte	92, 147, 107, 108, -7	; 1818989404
-	.byte	103, 109, 193, 27, -4	; 465661287
-	.byte	224, 228, 13, 71, -2	; 1192092896
-	.byte	245, 32, 230, 181, 0	; 3051757813
-	.byte	208, 237, 144, 46, 3	; 781250000
-	.byte	0, 148, 53, 119, 5	; 2000000000
-	.byte	0, 128, 132, 30, 8	; 512000000
-	.byte	0, 0, 32, 78, 10	; 1310720000
-	.byte	0, 0, 0, 200, 12	; 3355443200
-	.byte	51, 51, 51, 51, 15	; 858993459
-	.byte	152, 110, 18, 131, 17	; 2199023256
-	.byte	65, 239, 141, 33, 20	; 562949953
-	.byte	137, 59, 230, 85, 22	; 1441151881
-	.byte	207, 254, 230, 219, 24	; 3689348815
-	.byte	209, 132, 75, 56, 27	; 944473297
-	.byte	247, 124, 29, 144, 29	; 2417851639
-	.byte	164, 187, 228, 36, 32	; 618970020
-	.byte	50, 132, 114, 94, 34	; 1584563250
-	.byte	129, 0, 201, 241, 36	; 4056481921
-	.byte	236, 161, 229, 61, 39	; 1038459372
-    .size .L_base10, . - .L_base10
-
-	.end
+/* Copyright (c) 2005, Dmitry Xmelkov

+   All rights reserved.

+

+   Redistribution and use in source and binary forms, with or without

+   modification, are permitted provided that the following conditions are met:

+

+   * Redistributions of source code must retain the above copyright

+     notice, this list of conditions and the following disclaimer.

+   * Redistributions in binary form must reproduce the above copyright

+     notice, this list of conditions and the following disclaimer in

+     the documentation and/or other materials provided with the

+     distribution.

+   * Neither the name of the copyright holders nor the names of

+     contributors may be used to endorse or promote products derived

+     from this software without specific prior written permission.

+

+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+  POSSIBILITY OF SUCH DAMAGE. */

+

+/* $Id: ftoa_engine.S,v 1.3 2009/04/01 23:11:00 arcanum Exp $ */

+

+#ifndef	__DOXYGEN__

+

+#include "macros.inc"

+#include "ftoa_engine.h"

+

+#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__

+#  define AVR_ENH_LPM	1

+#else

+#  define AVR_ENH_LPM	0

+#endif

+

+/*

+   int __ftoa_engine (double val, char *buf,

+                      unsigned char prec, unsigned char maxdgs)

+ Input:

+    val    - value to convert

+    buf    - output buffer address

+    prec   - precision: number of decimal digits is 'prec + 1'

+    maxdgs - (0 if unused) precision restriction for "%f" specification

+

+ Output:

+    return     - decimal exponent of first digit

+    buf[0]     - flags (FTOA_***)

+    buf[1],... - decimal digits

+    Number of digits:

+	maxdgs == 0 ? prec+1 :

+	(buf[0] & FTOA_CARRY) == 0 || buf[1] != '1' ?

+	    aver(1, maxdgs+exp, prec+1) :

+	    aver(1, masdgs+exp-1, prec+1)

+

+ Notes:

+    * Output string is not 0-terminated. For possibility of user's buffer

+    usage in any case.

+    * If used, 'maxdgs' is a number of digits for value with zero exponent.

+*/

+

+    /* Input */

+#define maxdgs	r16

+#define	prec	r18

+#define	buf_lo	r20

+#define	buf_hi	r21

+#define	val_lo	r22

+#define	val_hi	r23

+#define	val_hlo	r24

+#define	val_hhi	r25

+

+    /* Float value parse	*/

+#define	flag	r19

+

+    /* Multiplication of mantisses	*/

+#define	exp_sv	r17

+#define	mlt_1	r19	/* lowest result byte	*/

+#define mlt_2	r14

+#define	mlt_3	r15

+#define	mlt_4	r20

+#define	mlt_5	r21

+#define	mlt_6	r28

+#define	mlt_7	r29

+

+    /* Conversion to string	*/

+#define	pwr_2	r1	/* lowest byte of 'powr10' element	*/

+#define	pwr_3	r17

+#define	pwr_4	r19

+#define	pwr_5	r22

+#define	pwr_6	r25

+#define	pwr_7	r0

+#define	digit	r23

+#define	exp10	r24

+

+    /* Fixed */

+#define	zero	r1

+

+/*    ASSEMBLY_CLIB_SECTION */

+	

+    .global	__ftoa_engine

+    .type	__ftoa_engine, "function"

+__ftoa_engine:

+

+/* --------------------------------------------------------------------

+   Float value parse.

+*/

+  ; limit 'prec'

+	cpi	prec, 8

+	brlo	1f

+	ldi	prec, 7

+1:

+  ; init.

+	clr	flag

+	X_movw	XL, buf_lo

+  ; val_hhi := exponent, sign test and remove

+#if  FTOA_MINUS != 1

+#  error  FTOA_MINUS must be 1:  add with carry used

+#endif

+	lsl	val_hhi

+	adc	flag, zero		; FTOA_MINUS

+	sbrc	val_hlo, 7

+	ori	val_hhi, 1

+  ; zero test

+	adiw	val_hlo, 0

+	cpc	val_lo, zero

+	cpc	val_hi, zero

+	brne	3f

+  ; return 0

+	ori	flag, FTOA_ZERO

+	subi	prec, -2

+2:	st	X+, flag

+	ldi	flag, '0'

+	dec	prec

+	brne	2b

+	ret				; r24,r25 == 0

+3:

+  ; infinity, NaN ?

+#if  FTOA_NAN != 2 * FTOA_INF

+#  error  Must: FTOA_NAN == 2*FTOA_INF: 'rjmp' is absent

+#endif

+	cpi	val_hhi, 0xff

+	brlo	6f

+	cpi	val_hlo, 0x80

+	cpc	val_hi, zero

+	cpc	val_lo, zero

+	breq	5f

+	subi	flag, -FTOA_INF		; FTOA_NAN

+5:	subi	flag, -FTOA_INF

+6:

+  ; write flags byte

+	st	X+, flag

+  ; hidden bit

+	cpi	val_hhi, 1

+	brlo	7f			; if subnormal value

+	ori	val_hlo, 0x80

+7:	adc	val_hhi, zero

+  ; pushes

+	push	r29

+	push	r28

+	push	r17

+	push	r16

+	push	r15

+	push	r14

+

+/* --------------------------------------------------------------------

+   Multiplication of mantisses (val and table).

+   At the begin:

+	val_hlo .. val_lo  - input value mantisse

+	val_hhi            - input value exponent

+	X                  - second byte address (string begin)

+   At the end:

+	mlt_7 .. mlt_2     - multiplication result

+	exp10              - decimal exponent

+*/

+

+  ; save

+	mov	exp_sv, val_hhi

+  ; Z := & base10[exp / 8]	(sizeof(base10[0]) == 5)

+	andi	val_hhi, ~7

+	lsr	val_hhi			; (exp/8) * 4

+	mov	ZL, val_hhi

+	lsr	val_hhi

+	lsr	val_hhi			; exp/8

+	add	ZL, val_hhi		; (exp/8) * 5

+	clr	ZH

+	subi	ZL, lo8(-(.L_base10))

+	sbci	ZH, hi8(-(.L_base10))

+  ; highest mantissa byte  (mult. shifting prepare)

+	clr	val_hhi

+  ; result initializ.

+	clr	mlt_1

+	clr	mlt_2

+	clr	mlt_3

+	X_movw	mlt_4, mlt_2

+	X_movw	mlt_6, mlt_2

+

+  ; multiply to 1-st table byte

+#if  AVR_ENH_LPM

+	lpm	r0, Z+

+#else

+	lpm

+	adiw	ZL, 1

+#endif

+	sec			; for loop end control

+	ror	r0

+  ; addition

+10:	brcc	11f

+	add	mlt_1, val_lo

+	adc	mlt_2, val_hi

+	adc	mlt_3, val_hlo

+	adc	mlt_4, val_hhi

+	adc	mlt_5, zero

+  ; arg shift

+11:	lsl	val_lo

+	rol	val_hi

+	rol	val_hlo

+	rol	val_hhi

+  ; next bit

+	lsr	r0

+	brne	10b

+

+  ; second table byte

+#if  AVR_ENH_LPM

+	lpm	r0, Z+		; C flag is stay 1

+#else

+	lpm

+	adiw	ZL, 1

+	sec

+#endif

+	ror	r0

+  ; addition

+12:	brcc	13f

+	add	mlt_2, val_hi		; val_hi is the least byte now

+	adc	mlt_3, val_hlo

+	adc	mlt_4, val_hhi

+	adc	mlt_5, val_lo

+	adc	mlt_6, zero

+  ; arg shift

+13:	lsl	val_hi

+	rol	val_hlo

+	rol	val_hhi

+	rol	val_lo

+  ; next bit

+	lsr	r0

+	brne	12b

+

+  ; 3-t table byte

+#if  AVR_ENH_LPM

+	lpm	r0, Z+		; C flag is stay 1

+#else

+	lpm

+	adiw	ZL, 1

+	sec

+#endif

+	ror	r0

+  ; addition

+14:	brcc	15f

+	add	mlt_3, val_hlo		; val_hlo is the least byte now

+	adc	mlt_4, val_hhi

+	adc	mlt_5, val_lo

+	adc	mlt_6, val_hi

+	adc	mlt_7, zero

+  ; arg shift

+15:	lsl	val_hlo

+	rol	val_hhi

+	rol	val_lo

+	rol	val_hi

+  ; next bit

+	lsr	r0

+	brne	14b

+

+  ; 4-t table byte

+#if  AVR_ENH_LPM

+	lpm	r0, Z+		; C flag is stay 1

+#else

+	lpm

+#endif

+	ror	r0

+  ; addition

+16:	brcc	17f

+	add	mlt_4, val_hhi		; val_hhi is the least byte now

+	adc	mlt_5, val_lo

+	adc	mlt_6, val_hi

+	adc	mlt_7, val_hlo

+  ; arg shift

+17:	lsl	val_hhi

+	rol	val_lo

+	rol	val_hi

+	rol	val_hlo

+  ; next bit

+	lsr	r0

+	brne	16b

+

+  ; decimal exponent

+#if  AVR_ENH_LPM

+	lpm	exp10, Z

+#else

+	adiw	ZL, 1

+	lpm

+	mov	exp10, r0

+#endif

+

+  ; result shift:  mlt_7..2 >>= (~exp & 7)

+	com	exp_sv

+	andi	exp_sv, 7

+	breq	19f

+18:	lsr	mlt_7

+	ror	mlt_6

+	ror	mlt_5

+	ror	mlt_4

+	ror	mlt_3

+	ror	mlt_2

+	dec	exp_sv

+	brne	18b

+19:

+

+/* --------------------------------------------------------------------

+   Conversion to string.

+

+   Registers usage:

+      mlt_7 .. mlt_2	- new mantissa (multiplication result)

+      pwr_7 .. pwr_2	- 'powr10' table element

+      Z			- 'powr10' table pointer

+      X			- output string pointer

+      maxdgs		- number of digits

+      prec		- number of digits stays to output

+      exp10		- decimal exponent

+      digit		- conversion process

+

+   At the end:

+      X			- end of buffer (nonfilled byte)

+      exp10		- corrected dec. exponent

+      mlt_7 .. mlt_2	- remainder

+      pwr_7 .. pwr_2	- last powr10[] element

+

+   Notes:

+     * It is possible to leave out powr10'x table with subnormal value.

+      Result: accuracy degrease on the rounding phase.  No matter: high

+      precision with subnormals is not needed. (Now 0x00000001 is converted

+      exactly on prec = 5, i.e. 6 digits.)

+*/

+

+  ; to find first digit

+	ldi	ZL, lo8(.L_powr10)

+	ldi	ZH, hi8(.L_powr10)

+	set

+  ; 'pwr10' element reading

+.L_digit:

+	X_lpm	pwr_2, Z+

+	X_lpm	pwr_3, Z+

+	X_lpm	pwr_4, Z+

+	X_lpm	pwr_5, Z+

+	X_lpm	pwr_6, Z+

+	X_lpm	pwr_7, Z+

+  ; 'digit' init.

+	ldi	digit, '0' - 1

+  ; subtraction loop

+20:	inc	digit

+	sub	mlt_2, pwr_2

+	sbc	mlt_3, pwr_3

+	sbc	mlt_4, pwr_4

+	sbc	mlt_5, pwr_5

+	sbc	mlt_6, pwr_6

+	sbc	mlt_7, pwr_7

+	brsh	20b

+  ; restore mult

+	add	mlt_2, pwr_2

+	adc	mlt_3, pwr_3

+	adc	mlt_4, pwr_4

+	adc	mlt_5, pwr_5

+	adc	mlt_6, pwr_6

+	adc	mlt_7, pwr_7

+  ; analisys

+	brtc	25f

+	cpi	digit, '0'

+	brne	21f		; this is the first digit finded

+	dec	exp10

+	rjmp	.L_digit

+  ; now is the first digit

+21:	clt

+  ; number of digits

+	subi	maxdgs, 1

+	brlo	23f			; maxdgs was 0

+	add	maxdgs, exp10

+	brpl	22f

+	clr	maxdgs

+22:	cp	maxdgs, prec

+	brsh	23f

+	mov	prec, maxdgs

+23:	inc	prec

+	mov	maxdgs, prec	

+  ; operate digit

+25:	cpi	digit, '0' + 10

+	brlo	27f

+  ; overflow, digit > '9'

+	ldi	digit, '9'

+26:	st	X+, digit

+	dec	prec

+	brne	26b

+	rjmp	.L_up

+  ; write digit

+27:	st	X+, digit

+	dec	prec

+	brne	.L_digit

+

+/* --------------------------------------------------------------------

+    Rounding.

+*/

+.L_round:

+  ; pwr10 /= 2

+	lsr	pwr_7

+	ror	pwr_6

+	ror	pwr_5

+	ror	pwr_4

+	ror	pwr_3

+	ror	pwr_2

+  ; mult -= pwr10  (half of last 'pwr10' value)

+	sub	mlt_2, pwr_2

+	sbc	mlt_3, pwr_3

+	sbc	mlt_4, pwr_4

+	sbc	mlt_5, pwr_5

+	sbc	mlt_6, pwr_6

+	sbc	mlt_7, pwr_7

+  ; rounding direction?

+	brlo	.L_rest

+  ; round to up

+.L_up:

+	inc	prec

+	ld	digit, -X

+	inc	digit

+	cpi	digit, '9' + 1

+	brlo	31f

+	ldi	digit, '0'

+31:	st	X, digit

+	cpse	prec, maxdgs

+	brsh	.L_up

+  ; it was a carry to master digit

+	ld	digit, -X		; flags

+	ori	digit, FTOA_CARRY	; 'C' is not changed

+	st	X+, digit

+	brlo	.L_rest			; above comparison

+  ; overflow

+	inc	exp10

+	ldi	digit, '1'

+32:	st	X+, digit

+	ldi	digit, '0'

+	dec	prec

+	brne	32b

+  ; restore

+.L_rest:

+	clr	zero

+	pop	r14

+	pop	r15

+	pop	r16

+	pop	r17

+	pop	r28

+	pop	r29

+  ; return

+	clr	r25

+	sbrc	exp10, 7		; high byte

+	com	r25

+	ret

+

+    .size  __ftoa_engine, . - __ftoa_engine

+

+/* --------------------------------------------------------------------

+    Tables.  '.L_powr10' is placed first -- for subnormals stability.

+*/

+    .section .progmem.data,"a",@progbits

+

+    .type .L_powr10, "object"

+.L_powr10:

+	.byte	0, 64, 122, 16, 243, 90	; 100000000000000

+	.byte	0, 160, 114, 78, 24, 9	; 10000000000000

+	.byte	0, 16, 165, 212, 232, 0	; 1000000000000

+	.byte	0, 232, 118, 72, 23, 0	; 100000000000

+	.byte	0, 228, 11, 84, 2, 0	; 10000000000

+	.byte	0, 202, 154, 59, 0, 0	; 1000000000

+	.byte	0, 225, 245, 5, 0, 0	; 100000000

+	.byte	128, 150, 152, 0, 0, 0	; 10000000

+	.byte	64, 66, 15, 0, 0, 0	; 1000000

+	.byte	160, 134, 1, 0, 0, 0	; 100000

+	.byte	16, 39, 0, 0, 0, 0	; 10000

+	.byte	232, 3, 0, 0, 0, 0	; 1000

+	.byte	100, 0, 0, 0, 0, 0	; 100

+	.byte	10, 0, 0, 0, 0, 0	; 10

+	.byte	1, 0, 0, 0, 0, 0	; 1

+    .size .L_powr10, . - .L_powr10

+

+    .type	.L_base10, "object"

+.L_base10:

+	.byte	44, 118, 216, 136, -36	; 2295887404

+	.byte	103, 79, 8, 35, -33	; 587747175

+	.byte	193, 223, 174, 89, -31	; 1504632769

+	.byte	177, 183, 150, 229, -29	; 3851859889

+	.byte	228, 83, 198, 58, -26	; 986076132

+	.byte	81, 153, 118, 150, -24	; 2524354897

+	.byte	230, 194, 132, 38, -21	; 646234854

+	.byte	137, 140, 155, 98, -19	; 1654361225

+	.byte	64, 124, 111, 252, -17	; 4235164736

+	.byte	188, 156, 159, 64, -14	; 1084202172

+	.byte	186, 165, 111, 165, -12	; 2775557562

+	.byte	144, 5, 90, 42, -9	; 710542736

+	.byte	92, 147, 107, 108, -7	; 1818989404

+	.byte	103, 109, 193, 27, -4	; 465661287

+	.byte	224, 228, 13, 71, -2	; 1192092896

+	.byte	245, 32, 230, 181, 0	; 3051757813

+	.byte	208, 237, 144, 46, 3	; 781250000

+	.byte	0, 148, 53, 119, 5	; 2000000000

+	.byte	0, 128, 132, 30, 8	; 512000000

+	.byte	0, 0, 32, 78, 10	; 1310720000

+	.byte	0, 0, 0, 200, 12	; 3355443200

+	.byte	51, 51, 51, 51, 15	; 858993459

+	.byte	152, 110, 18, 131, 17	; 2199023256

+	.byte	65, 239, 141, 33, 20	; 562949953

+	.byte	137, 59, 230, 85, 22	; 1441151881

+	.byte	207, 254, 230, 219, 24	; 3689348815

+	.byte	209, 132, 75, 56, 27	; 944473297

+	.byte	247, 124, 29, 144, 29	; 2417851639

+	.byte	164, 187, 228, 36, 32	; 618970020

+	.byte	50, 132, 114, 94, 34	; 1584563250

+	.byte	129, 0, 201, 241, 36	; 4056481921

+	.byte	236, 161, 229, 61, 39	; 1038459372

+    .size .L_base10, . - .L_base10

+

+	.end

 #endif	/* !__DOXYGEN__ */

--- a/libraries/FastSerial/ftoa_engine.h
+++ b/libraries/FastSerial/ftoa_engine.h
@@ -1,49 +1,49 @@
-/* Copyright (c) 2005, Dmitry Xmelkov
-   All rights reserved.
+/* Copyright (c) 2005, Dmitry Xmelkov

+   All rights reserved.

+

+   Redistribution and use in source and binary forms, with or without

+   modification, are permitted provided that the following conditions are met:

+

+   * Redistributions of source code must retain the above copyright

+     notice, this list of conditions and the following disclaimer.

+   * Redistributions in binary form must reproduce the above copyright

+     notice, this list of conditions and the following disclaimer in

+     the documentation and/or other materials provided with the

+     distribution.

+   * Neither the name of the copyright holders nor the names of

+     contributors may be used to endorse or promote products derived

+     from this software without specific prior written permission.

+

+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+  POSSIBILITY OF SUCH DAMAGE. */

+

+/* $Id: ftoa_engine.h 1218 2007-02-18 13:18:41Z dmix $ */

+

+#ifndef	_FTOA_ENGINE_H

+#define	_FTOA_ENGINE_H

+

+#ifndef	__ASSEMBLER__

+

+int __ftoa_engine (double val, char *buf,

+                   unsigned char prec, unsigned char maxdgs);

+

+#endif

+

+/* '__ftoa_engine' return next flags (in buf[0]):	*/

+#define	FTOA_MINUS	1

+#define	FTOA_ZERO	2

+#define	FTOA_INF	4

+#define	FTOA_NAN	8

+#define	FTOA_CARRY	16	/* Carry was to master position.	*/

+

+#endif	/* !_FTOA_ENGINE_H */

 
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in
-     the documentation and/or other materials provided with the
-     distribution.
-   * Neither the name of the copyright holders nor the names of
-     contributors may be used to endorse or promote products derived
-     from this software without specific prior written permission.
-
-  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-  POSSIBILITY OF SUCH DAMAGE. */
-
-/* $Id: ftoa_engine.h 1218 2007-02-18 13:18:41Z dmix $ */
-
-#ifndef	_FTOA_ENGINE_H
-#define	_FTOA_ENGINE_H
-
-#ifndef	__ASSEMBLER__
-
-int __ftoa_engine (double val, char *buf,
-                   unsigned char prec, unsigned char maxdgs);
-
-#endif
-
-/* '__ftoa_engine' return next flags (in buf[0]):	*/
-#define	FTOA_MINUS	1
-#define	FTOA_ZERO	2
-#define	FTOA_INF	4
-#define	FTOA_NAN	8
-#define	FTOA_CARRY	16	/* Carry was to master position.	*/
-
-#endif	/* !_FTOA_ENGINE_H */
-

--- a/libraries/FastSerial/keywords.txt
+++ b/libraries/FastSerial/keywords.txt
@@ -1,8 +1,8 @@
-FastSerial	KEYWORD1
-begin		KEYWORD2
-end		KEYWORD2
-available	KEYWORD2
-read		KEYWORD2
-flush		KEYWORD2
-write		KEYWORD2
+FastSerial	KEYWORD1

+begin		KEYWORD2

+end		KEYWORD2

+available	KEYWORD2

+read		KEYWORD2

+flush		KEYWORD2

+write		KEYWORD2

 

--- a/libraries/FastSerial/macros.inc
+++ b/libraries/FastSerial/macros.inc
@@ -1,366 +1,366 @@
-/* Copyright (c) 2002, 2005, 2006, 2007 Marek Michalkiewicz
-   Copyright (c) 2006 Dmitry Xmelkov
-   All rights reserved.
+/* Copyright (c) 2002, 2005, 2006, 2007 Marek Michalkiewicz

+   Copyright (c) 2006 Dmitry Xmelkov

+   All rights reserved.

+

+   Redistribution and use in source and binary forms, with or without

+   modification, are permitted provided that the following conditions are met:

+

+   * Redistributions of source code must retain the above copyright

+     notice, this list of conditions and the following disclaimer.

+

+   * Redistributions in binary form must reproduce the above copyright

+     notice, this list of conditions and the following disclaimer in

+     the documentation and/or other materials provided with the

+     distribution.

+

+   * Neither the name of the copyright holders nor the names of

+     contributors may be used to endorse or promote products derived

+     from this software without specific prior written permission.

+

+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+  POSSIBILITY OF SUCH DAMAGE. */

+

+/*

+   macros.inc - macros for use in assembler sources

+

+   Contributors:

+     Created by Marek Michalkiewicz <marekm@linux.org.pl>

+ */

+

+#include <avr/io.h>

+//#include "sectionname.h"

+

+/* if not defined, assume old version with underscores */

+#ifndef __USER_LABEL_PREFIX__

+#define __USER_LABEL_PREFIX__ _

+#endif

+

+#ifndef __REGISTER_PREFIX__

+#define __REGISTER_PREFIX__

+#endif

+

+/* the assembler line separator (just in case it ever changes) */

+#define _L $

+

+#define CONCAT1(a, b) CONCAT2(a, b)

+#define CONCAT2(a, b) a ## b

+

+#define _U(x) CONCAT1(__USER_LABEL_PREFIX__, x)

+

+#define _R(x) CONCAT1(__REGISTER_PREFIX__, x)

+

+/* these should help to fix the "can't have function named r1()" bug

+   which may require adding '%' in front of register names.  */

+

+#define r0 _R(r0)

+#define r1 _R(r1)

+#define r2 _R(r2)

+#define r3 _R(r3)

+#define r4 _R(r4)

+#define r5 _R(r5)

+#define r6 _R(r6)

+#define r7 _R(r7)

+#define r8 _R(r8)

+#define r9 _R(r9)

+#define r10 _R(r10)

+#define r11 _R(r11)

+#define r12 _R(r12)

+#define r13 _R(r13)

+#define r14 _R(r14)

+#define r15 _R(r15)

+#define r16 _R(r16)

+#define r17 _R(r17)

+#define r18 _R(r18)

+#define r19 _R(r19)

+#define r20 _R(r20)

+#define r21 _R(r21)

+#define r22 _R(r22)

+#define r23 _R(r23)

+#define r24 _R(r24)

+#define r25 _R(r25)

+#define r26 _R(r26)

+#define r27 _R(r27)

+#define r28 _R(r28)

+#define r29 _R(r29)

+#define r30 _R(r30)

+#define r31 _R(r31)

+

+#ifndef __tmp_reg__

+#define __tmp_reg__ r0

+#endif

+

+#ifndef __zero_reg__

+#define __zero_reg__ r1

+#endif

+

+#if __AVR_MEGA__

+  #define XJMP jmp

+  #define XCALL call

+#else

+  #define XJMP rjmp

+  #define XCALL rcall

+#endif

+

+/* used only by fplib/strtod.S - libgcc internal function calls */

+#define PROLOGUE_SAVES(offset) XJMP (__prologue_saves__ + 2 * (offset))

+#define EPILOGUE_RESTORES(offset) XJMP (__epilogue_restores__ + 2 * (offset))

+

+#if FLASHEND > 0x10000  /* ATmega103 */

+  #define BIG_CODE 1

+#else

+  #define BIG_CODE 0

+#endif

+

+#ifndef __AVR_HAVE_MOVW__

+#  if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__

+#   define __AVR_HAVE_MOVW__ 1

+#  endif

+#endif

+

+#ifndef __AVR_HAVE_LPMX__

+# if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__

+#  define __AVR_HAVE_LPMX__ 1

+# endif

+#endif

+

+#ifndef __AVR_HAVE_MUL__

+# if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__

+#  define __AVR_HAVE_MUL__ 1

+# endif

+#endif

+

+/*

+   Smart version of movw:

+    - uses "movw" if possible (supported by MCU, and both registers even)

+    - handles overlapping register pairs correctly

+    - no instruction generated if source and destination are the same

+   (may expand to 0, 1 or 2 instructions).

+ */

+

+.macro  X_movw dst src

+	.L_movw_dst = -1

+	.L_movw_src = -1

+	.L_movw_n = 0

+	.irp  reg,	r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \

+			r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \

+			r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \

+			r30,r31

+		.ifc  \reg,\dst

+			.L_movw_dst = .L_movw_n

+		.endif

+		.ifc  \reg,\src

+			.L_movw_src = .L_movw_n

+		.endif

+		.L_movw_n = .L_movw_n + 1

+	.endr

+	.L_movw_n = 0

+	.irp  reg,	R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \

+			R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \

+			R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \

+			R30,R31

+		.ifc  \reg,\dst

+			.L_movw_dst = .L_movw_n

+		.endif

+		.ifc  \reg,\src

+			.L_movw_src = .L_movw_n

+		.endif

+		.L_movw_n = .L_movw_n + 1

+	.endr

+	.if   .L_movw_dst < 0

+		.L_movw_n = 0

+		.rept   32

+			.if \dst == .L_movw_n

+				.L_movw_dst = .L_movw_n

+			.endif

+			.L_movw_n = .L_movw_n + 1

+		.endr

+	.endif

+	.if   .L_movw_src < 0

+		.L_movw_n = 0

+		.rept   32

+			.if \src == .L_movw_n

+				.L_movw_src = .L_movw_n

+			.endif

+			.L_movw_n = .L_movw_n + 1

+		.endr

+	.endif

+	.if   (.L_movw_dst < 0) || (.L_movw_src < 0)

+		.err    ; Invalid 'X_movw' arg.

+	.endif

+                

+	.if ((.L_movw_src) - (.L_movw_dst))  /* different registers */

+		.if (((.L_movw_src) | (.L_movw_dst)) & 0x01)

+			.if (((.L_movw_src)-(.L_movw_dst)) & 0x80) /* src < dest */

+				mov     (.L_movw_dst)+1, (.L_movw_src)+1

+				mov     (.L_movw_dst), (.L_movw_src)

+			.else                                      /* src > dest */

+				mov     (.L_movw_dst), (.L_movw_src)

+				mov     (.L_movw_dst)+1, (.L_movw_src)+1

+			.endif

+		.else  /* both even -> overlap not possible */

+#if  defined(__AVR_HAVE_MOVW__) && __AVR_HAVE_MOVW__

+			movw    \dst, \src

+#else

+			mov     (.L_movw_dst), (.L_movw_src)

+			mov     (.L_movw_dst)+1, (.L_movw_src)+1

+#endif

+		.endif

+	.endif

+.endm

+

+/* Macro 'X_lpm' extends enhanced lpm instruction for classic chips.

+   Usage:

+	X_lpm	reg, dst

+   where

+	reg	is 0..31, r0..r31 or R0..R31

+	dst	is z, Z, z+ or Z+

+   It is possible to omit both arguments.

+

+   Possible results for classic chips:

+	lpm

+	lpm / mov Rd,r0

+	lpm / adiw ZL,1

+	lpm / mov Rd,r0 / adiw ZL,1

+	

+   For enhanced chips it is one instruction always.

+

+   ATTENTION:  unlike enhanced chips SREG (S,V,N,Z,C) flags are

+   changed in case of 'Z+' dst.  R0 is scratch.

+ */

+.macro	X_lpm	dst=r0, src=Z

+

+  /* dst evaluation	*/

+  .L_lpm_dst = -1

+

+  .L_lpm_n = 0

+  .irp  reg,  r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \

+	     r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \

+	     r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \

+	     r30,r31

+    .ifc  \reg,\dst

+      .L_lpm_dst = .L_lpm_n

+    .endif

+    .L_lpm_n = .L_lpm_n + 1

+  .endr

+

+  .L_lpm_n = 0

+  .irp  reg,  R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \

+	     R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \

+	     R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \

+	     R30,R31

+    .ifc  \reg,\dst

+      .L_lpm_dst = .L_lpm_n

+    .endif

+    .L_lpm_n = .L_lpm_n + 1

+  .endr

+

+  .if  .L_lpm_dst < 0

+    .L_lpm_n = 0

+    .rept 32

+      .if  \dst == .L_lpm_n

+	.L_lpm_dst = .L_lpm_n

+      .endif

+      .L_lpm_n = .L_lpm_n + 1

+    .endr

+  .endif

+

+  .if  (.L_lpm_dst < 0)

+    .err	; Invalid dst arg of 'X_lpm' macro.

+  .endif

+

+  /* src evaluation	*/    

+  .L_lpm_src = -1

+  .L_lpm_n = 0

+  .irp  reg,  z,Z,z+,Z+

+    .ifc  \reg,\src

+      .L_lpm_src = .L_lpm_n

+    .endif

+    .L_lpm_n = .L_lpm_n + 1

+  .endr

+

+  .if  (.L_lpm_src < 0)

+    .err	; Invalid src arg of 'X_lpm' macro.

+  .endif

+

+  /* instruction(s)	*/    

+  .if  .L_lpm_src < 2

+    .if  .L_lpm_dst == 0

+	lpm

+    .else

+#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__

+	lpm	.L_lpm_dst, Z

+#else

+	lpm

+	mov	.L_lpm_dst, r0

+#endif

+    .endif

+  .else

+    .if  (.L_lpm_dst >= 30)

+      .err	; Registers 30 and 31 are inhibited as 'X_lpm *,Z+' dst.

+    .endif

+#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__

+	lpm	.L_lpm_dst, Z+

+#else

+	lpm

+    .if  .L_lpm_dst

+	mov	.L_lpm_dst, r0

+    .endif

+	adiw	r30, 1

+#endif

+  .endif

+.endm

+

+/*

+   LPM_R0_ZPLUS_INIT is used before the loop to initialize RAMPZ

+   for future devices with RAMPZ:Z auto-increment - [e]lpm r0, Z+.

+

+   LPM_R0_ZPLUS_NEXT is used inside the loop to load a byte from

+   the program memory at [RAMPZ:]Z to R0, and increment [RAMPZ:]Z.

+

+   The argument in both macros is a register that contains the

+   high byte (bits 23-16) of the address, bits 15-0 should be in

+   the Z (r31:r30) register.  It can be any register except for:

+   r0, r1 (__zero_reg__ - assumed to always contain 0), r30, r31.

+ */

+

+	.macro	LPM_R0_ZPLUS_INIT hhi

+#if __AVR_ENHANCED__

+  #if BIG_CODE

+	out	AVR_RAMPZ_ADDR, \hhi

+  #endif

+#endif

+	.endm

+

+	.macro	LPM_R0_ZPLUS_NEXT hhi

+#if __AVR_ENHANCED__

+  #if BIG_CODE

+    /* ELPM with RAMPZ:Z post-increment, load RAMPZ only once */

+	elpm	r0, Z+

+  #else

+    /* LPM with Z post-increment, max 64K, no RAMPZ (ATmega83/161/163/32) */

+	lpm	r0, Z+

+  #endif

+#else

+  #if BIG_CODE

+    /* ELPM without post-increment, load RAMPZ each time (ATmega103) */

+	out	AVR_RAMPZ_ADDR, \hhi

+	elpm

+	adiw	r30,1

+	adc	\hhi, __zero_reg__

+  #else

+    /* LPM without post-increment, max 64K, no RAMPZ (AT90S*) */

+	lpm

+	adiw	r30,1

+  #endif

+#endif

+	.endm

 
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in
-     the documentation and/or other materials provided with the
-     distribution.
-
-   * Neither the name of the copyright holders nor the names of
-     contributors may be used to endorse or promote products derived
-     from this software without specific prior written permission.
-
-  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-  POSSIBILITY OF SUCH DAMAGE. */
-
-/*
-   macros.inc - macros for use in assembler sources
-
-   Contributors:
-     Created by Marek Michalkiewicz <marekm@linux.org.pl>
- */
-
-#include <avr/io.h>
-//#include "sectionname.h"
-
-/* if not defined, assume old version with underscores */
-#ifndef __USER_LABEL_PREFIX__
-#define __USER_LABEL_PREFIX__ _
-#endif
-
-#ifndef __REGISTER_PREFIX__
-#define __REGISTER_PREFIX__
-#endif
-
-/* the assembler line separator (just in case it ever changes) */
-#define _L $
-
-#define CONCAT1(a, b) CONCAT2(a, b)
-#define CONCAT2(a, b) a ## b
-
-#define _U(x) CONCAT1(__USER_LABEL_PREFIX__, x)
-
-#define _R(x) CONCAT1(__REGISTER_PREFIX__, x)
-
-/* these should help to fix the "can't have function named r1()" bug
-   which may require adding '%' in front of register names.  */
-
-#define r0 _R(r0)
-#define r1 _R(r1)
-#define r2 _R(r2)
-#define r3 _R(r3)
-#define r4 _R(r4)
-#define r5 _R(r5)
-#define r6 _R(r6)
-#define r7 _R(r7)
-#define r8 _R(r8)
-#define r9 _R(r9)
-#define r10 _R(r10)
-#define r11 _R(r11)
-#define r12 _R(r12)
-#define r13 _R(r13)
-#define r14 _R(r14)
-#define r15 _R(r15)
-#define r16 _R(r16)
-#define r17 _R(r17)
-#define r18 _R(r18)
-#define r19 _R(r19)
-#define r20 _R(r20)
-#define r21 _R(r21)
-#define r22 _R(r22)
-#define r23 _R(r23)
-#define r24 _R(r24)
-#define r25 _R(r25)
-#define r26 _R(r26)
-#define r27 _R(r27)
-#define r28 _R(r28)
-#define r29 _R(r29)
-#define r30 _R(r30)
-#define r31 _R(r31)
-
-#ifndef __tmp_reg__
-#define __tmp_reg__ r0
-#endif
-
-#ifndef __zero_reg__
-#define __zero_reg__ r1
-#endif
-
-#if __AVR_MEGA__
-  #define XJMP jmp
-  #define XCALL call
-#else
-  #define XJMP rjmp
-  #define XCALL rcall
-#endif
-
-/* used only by fplib/strtod.S - libgcc internal function calls */
-#define PROLOGUE_SAVES(offset) XJMP (__prologue_saves__ + 2 * (offset))
-#define EPILOGUE_RESTORES(offset) XJMP (__epilogue_restores__ + 2 * (offset))
-
-#if FLASHEND > 0x10000  /* ATmega103 */
-  #define BIG_CODE 1
-#else
-  #define BIG_CODE 0
-#endif
-
-#ifndef __AVR_HAVE_MOVW__
-#  if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
-#   define __AVR_HAVE_MOVW__ 1
-#  endif
-#endif
-
-#ifndef __AVR_HAVE_LPMX__
-# if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
-#  define __AVR_HAVE_LPMX__ 1
-# endif
-#endif
-
-#ifndef __AVR_HAVE_MUL__
-# if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
-#  define __AVR_HAVE_MUL__ 1
-# endif
-#endif
-
-/*
-   Smart version of movw:
-    - uses "movw" if possible (supported by MCU, and both registers even)
-    - handles overlapping register pairs correctly
-    - no instruction generated if source and destination are the same
-   (may expand to 0, 1 or 2 instructions).
- */
-
-.macro  X_movw dst src
-	.L_movw_dst = -1
-	.L_movw_src = -1
-	.L_movw_n = 0
-	.irp  reg,	r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \
-			r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \
-			r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \
-			r30,r31
-		.ifc  \reg,\dst
-			.L_movw_dst = .L_movw_n
-		.endif
-		.ifc  \reg,\src
-			.L_movw_src = .L_movw_n
-		.endif
-		.L_movw_n = .L_movw_n + 1
-	.endr
-	.L_movw_n = 0
-	.irp  reg,	R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \
-			R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \
-			R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \
-			R30,R31
-		.ifc  \reg,\dst
-			.L_movw_dst = .L_movw_n
-		.endif
-		.ifc  \reg,\src
-			.L_movw_src = .L_movw_n
-		.endif
-		.L_movw_n = .L_movw_n + 1
-	.endr
-	.if   .L_movw_dst < 0
-		.L_movw_n = 0
-		.rept   32
-			.if \dst == .L_movw_n
-				.L_movw_dst = .L_movw_n
-			.endif
-			.L_movw_n = .L_movw_n + 1
-		.endr
-	.endif
-	.if   .L_movw_src < 0
-		.L_movw_n = 0
-		.rept   32
-			.if \src == .L_movw_n
-				.L_movw_src = .L_movw_n
-			.endif
-			.L_movw_n = .L_movw_n + 1
-		.endr
-	.endif
-	.if   (.L_movw_dst < 0) || (.L_movw_src < 0)
-		.err    ; Invalid 'X_movw' arg.
-	.endif
-                
-	.if ((.L_movw_src) - (.L_movw_dst))  /* different registers */
-		.if (((.L_movw_src) | (.L_movw_dst)) & 0x01)
-			.if (((.L_movw_src)-(.L_movw_dst)) & 0x80) /* src < dest */
-				mov     (.L_movw_dst)+1, (.L_movw_src)+1
-				mov     (.L_movw_dst), (.L_movw_src)
-			.else                                      /* src > dest */
-				mov     (.L_movw_dst), (.L_movw_src)
-				mov     (.L_movw_dst)+1, (.L_movw_src)+1
-			.endif
-		.else  /* both even -> overlap not possible */
-#if  defined(__AVR_HAVE_MOVW__) && __AVR_HAVE_MOVW__
-			movw    \dst, \src
-#else
-			mov     (.L_movw_dst), (.L_movw_src)
-			mov     (.L_movw_dst)+1, (.L_movw_src)+1
-#endif
-		.endif
-	.endif
-.endm
-
-/* Macro 'X_lpm' extends enhanced lpm instruction for classic chips.
-   Usage:
-	X_lpm	reg, dst
-   where
-	reg	is 0..31, r0..r31 or R0..R31
-	dst	is z, Z, z+ or Z+
-   It is possible to omit both arguments.
-
-   Possible results for classic chips:
-	lpm
-	lpm / mov Rd,r0
-	lpm / adiw ZL,1
-	lpm / mov Rd,r0 / adiw ZL,1
-	
-   For enhanced chips it is one instruction always.
-
-   ATTENTION:  unlike enhanced chips SREG (S,V,N,Z,C) flags are
-   changed in case of 'Z+' dst.  R0 is scratch.
- */
-.macro	X_lpm	dst=r0, src=Z
-
-  /* dst evaluation	*/
-  .L_lpm_dst = -1
-
-  .L_lpm_n = 0
-  .irp  reg,  r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \
-	     r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \
-	     r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \
-	     r30,r31
-    .ifc  \reg,\dst
-      .L_lpm_dst = .L_lpm_n
-    .endif
-    .L_lpm_n = .L_lpm_n + 1
-  .endr
-
-  .L_lpm_n = 0
-  .irp  reg,  R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \
-	     R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \
-	     R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \
-	     R30,R31
-    .ifc  \reg,\dst
-      .L_lpm_dst = .L_lpm_n
-    .endif
-    .L_lpm_n = .L_lpm_n + 1
-  .endr
-
-  .if  .L_lpm_dst < 0
-    .L_lpm_n = 0
-    .rept 32
-      .if  \dst == .L_lpm_n
-	.L_lpm_dst = .L_lpm_n
-      .endif
-      .L_lpm_n = .L_lpm_n + 1
-    .endr
-  .endif
-
-  .if  (.L_lpm_dst < 0)
-    .err	; Invalid dst arg of 'X_lpm' macro.
-  .endif
-
-  /* src evaluation	*/    
-  .L_lpm_src = -1
-  .L_lpm_n = 0
-  .irp  reg,  z,Z,z+,Z+
-    .ifc  \reg,\src
-      .L_lpm_src = .L_lpm_n
-    .endif
-    .L_lpm_n = .L_lpm_n + 1
-  .endr
-
-  .if  (.L_lpm_src < 0)
-    .err	; Invalid src arg of 'X_lpm' macro.
-  .endif
-
-  /* instruction(s)	*/    
-  .if  .L_lpm_src < 2
-    .if  .L_lpm_dst == 0
-	lpm
-    .else
-#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
-	lpm	.L_lpm_dst, Z
-#else
-	lpm
-	mov	.L_lpm_dst, r0
-#endif
-    .endif
-  .else
-    .if  (.L_lpm_dst >= 30)
-      .err	; Registers 30 and 31 are inhibited as 'X_lpm *,Z+' dst.
-    .endif
-#if  defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__
-	lpm	.L_lpm_dst, Z+
-#else
-	lpm
-    .if  .L_lpm_dst
-	mov	.L_lpm_dst, r0
-    .endif
-	adiw	r30, 1
-#endif
-  .endif
-.endm
-
-/*
-   LPM_R0_ZPLUS_INIT is used before the loop to initialize RAMPZ
-   for future devices with RAMPZ:Z auto-increment - [e]lpm r0, Z+.
-
-   LPM_R0_ZPLUS_NEXT is used inside the loop to load a byte from
-   the program memory at [RAMPZ:]Z to R0, and increment [RAMPZ:]Z.
-
-   The argument in both macros is a register that contains the
-   high byte (bits 23-16) of the address, bits 15-0 should be in
-   the Z (r31:r30) register.  It can be any register except for:
-   r0, r1 (__zero_reg__ - assumed to always contain 0), r30, r31.
- */
-
-	.macro	LPM_R0_ZPLUS_INIT hhi
-#if __AVR_ENHANCED__
-  #if BIG_CODE
-	out	AVR_RAMPZ_ADDR, \hhi
-  #endif
-#endif
-	.endm
-
-	.macro	LPM_R0_ZPLUS_NEXT hhi
-#if __AVR_ENHANCED__
-  #if BIG_CODE
-    /* ELPM with RAMPZ:Z post-increment, load RAMPZ only once */
-	elpm	r0, Z+
-  #else
-    /* LPM with Z post-increment, max 64K, no RAMPZ (ATmega83/161/163/32) */
-	lpm	r0, Z+
-  #endif
-#else
-  #if BIG_CODE
-    /* ELPM without post-increment, load RAMPZ each time (ATmega103) */
-	out	AVR_RAMPZ_ADDR, \hhi
-	elpm
-	adiw	r30,1
-	adc	\hhi, __zero_reg__
-  #else
-    /* LPM without post-increment, max 64K, no RAMPZ (AT90S*) */
-	lpm
-	adiw	r30,1
-  #endif
-#endif
-	.endm
-

--- a/libraries/FastSerial/ntz.h
+++ b/libraries/FastSerial/ntz.h
@@ -1,55 +1,55 @@
-/* Copyright (c) 2007, Dmitry Xmelkov
-   All rights reserved.
+/* Copyright (c) 2007, Dmitry Xmelkov

+   All rights reserved.

+

+   Redistribution and use in source and binary forms, with or without

+   modification, are permitted provided that the following conditions are met:

+

+   * Redistributions of source code must retain the above copyright

+     notice, this list of conditions and the following disclaimer.

+   * Redistributions in binary form must reproduce the above copyright

+     notice, this list of conditions and the following disclaimer in

+     the documentation and/or other materials provided with the

+     distribution.

+   * Neither the name of the copyright holders nor the names of

+     contributors may be used to endorse or promote products derived

+     from this software without specific prior written permission.

+

+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+  POSSIBILITY OF SUCH DAMAGE. */

+

+/* $Id: ntz.h 1217 2007-02-18 13:18:05Z dmix $	*/

+

+#ifndef	_NTZ_H

+#define	_NTZ_H

+

+/* Number of Tail Zeros:  ntz(x)= (ffs(x) ? ffs(x)-1 : 16)

+   It works with all: cpp, gcc and gas expressions.	*/

+#define ntz(x)	\

+	( (1 & (((x) & 1) == 0))        \

+	+ (1 & (((x) & 3) == 0))        \

+    	+ (1 & (((x) & 7) == 0))        \

+        + (1 & (((x) & 017) == 0))      \

+        + (1 & (((x) & 037) == 0))      \

+	+ (1 & (((x) & 077) == 0))      \

+	+ (1 & (((x) & 0177) == 0))     \

+	+ (1 & (((x) & 0377) == 0))     \

+	+ (1 & (((x) & 0777) == 0))     \

+	+ (1 & (((x) & 01777) == 0))    \

+	+ (1 & (((x) & 03777) == 0))    \

+	+ (1 & (((x) & 07777) == 0))    \

+	+ (1 & (((x) & 017777) == 0))   \

+	+ (1 & (((x) & 037777) == 0))   \

+	+ (1 & (((x) & 077777) == 0))   \

+	+ (1 & (((x) & 0177777) == 0)) )

+

+#endif	/* !_NTZ_H */

 
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in
-     the documentation and/or other materials provided with the
-     distribution.
-   * Neither the name of the copyright holders nor the names of
-     contributors may be used to endorse or promote products derived
-     from this software without specific prior written permission.
-
-  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-  POSSIBILITY OF SUCH DAMAGE. */
-
-/* $Id: ntz.h 1217 2007-02-18 13:18:05Z dmix $	*/
-
-#ifndef	_NTZ_H
-#define	_NTZ_H
-
-/* Number of Tail Zeros:  ntz(x)= (ffs(x) ? ffs(x)-1 : 16)
-   It works with all: cpp, gcc and gas expressions.	*/
-#define ntz(x)	\
-	( (1 & (((x) & 1) == 0))        \
-	+ (1 & (((x) & 3) == 0))        \
-    	+ (1 & (((x) & 7) == 0))        \
-        + (1 & (((x) & 017) == 0))      \
-        + (1 & (((x) & 037) == 0))      \
-	+ (1 & (((x) & 077) == 0))      \
-	+ (1 & (((x) & 0177) == 0))     \
-	+ (1 & (((x) & 0377) == 0))     \
-	+ (1 & (((x) & 0777) == 0))     \
-	+ (1 & (((x) & 01777) == 0))    \
-	+ (1 & (((x) & 03777) == 0))    \
-	+ (1 & (((x) & 07777) == 0))    \
-	+ (1 & (((x) & 017777) == 0))   \
-	+ (1 & (((x) & 037777) == 0))   \
-	+ (1 & (((x) & 077777) == 0))   \
-	+ (1 & (((x) & 0177777) == 0)) )
-
-#endif	/* !_NTZ_H */
-

--- a/libraries/FastSerial/ultoa_invert.S
+++ b/libraries/FastSerial/ultoa_invert.S
@@ -1,217 +1,217 @@
-/* Copyright (c) 2005,2007  Dmitry Xmelkov
-   All rights reserved.
+/* Copyright (c) 2005,2007  Dmitry Xmelkov

+   All rights reserved.

+

+   Redistribution and use in source and binary forms, with or without

+   modification, are permitted provided that the following conditions are met:

+

+   * Redistributions of source code must retain the above copyright

+     notice, this list of conditions and the following disclaimer.

+   * Redistributions in binary form must reproduce the above copyright

+     notice, this list of conditions and the following disclaimer in

+     the documentation and/or other materials provided with the

+     distribution.

+   * Neither the name of the copyright holders nor the names of

+     contributors may be used to endorse or promote products derived

+     from this software without specific prior written permission.

+

+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+  POSSIBILITY OF SUCH DAMAGE. */

+

+/* $Id: ultoa_invert.S 1944 2009-04-01 23:12:20Z arcanum $	*/

+

+#ifndef	__DOXYGEN__

+

+#include "macros.inc"

+#include "ntz.h"

+#include "xtoa_fast.h"

+

+/* --------------------------------------------------------------------

+   char * __ultoa_invert (unsigned long val, char * str, int base)

+

+   This function is intended for usage as internal printf's one.

+   It differs from others of `xtoa_fast' family:

+       * srt[] will NOT 0 terminated.

+       * Sequence of digits is inverted.

+       * It returns pointer to first byte after a string.

+       * Only `XTOA_UPPER' flag is operated.

+   Notes:

+       * base: check only 8 and 16, all others are treated as 10.

+       (internal printf's function).

+*/

+

+    /* Input	*/

+#define v_lo	r22

+#define	v_hi	r23

+#define	v_hlo	r24

+#define	v_hhi	r25

+#define	str_lo	r20

+#define	str_hi	r21

+#define	base	r18

+#define	flags	r19

+

+    /* Used	*/

+#define	v_fifth	r26	/* val: bits 39..32			*/

+#define	t_lo	r18	/* temporary for shifted `val'		*/

+#define	t_hi	r19

+#define	t_hlo	r20

+#define	t_hhi	r21

+#define	symb	r20	/* write to string			*/

+#define	cnt	r27	/* shift loop counter, local arg	*/

+

+    /* Fixed	*/

+#define	rzero	r1

+

+/*	ASSEMBLY_CLIB_SECTION */

+	.global	__ultoa_invert

+	.type	__ultoa_invert, "function"

+

+__ultoa_invert:

+	X_movw	ZL, str_lo

+	clr	v_fifth			; needed for all (ultoa_lsr)

+	cpi	base, 8

+	breq	.L_oct

+	cpi	base, 16

+	breq	.L_hex

+

+  ; decimal format

+	clt				; flag of val == 0

+.L_dec_loop:

+	push	v_lo			; to calculate remander

+  ; val &= ~1

+	andi	v_lo, ~1

+  ; val += 2

+	subi	v_lo, lo8(-2)

+	sbci	v_hi, hi8(-2)

+	sbci	v_hlo, hlo8(-2)

+	sbci	v_hhi, hhi8(-2)

+	sbci	v_fifth, hhi8(-2)

+  ; val += val/2

+	ldi	cnt, 1

+	rcall	.L_div_add

+  ; val += val/16

+	ldi	cnt, 4

+	rcall	.L_div_add

+  ; val += val/256

+	add	v_lo, v_hi

+	adc	v_hi, v_hlo

+	adc	v_hlo, v_hhi

+	adc	v_hhi, v_fifth

+	adc	v_fifth, rzero

+  ; val += val/65536

+	add	v_lo, v_hlo

+	adc	v_hi, v_hhi

+	adc	v_hlo, v_fifth

+	adc	v_hhi, rzero

+	adc	v_fifth, rzero

+  ; val += val >> 32

+	add	v_lo, v_fifth

+	adc	v_hi, rzero

+	adc	v_hlo, rzero

+	adc	v_hhi, rzero

+	adc	v_fifth, rzero

+  ; division result:  val /= 16

+	rcall	.L_lsr_4		; v_fitth := 0

+	brne	1f

+	set				; T := Z flag

+1:

+  ; rem:  val_original - 10*val

+	pop	t_hi

+#if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__

+	ldi	t_lo, 10

+	mul	t_lo, v_lo

+	clr	r1

+#else

+	mov	r0, v_lo

+	lsl	r0

+	sub	t_hi, r0

+	lsl	r0

+	lsl	r0

+#endif

+	sub	t_hi, r0

+  ; output digit

+	subi	t_hi, lo8(-'0')

+	st	Z+, t_hi

+  ; quotient == 0 ?

+	brtc	.L_dec_loop

+  ; end of string

+.L_eos:

+	X_movw	r24, ZL

+	ret

+

+  ; octal format

+.L_oct:

+	mov	symb, v_lo

+	andi	symb, 7

+	subi	symb, lo8(-'0')

+	st	Z+, symb

+	ldi	cnt, 3

+	rcall	.L_lsr

+	brne	.L_oct

+	rjmp	.L_eos

+

+  ; hex format

+.L_hex:

+	mov	symb, v_lo

+	andi	symb, 0x0f

+	subi	symb, lo8(-'0')

+	cpi	symb, '9' + 1

+	brlo	3f

+	subi	symb, lo8('9' + 1 - 'a')

+	sbrc	flags, ntz(XTOA_UPPER) - 8

+	subi	symb, lo8('a' - 'A')

+3:	st	Z+, symb

+	rcall	.L_lsr_4

+	brne	.L_hex

+	rjmp	.L_eos

+	

+.L_lsr_4:

+	ldi	cnt, 4

+.L_lsr:

+	lsr	v_fifth

+	ror	v_hhi

+	ror	v_hlo

+	ror	v_hi

+	ror	v_lo

+	dec	cnt

+	brne	.L_lsr

+  ; tst

+	sbiw	v_hlo, 0		; only Z flag is needed

+	cpc	v_lo, rzero

+	cpc	v_hi, rzero

+	ret

+

+.L_div_add:

+  ; copy to temporary

+	X_movw	t_lo, v_lo

+	X_movw	t_hlo, v_hlo

+	mov	r0, v_fifth

+  ; lsr temporary

+7:	lsr	r0

+	ror	t_hhi

+	ror	t_hlo

+	ror	t_hi

+	ror	t_lo

+	dec	cnt

+	brne	7b

+  ; add

+	add	v_lo, t_lo

+	adc	v_hi, t_hi

+	adc	v_hlo, t_hlo

+	adc	v_hhi, t_hhi

+	adc	v_fifth, r0		; here r0 == 0

+	ret

+

+	.size	__ultoa_invert, . - __ultoa_invert

+	.end

+

+#endif	/* !__DOXYGEN__ */

 
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in
-     the documentation and/or other materials provided with the
-     distribution.
-   * Neither the name of the copyright holders nor the names of
-     contributors may be used to endorse or promote products derived
-     from this software without specific prior written permission.
-
-  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-  POSSIBILITY OF SUCH DAMAGE. */
-
-/* $Id: ultoa_invert.S 1944 2009-04-01 23:12:20Z arcanum $	*/
-
-#ifndef	__DOXYGEN__
-
-#include "macros.inc"
-#include "ntz.h"
-#include "xtoa_fast.h"
-
-/* --------------------------------------------------------------------
-   char * __ultoa_invert (unsigned long val, char * str, int base)
-
-   This function is intended for usage as internal printf's one.
-   It differs from others of `xtoa_fast' family:
-       * srt[] will NOT 0 terminated.
-       * Sequence of digits is inverted.
-       * It returns pointer to first byte after a string.
-       * Only `XTOA_UPPER' flag is operated.
-   Notes:
-       * base: check only 8 and 16, all others are treated as 10.
-       (internal printf's function).
-*/
-
-    /* Input	*/
-#define v_lo	r22
-#define	v_hi	r23
-#define	v_hlo	r24
-#define	v_hhi	r25
-#define	str_lo	r20
-#define	str_hi	r21
-#define	base	r18
-#define	flags	r19
-
-    /* Used	*/
-#define	v_fifth	r26	/* val: bits 39..32			*/
-#define	t_lo	r18	/* temporary for shifted `val'		*/
-#define	t_hi	r19
-#define	t_hlo	r20
-#define	t_hhi	r21
-#define	symb	r20	/* write to string			*/
-#define	cnt	r27	/* shift loop counter, local arg	*/
-
-    /* Fixed	*/
-#define	rzero	r1
-
-/*	ASSEMBLY_CLIB_SECTION */
-	.global	__ultoa_invert
-	.type	__ultoa_invert, "function"
-
-__ultoa_invert:
-	X_movw	ZL, str_lo
-	clr	v_fifth			; needed for all (ultoa_lsr)
-	cpi	base, 8
-	breq	.L_oct
-	cpi	base, 16
-	breq	.L_hex
-
-  ; decimal format
-	clt				; flag of val == 0
-.L_dec_loop:
-	push	v_lo			; to calculate remander
-  ; val &= ~1
-	andi	v_lo, ~1
-  ; val += 2
-	subi	v_lo, lo8(-2)
-	sbci	v_hi, hi8(-2)
-	sbci	v_hlo, hlo8(-2)
-	sbci	v_hhi, hhi8(-2)
-	sbci	v_fifth, hhi8(-2)
-  ; val += val/2
-	ldi	cnt, 1
-	rcall	.L_div_add
-  ; val += val/16
-	ldi	cnt, 4
-	rcall	.L_div_add
-  ; val += val/256
-	add	v_lo, v_hi
-	adc	v_hi, v_hlo
-	adc	v_hlo, v_hhi
-	adc	v_hhi, v_fifth
-	adc	v_fifth, rzero
-  ; val += val/65536
-	add	v_lo, v_hlo
-	adc	v_hi, v_hhi
-	adc	v_hlo, v_fifth
-	adc	v_hhi, rzero
-	adc	v_fifth, rzero
-  ; val += val >> 32
-	add	v_lo, v_fifth
-	adc	v_hi, rzero
-	adc	v_hlo, rzero
-	adc	v_hhi, rzero
-	adc	v_fifth, rzero
-  ; division result:  val /= 16
-	rcall	.L_lsr_4		; v_fitth := 0
-	brne	1f
-	set				; T := Z flag
-1:
-  ; rem:  val_original - 10*val
-	pop	t_hi
-#if  defined(__AVR_ENHANCED__) && __AVR_ENHANCED__
-	ldi	t_lo, 10
-	mul	t_lo, v_lo
-	clr	r1
-#else
-	mov	r0, v_lo
-	lsl	r0
-	sub	t_hi, r0
-	lsl	r0
-	lsl	r0
-#endif
-	sub	t_hi, r0
-  ; output digit
-	subi	t_hi, lo8(-'0')
-	st	Z+, t_hi
-  ; quotient == 0 ?
-	brtc	.L_dec_loop
-  ; end of string
-.L_eos:
-	X_movw	r24, ZL
-	ret
-
-  ; octal format
-.L_oct:
-	mov	symb, v_lo
-	andi	symb, 7
-	subi	symb, lo8(-'0')
-	st	Z+, symb
-	ldi	cnt, 3
-	rcall	.L_lsr
-	brne	.L_oct
-	rjmp	.L_eos
-
-  ; hex format
-.L_hex:
-	mov	symb, v_lo
-	andi	symb, 0x0f
-	subi	symb, lo8(-'0')
-	cpi	symb, '9' + 1
-	brlo	3f
-	subi	symb, lo8('9' + 1 - 'a')
-	sbrc	flags, ntz(XTOA_UPPER) - 8
-	subi	symb, lo8('a' - 'A')
-3:	st	Z+, symb
-	rcall	.L_lsr_4
-	brne	.L_hex
-	rjmp	.L_eos
-	
-.L_lsr_4:
-	ldi	cnt, 4
-.L_lsr:
-	lsr	v_fifth
-	ror	v_hhi
-	ror	v_hlo
-	ror	v_hi
-	ror	v_lo
-	dec	cnt
-	brne	.L_lsr
-  ; tst
-	sbiw	v_hlo, 0		; only Z flag is needed
-	cpc	v_lo, rzero
-	cpc	v_hi, rzero
-	ret
-
-.L_div_add:
-  ; copy to temporary
-	X_movw	t_lo, v_lo
-	X_movw	t_hlo, v_hlo
-	mov	r0, v_fifth
-  ; lsr temporary
-7:	lsr	r0
-	ror	t_hhi
-	ror	t_hlo
-	ror	t_hi
-	ror	t_lo
-	dec	cnt
-	brne	7b
-  ; add
-	add	v_lo, t_lo
-	adc	v_hi, t_hi
-	adc	v_hlo, t_hlo
-	adc	v_hhi, t_hhi
-	adc	v_fifth, r0		; here r0 == 0
-	ret
-
-	.size	__ultoa_invert, . - __ultoa_invert
-	.end
-
-#endif	/* !__DOXYGEN__ */
-

--- a/libraries/FastSerial/vprintf.cpp
+++ b/libraries/FastSerial/vprintf.cpp
@@ -1,522 +1,535 @@
-// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
-/*
-   Adapted from the avr-libc vfprintf:
+// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-

+/*

+   Adapted from the avr-libc vfprintf:

+

+   Copyright (c) 2002, Alexander Popov (sasho@vip.bg)

+   Copyright (c) 2002,2004,2005 Joerg Wunsch

+   Copyright (c) 2005, Helmut Wallner

+   Copyright (c) 2007, Dmitry Xmelkov

+   All rights reserved.

+

+   Redistribution and use in source and binary forms, with or without

+   modification, are permitted provided that the following conditions are met:

+

+   * Redistributions of source code must retain the above copyright

+   notice, this list of conditions and the following disclaimer.

+   * Redistributions in binary form must reproduce the above copyright

+   notice, this list of conditions and the following disclaimer in

+   the documentation and/or other materials provided with the

+   distribution.

+   * Neither the name of the copyright holders nor the names of

+   contributors may be used to endorse or promote products derived

+   from this software without specific prior written permission.

+

+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+   POSSIBILITY OF SUCH DAMAGE.

+*/

+

+/* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */

+/* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */

+

+#include "BetterStream.h"

+

+#include <avr/pgmspace.h>

+#include <stdarg.h>

+#include <string.h>

+extern "C" {

+#include "ftoa_engine.h"

+#include "ntz.h"

+#include "xtoa_fast.h"

+}

+

+// workaround for GCC bug c++/34734

+#undef PROGMEM 

+#define PROGMEM __attribute__(( section(".progmem.data") )) 

+#undef PSTR 

+#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 

+

+#define GETBYTE(flag, mask, pnt)        ({                              \

+                        unsigned char __c;                              \

+                        asm (                                           \

+                             "sbrc      %2,%3   \n\t"                   \

+                             "lpm       %0,Z+   \n\t"                   \

+                             "sbrs      %2,%3   \n\t"                   \

+                             "ld        %0,Z+   "                       \

+                             : "=r" (__c),                              \

+                               "+z" (pnt)                               \

+                             : "r" (flag),                              \

+                               "I" (ntz(mask))                          \

+                        );                                              \

+                        __c;                                            \

+                })

+

+#define FL_ZFILL	0x01

+#define FL_PLUS		0x02

+#define FL_SPACE	0x04

+#define FL_LPAD		0x08

+#define FL_ALT		0x10

+#define FL_WIDTH	0x20

+#define FL_PREC		0x40

+#define FL_LONG		0x80

+

+#define FL_PGMSTRING	FL_LONG

+#define FL_NEGATIVE	FL_LONG

+

+#define FL_ALTUPP	FL_PLUS

+#define FL_ALTHEX	FL_SPACE

+

+#define	FL_FLTUPP	FL_ALT

+#define FL_FLTEXP	FL_PREC

+#define	FL_FLTFIX	FL_LONG

+

+#ifdef DESKTOP_BUILD

+void

+BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)

+{

+        char *str = NULL;

+        int i;

+        char *fmt2 = strdup(fmt);

+        for (i=0; fmt2[i]; i++) {

+                // cope with %S

+                if (fmt2[i] == '%' && fmt2[i+1] == 'S') {

+                        fmt2[i+1] = 's';

+                }

+        }

+        vasprintf(&str, fmt2, ap);

+        for (i=0; str[i]; i++) {

+                write(str[i]);

+        }

+        free(str);

+        free(fmt2);

+}

+#else

+void

+BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)

+{

+        unsigned char c;        /* holds a char from the format string */

+        unsigned char flags;

+        unsigned char width;

+        unsigned char prec;

+        unsigned char buf[11];  /* size for -1 in octal, without '\0'   */

+

+        for (;;) {

+

+                /*

+                 * Process non-format characters

+                 */

+                for (;;) {

+                        c = GETBYTE (in_progmem, 1, fmt);

+                        if (!c) return;

+                        if (c == '%') {

+                                c = GETBYTE (in_progmem, 1, fmt);

+                                if (c != '%') break;

+                        }

+                        /* emit cr before lf to make most terminals happy */

+                        if (c == '\n')

+                                write('\r');

+                        write(c);

+                }

+

+                flags = 0;

+                width = 0;

+                prec = 0;

+                

+                /*

+                 * Process format adjustment characters, precision, width.

+                 */

+                do {

+                        if (flags < FL_WIDTH) {

+                                switch (c) {

+                                case '0':

+                                        flags |= FL_ZFILL;

+                                        continue;

+                                case '+':

+                                        flags |= FL_PLUS;

+                                        /* FALLTHROUGH */

+                                case ' ':

+                                        flags |= FL_SPACE;

+                                        continue;

+                                case '-':

+                                        flags |= FL_LPAD;

+                                        continue;

+                                case '#':

+                                        flags |= FL_ALT;

+                                        continue;

+                                }

+                        }

+

+                        if (flags < FL_LONG) {

+                                if (c >= '0' && c <= '9') {

+                                        c -= '0';

+                                        if (flags & FL_PREC) {

+                                                prec = 10*prec + c;

+                                                continue;

+                                        }

+                                        width = 10*width + c;

+                                        flags |= FL_WIDTH;

+                                        continue;

+                                }

+                                if (c == '.') {

+                                        if (flags & FL_PREC)

+                                                return;

+                                        flags |= FL_PREC;

+                                        continue;

+                                }

+                                if (c == 'l') {

+                                        flags |= FL_LONG;

+                                        continue;

+                                }

+                                if (c == 'h')

+                                        continue;

+                        }

+            

+                        break;

+                } while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0);

+

+                /*

+                 * Handle floating-point formats E, F, G, e, f, g.

+                 */

+                if (c >= 'E' && c <= 'G') {

+                        flags |= FL_FLTUPP;

+                        c += 'e' - 'E';

+                        goto flt_oper;

+

+                } else if (c >= 'e' && c <= 'g') {

+

+                        int exp;                /* exponent of master decimal digit     */

+                        int n;

+                        unsigned char vtype;    /* result of float value parse  */

+                        unsigned char sign;     /* sign character (or 0)        */

+                        unsigned char ndigs;

+

+                        flags &= ~FL_FLTUPP;

+

+                flt_oper:

+                        if (!(flags & FL_PREC))

+                                prec = 6;

+                        flags &= ~(FL_FLTEXP | FL_FLTFIX);

+                        if (c == 'e')

+                                flags |= FL_FLTEXP;

+                        else if (c == 'f')

+                                flags |= FL_FLTFIX;

+                        else if (prec > 0)

+                                prec -= 1;

+

+                        if (flags & FL_FLTFIX) {

+                                vtype = 7;              /* 'prec' arg for 'ftoa_engine' */

+                                ndigs = prec < 60 ? prec + 1 : 60;

+                        } else {

+                                if (prec > 7) prec = 7;

+                                vtype = prec;

+                                ndigs = 0;

+                        }

+                        exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs);

+                        vtype = buf[0];

+    

+                        sign = 0;

+                        if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN))

+                                sign = '-';

+                        else if (flags & FL_PLUS)

+                                sign = '+';

+                        else if (flags & FL_SPACE)

+                                sign = ' ';

+

+                        if (vtype & (FTOA_NAN | FTOA_INF)) {

+                                const char *p;

+                                ndigs = sign ? 4 : 3;

+                                if (width > ndigs) {

+                                        width -= ndigs;

+                                        if (!(flags & FL_LPAD)) {

+                                                do {

+                                                        write(' ');

+                                                } while (--width);

+                                        }

+                                } else {

+                                        width = 0;

+                                }

+                                if (sign)

+                                        write(sign);

+                                p = PSTR("inf");

+                                if (vtype & FTOA_NAN)

+                                        p = PSTR("nan");

+                                while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) {

+                                        if (flags & FL_FLTUPP)

+                                                ndigs += 'I' - 'i';

+                                        write(ndigs);

+                                        p++;

+                                }

+                                goto tail;

+                        }

+

+                        /* Output format adjustment, number of decimal digits in buf[] */

+                        if (flags & FL_FLTFIX) {

+                                ndigs += exp;

+                                if ((vtype & FTOA_CARRY) && buf[1] == '1')

+                                        ndigs -= 1;

+                                if ((signed char)ndigs < 1)

+                                        ndigs = 1;

+                                else if (ndigs > 8)

+                                        ndigs = 8;

+                        } else if (!(flags & FL_FLTEXP)) {              /* 'g(G)' format */

+                                if (exp <= prec && exp >= -4)

+                                        flags |= FL_FLTFIX;

+                                while (prec && buf[1+prec] == '0')

+                                        prec--;

+                                if (flags & FL_FLTFIX) {

+                                        ndigs = prec + 1;               /* number of digits in buf */

+                                        prec = prec > exp

+                                                ? prec - exp : 0;       /* fractional part length  */

+                                }

+                        }

+    

+                        /* Conversion result length, width := free space length */

+                        if (flags & FL_FLTFIX)

+                                n = (exp>0 ? exp+1 : 1);

+                        else

+                                n = 5;          /* 1e+00 */

+                        if (sign) n += 1;

+                        if (prec) n += prec + 1;

+                        width = width > n ? width - n : 0;

+    

+                        /* Output before first digit    */

+                        if (!(flags & (FL_LPAD | FL_ZFILL))) {

+                                while (width) {

+                                        write(' ');

+                                        width--;

+                                }

+                        }

+                        if (sign) write(sign);

+                        if (!(flags & FL_LPAD)) {

+                                while (width) {

+                                        write('0');

+                                        width--;

+                                }

+                        }

+    

+                        if (flags & FL_FLTFIX) {                /* 'f' format           */

+

+                                n = exp > 0 ? exp : 0;          /* exponent of left digit */

+                                do {

+                                        if (n == -1)

+                                                write('.');

+                                        flags = (n <= exp && n > exp - ndigs)

+                                                ? buf[exp - n + 1] : '0';

+                                        if (--n < -prec)

+                                                break;

+                                        write(flags);

+                                } while (1);

+                                if (n == exp

+                                    && (buf[1] > '5'

+                                        || (buf[1] == '5' && !(vtype & FTOA_CARRY))) )

+                                        {

+                                                flags = '1';

+                                        }

+                                write(flags);

+        

+                        } else {                                /* 'e(E)' format        */

+

+                                /* mantissa     */

+                                if (buf[1] != '1')

+                                        vtype &= ~FTOA_CARRY;

+                                write(buf[1]);

+                                if (prec) {

+                                        write('.');

+                                        sign = 2;

+                                        do {

+                                                write(buf[sign++]);

+                                        } while (--prec);

+                                }

+

+                                /* exponent     */

+                                write(flags & FL_FLTUPP ? 'E' : 'e');

+                                ndigs = '+';

+                                if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) {

+                                        exp = -exp;

+                                        ndigs = '-';

+                                }

+                                write(ndigs);

+                                for (ndigs = '0'; exp >= 10; exp -= 10)

+                                        ndigs += 1;

+                                write(ndigs);

+                                write('0' + exp);

+                        }

+

+                        goto tail;

+                }

+

+                /*

+                 * Handle string formats c, s, S.

+                 */

+                {

+                        const char * pnt;

+                        size_t size;

+

+                        switch (c) {

+

+                        case 'c':

+                                buf[0] = va_arg (ap, int);

+                                pnt = (char *)buf;

+                                size = 1;

+                                goto no_pgmstring;

+

+                        case 's':

+                                pnt = va_arg (ap, char *);

+                                size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0);

+                        no_pgmstring:

+                                flags &= ~FL_PGMSTRING;

+                                goto str_lpad;

+

+                        case 'S':

+                        // pgmstring: // not yet used

+                                pnt = va_arg (ap, char *);

+                                size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0);

+                                flags |= FL_PGMSTRING;

+

+                        str_lpad:

+                                if (!(flags & FL_LPAD)) {

+                                        while (size < width) {

+                                                write(' ');

+                                                width--;

+                                        }

+                                }

+                                while (size) {

+                                        write(GETBYTE (flags, FL_PGMSTRING, pnt));

+                                        if (width) width -= 1;

+                                        size -= 1;

+                                }

+                                goto tail;

+                        }

+                }

+

+                /*

+                 * Handle integer formats variations for d/i, u, o, p, x, X.

+                 */

+                if (c == 'd' || c == 'i') {

+                        long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int);

+                        flags &= ~(FL_NEGATIVE | FL_ALT);

+                        if (x < 0) {

+                                x = -x;

+                                flags |= FL_NEGATIVE;

+                        }

+                        c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf;

+

+                } else {

+                        int base;

+

+                        if (c == 'u') {

+                                flags &= ~FL_ALT;

+                                base = 10;

+                                goto ultoa;

+                        }

+

+                        flags &= ~(FL_PLUS | FL_SPACE);

+

+                        switch (c) {

+                        case 'o':

+                                base = 8;

+                                goto ultoa;

+                        case 'p':

+                                flags |= FL_ALT;

+                                /* no break */

+                        case 'x':

+                                if (flags & FL_ALT)

+                                        flags |= FL_ALTHEX;

+                                base = 16;

+                                goto ultoa;

+                        case 'X':

+                                if (flags & FL_ALT)

+                                        flags |= (FL_ALTHEX | FL_ALTUPP);

+                                base = 16 | XTOA_UPPER;

+                        ultoa:

+                                c = __ultoa_invert ((flags & FL_LONG)

+                                                    ? va_arg(ap, unsigned long)

+                                                    : va_arg(ap, unsigned int),

+                                                    (char *)buf, base)  -  (char *)buf;

+                                flags &= ~FL_NEGATIVE;

+                                break;

+

+                        default:

+                                return;

+                        }

+                }

+

+                /*

+                 * Format integers.

+                 */

+                {

+                        unsigned char len;

+

+                        len = c;

+                        if (flags & FL_PREC) {

+                                flags &= ~FL_ZFILL;

+                                if (len < prec) {

+                                        len = prec;

+                                        if ((flags & FL_ALT) && !(flags & FL_ALTHEX))

+                                                flags &= ~FL_ALT;

+                                }

+                        }

+                        if (flags & FL_ALT) {

+                                if (buf[c-1] == '0') {

+                                        flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP);

+                                } else {

+                                        len += 1;

+                                        if (flags & FL_ALTHEX)

+                                                len += 1;

+                                }

+                        } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {

+                                len += 1;

+                        }

+

+                        if (!(flags & FL_LPAD)) {

+                                if (flags & FL_ZFILL) {

+                                        prec = c;

+                                        if (len < width) {

+                                                prec += width - len;

+                                                len = width;

+                                        }

+                                }

+                                while (len < width) {

+                                        write(' ');

+                                        len++;

+                                }

+                        }

+        

+                        width =  (len < width) ? width - len : 0;

+

+                        if (flags & FL_ALT) {

+                                write('0');

+                                if (flags & FL_ALTHEX)

+                                        write(flags & FL_ALTUPP ? 'X' : 'x');

+                        } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {

+                                unsigned char z = ' ';

+                                if (flags & FL_PLUS) z = '+';

+                                if (flags & FL_NEGATIVE) z = '-';

+                                write(z);

+                        }

+                

+                        while (prec > c) {

+                                write('0');

+                                prec--;

+                        }

+        

+                        do {

+                                write(buf[--c]);

+                        } while (c);

+                }

+        

+        tail:

+                /* Tail is possible.    */

+                while (width) {

+                        write(' ');

+                        width--;

+                }

+        } /* for (;;) */

+}

+#endif // DESKTOP_BUILD

 
-   Copyright (c) 2002, Alexander Popov (sasho@vip.bg)
-   Copyright (c) 2002,2004,2005 Joerg Wunsch
-   Copyright (c) 2005, Helmut Wallner
-   Copyright (c) 2007, Dmitry Xmelkov
-   All rights reserved.
-
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-
-   * Redistributions of source code must retain the above copyright
-   notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-   notice, this list of conditions and the following disclaimer in
-   the documentation and/or other materials provided with the
-   distribution.
-   * Neither the name of the copyright holders nor the names of
-   contributors may be used to endorse or promote products derived
-   from this software without specific prior written permission.
-
-   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-   POSSIBILITY OF SUCH DAMAGE.
-*/
-
-/* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */
-/* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */
-
-#include "BetterStream.h"
-
-#include <avr/pgmspace.h>
-#include <stdarg.h>
-#include <string.h>
-extern "C" {
-#include "ftoa_engine.h"
-#include "ntz.h"
-#include "xtoa_fast.h"
-}
-
-// workaround for GCC bug c++/34734
-#undef PROGMEM 
-#define PROGMEM __attribute__(( section(".progmem.data") )) 
-#undef PSTR 
-#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 
-
-#define GETBYTE(flag, mask, pnt)        ({                              \
-                        unsigned char __c;                              \
-                        asm (                                           \
-                             "sbrc      %2,%3   \n\t"                   \
-                             "lpm       %0,Z+   \n\t"                   \
-                             "sbrs      %2,%3   \n\t"                   \
-                             "ld        %0,Z+   "                       \
-                             : "=r" (__c),                              \
-                               "+z" (pnt)                               \
-                             : "r" (flag),                              \
-                               "I" (ntz(mask))                          \
-                        );                                              \
-                        __c;                                            \
-                })
-/*
-#define GETBYTE(flag, mask, pnt)        ({      \
-       unsigned char __c;                       \
-       __c = ((flag) & (mask))                  \
-             ? pgm_read_byte(pnt) : *pnt;       \
-       pnt++;                                   \
-       __c;                                     \
-})
-*/
-
-#define FL_ZFILL	0x01
-#define FL_PLUS		0x02
-#define FL_SPACE	0x04
-#define FL_LPAD		0x08
-#define FL_ALT		0x10
-#define FL_WIDTH	0x20
-#define FL_PREC		0x40
-#define FL_LONG		0x80
-
-#define FL_PGMSTRING	FL_LONG
-#define FL_NEGATIVE	FL_LONG
-
-#define FL_ALTUPP	FL_PLUS
-#define FL_ALTHEX	FL_SPACE
-
-#define	FL_FLTUPP	FL_ALT
-#define FL_FLTEXP	FL_PREC
-#define	FL_FLTFIX	FL_LONG
-
-void
-BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
-{
-        unsigned char c;        /* holds a char from the format string */
-        unsigned char flags;
-        unsigned char width;
-        unsigned char prec;
-        unsigned char buf[11];  /* size for -1 in octal, without '\0'   */
-
-        for (;;) {
-
-                /*
-                 * Process non-format characters
-                 */
-                for (;;) {
-                        c = GETBYTE (in_progmem, 1, fmt);
-                        if (!c) return;
-                        if (c == '%') {
-                                c = GETBYTE (in_progmem, 1, fmt);
-                                if (c != '%') break;
-                        }
-                        /* emit cr before lf to make most terminals happy */
-                        if (c == '\n')
-                                write('\r');
-                        write(c);
-                }
-
-                flags = 0;
-                width = 0;
-                prec = 0;
-                
-                /*
-                 * Process format adjustment characters, precision, width.
-                 */
-                do {
-                        if (flags < FL_WIDTH) {
-                                switch (c) {
-                                case '0':
-                                        flags |= FL_ZFILL;
-                                        continue;
-                                case '+':
-                                        flags |= FL_PLUS;
-                                        /* FALLTHROUGH */
-                                case ' ':
-                                        flags |= FL_SPACE;
-                                        continue;
-                                case '-':
-                                        flags |= FL_LPAD;
-                                        continue;
-                                case '#':
-                                        flags |= FL_ALT;
-                                        continue;
-                                }
-                        }
-
-                        if (flags < FL_LONG) {
-                                if (c >= '0' && c <= '9') {
-                                        c -= '0';
-                                        if (flags & FL_PREC) {
-                                                prec = 10*prec + c;
-                                                continue;
-                                        }
-                                        width = 10*width + c;
-                                        flags |= FL_WIDTH;
-                                        continue;
-                                }
-                                if (c == '.') {
-                                        if (flags & FL_PREC)
-                                                return;
-                                        flags |= FL_PREC;
-                                        continue;
-                                }
-                                if (c == 'l') {
-                                        flags |= FL_LONG;
-                                        continue;
-                                }
-                                if (c == 'h')
-                                        continue;
-                        }
-            
-                        break;
-                } while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0);
-
-                /*
-                 * Handle floating-point formats E, F, G, e, f, g.
-                 */
-                if (c >= 'E' && c <= 'G') {
-                        flags |= FL_FLTUPP;
-                        c += 'e' - 'E';
-                        goto flt_oper;
-
-                } else if (c >= 'e' && c <= 'g') {
-
-                        int exp;                /* exponent of master decimal digit     */
-                        int n;
-                        unsigned char vtype;    /* result of float value parse  */
-                        unsigned char sign;     /* sign character (or 0)        */
-                        unsigned char ndigs;
-
-                        flags &= ~FL_FLTUPP;
-
-                flt_oper:
-                        if (!(flags & FL_PREC))
-                                prec = 6;
-                        flags &= ~(FL_FLTEXP | FL_FLTFIX);
-                        if (c == 'e')
-                                flags |= FL_FLTEXP;
-                        else if (c == 'f')
-                                flags |= FL_FLTFIX;
-                        else if (prec > 0)
-                                prec -= 1;
-
-                        if (flags & FL_FLTFIX) {
-                                vtype = 7;              /* 'prec' arg for 'ftoa_engine' */
-                                ndigs = prec < 60 ? prec + 1 : 60;
-                        } else {
-                                if (prec > 7) prec = 7;
-                                vtype = prec;
-                                ndigs = 0;
-                        }
-                        exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs);
-                        vtype = buf[0];
-    
-                        sign = 0;
-                        if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN))
-                                sign = '-';
-                        else if (flags & FL_PLUS)
-                                sign = '+';
-                        else if (flags & FL_SPACE)
-                                sign = ' ';
-
-                        if (vtype & (FTOA_NAN | FTOA_INF)) {
-                                const char *p;
-                                ndigs = sign ? 4 : 3;
-                                if (width > ndigs) {
-                                        width -= ndigs;
-                                        if (!(flags & FL_LPAD)) {
-                                                do {
-                                                        write(' ');
-                                                } while (--width);
-                                        }
-                                } else {
-                                        width = 0;
-                                }
-                                if (sign)
-                                        write(sign);
-                                p = PSTR("inf");
-                                if (vtype & FTOA_NAN)
-                                        p = PSTR("nan");
-                                while ( (ndigs = pgm_read_byte(p)) != 0) {
-                                        if (flags & FL_FLTUPP)
-                                                ndigs += 'I' - 'i';
-                                        write(ndigs);
-                                        p++;
-                                }
-                                goto tail;
-                        }
-
-                        /* Output format adjustment, number of decimal digits in buf[] */
-                        if (flags & FL_FLTFIX) {
-                                ndigs += exp;
-                                if ((vtype & FTOA_CARRY) && buf[1] == '1')
-                                        ndigs -= 1;
-                                if ((signed char)ndigs < 1)
-                                        ndigs = 1;
-                                else if (ndigs > 8)
-                                        ndigs = 8;
-                        } else if (!(flags & FL_FLTEXP)) {              /* 'g(G)' format */
-                                if (exp <= prec && exp >= -4)
-                                        flags |= FL_FLTFIX;
-                                while (prec && buf[1+prec] == '0')
-                                        prec--;
-                                if (flags & FL_FLTFIX) {
-                                        ndigs = prec + 1;               /* number of digits in buf */
-                                        prec = prec > exp
-                                                ? prec - exp : 0;       /* fractional part length  */
-                                }
-                        }
-    
-                        /* Conversion result length, width := free space length */
-                        if (flags & FL_FLTFIX)
-                                n = (exp>0 ? exp+1 : 1);
-                        else
-                                n = 5;          /* 1e+00 */
-                        if (sign) n += 1;
-                        if (prec) n += prec + 1;
-                        width = width > n ? width - n : 0;
-    
-                        /* Output before first digit    */
-                        if (!(flags & (FL_LPAD | FL_ZFILL))) {
-                                while (width) {
-                                        write(' ');
-                                        width--;
-                                }
-                        }
-                        if (sign) write(sign);
-                        if (!(flags & FL_LPAD)) {
-                                while (width) {
-                                        write('0');
-                                        width--;
-                                }
-                        }
-    
-                        if (flags & FL_FLTFIX) {                /* 'f' format           */
-
-                                n = exp > 0 ? exp : 0;          /* exponent of left digit */
-                                do {
-                                        if (n == -1)
-                                                write('.');
-                                        flags = (n <= exp && n > exp - ndigs)
-                                                ? buf[exp - n + 1] : '0';
-                                        if (--n < -prec)
-                                                break;
-                                        write(flags);
-                                } while (1);
-                                if (n == exp
-                                    && (buf[1] > '5'
-                                        || (buf[1] == '5' && !(vtype & FTOA_CARRY))) )
-                                        {
-                                                flags = '1';
-                                        }
-                                write(flags);
-        
-                        } else {                                /* 'e(E)' format        */
-
-                                /* mantissa     */
-                                if (buf[1] != '1')
-                                        vtype &= ~FTOA_CARRY;
-                                write(buf[1]);
-                                if (prec) {
-                                        write('.');
-                                        sign = 2;
-                                        do {
-                                                write(buf[sign++]);
-                                        } while (--prec);
-                                }
-
-                                /* exponent     */
-                                write(flags & FL_FLTUPP ? 'E' : 'e');
-                                ndigs = '+';
-                                if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) {
-                                        exp = -exp;
-                                        ndigs = '-';
-                                }
-                                write(ndigs);
-                                for (ndigs = '0'; exp >= 10; exp -= 10)
-                                        ndigs += 1;
-                                write(ndigs);
-                                write('0' + exp);
-                        }
-
-                        goto tail;
-                }
-
-                /*
-                 * Handle string formats c, s, S.
-                 */
-                {
-                        const char * pnt;
-                        size_t size;
-
-                        switch (c) {
-
-                        case 'c':
-                                buf[0] = va_arg (ap, int);
-                                pnt = (char *)buf;
-                                size = 1;
-                                goto no_pgmstring;
-
-                        case 's':
-                                pnt = va_arg (ap, char *);
-                                size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0);
-                        no_pgmstring:
-                                flags &= ~FL_PGMSTRING;
-                                goto str_lpad;
-
-                        case 'S':
-                        pgmstring:
-                                pnt = va_arg (ap, char *);
-                                size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0);
-                                flags |= FL_PGMSTRING;
-
-                        str_lpad:
-                                if (!(flags & FL_LPAD)) {
-                                        while (size < width) {
-                                                write(' ');
-                                                width--;
-                                        }
-                                }
-                                while (size) {
-                                        write(GETBYTE (flags, FL_PGMSTRING, pnt));
-                                        if (width) width -= 1;
-                                        size -= 1;
-                                }
-                                goto tail;
-                        }
-                }
-
-                /*
-                 * Handle integer formats variations for d/i, u, o, p, x, X.
-                 */
-                if (c == 'd' || c == 'i') {
-                        long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int);
-                        flags &= ~(FL_NEGATIVE | FL_ALT);
-                        if (x < 0) {
-                                x = -x;
-                                flags |= FL_NEGATIVE;
-                        }
-                        c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf;
-
-                } else {
-                        int base;
-
-                        if (c == 'u') {
-                                flags &= ~FL_ALT;
-                                base = 10;
-                                goto ultoa;
-                        }
-
-                        flags &= ~(FL_PLUS | FL_SPACE);
-
-                        switch (c) {
-                        case 'o':
-                                base = 8;
-                                goto ultoa;
-                        case 'p':
-                                flags |= FL_ALT;
-                                /* no break */
-                        case 'x':
-                                if (flags & FL_ALT)
-                                        flags |= FL_ALTHEX;
-                                base = 16;
-                                goto ultoa;
-                        case 'X':
-                                if (flags & FL_ALT)
-                                        flags |= (FL_ALTHEX | FL_ALTUPP);
-                                base = 16 | XTOA_UPPER;
-                        ultoa:
-                                c = __ultoa_invert ((flags & FL_LONG)
-                                                    ? va_arg(ap, unsigned long)
-                                                    : va_arg(ap, unsigned int),
-                                                    (char *)buf, base)  -  (char *)buf;
-                                flags &= ~FL_NEGATIVE;
-                                break;
-
-                        default:
-                                return;
-                        }
-                }
-
-                /*
-                 * Format integers.
-                 */
-                {
-                        unsigned char len;
-
-                        len = c;
-                        if (flags & FL_PREC) {
-                                flags &= ~FL_ZFILL;
-                                if (len < prec) {
-                                        len = prec;
-                                        if ((flags & FL_ALT) && !(flags & FL_ALTHEX))
-                                                flags &= ~FL_ALT;
-                                }
-                        }
-                        if (flags & FL_ALT) {
-                                if (buf[c-1] == '0') {
-                                        flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP);
-                                } else {
-                                        len += 1;
-                                        if (flags & FL_ALTHEX)
-                                                len += 1;
-                                }
-                        } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
-                                len += 1;
-                        }
-
-                        if (!(flags & FL_LPAD)) {
-                                if (flags & FL_ZFILL) {
-                                        prec = c;
-                                        if (len < width) {
-                                                prec += width - len;
-                                                len = width;
-                                        }
-                                }
-                                while (len < width) {
-                                        write(' ');
-                                        len++;
-                                }
-                        }
-        
-                        width =  (len < width) ? width - len : 0;
-
-                        if (flags & FL_ALT) {
-                                write('0');
-                                if (flags & FL_ALTHEX)
-                                        write(flags & FL_ALTUPP ? 'X' : 'x');
-                        } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
-                                unsigned char z = ' ';
-                                if (flags & FL_PLUS) z = '+';
-                                if (flags & FL_NEGATIVE) z = '-';
-                                write(z);
-                        }
-                
-                        while (prec > c) {
-                                write('0');
-                                prec--;
-                        }
-        
-                        do {
-                                write(buf[--c]);
-                        } while (c);
-                }
-        
-        tail:
-                /* Tail is possible.    */
-                while (width) {
-                        write(' ');
-                        width--;
-                }
-        } /* for (;;) */
-}
-

--- a/libraries/FastSerial/xtoa_fast.h
+++ b/libraries/FastSerial/xtoa_fast.h
@@ -1,49 +1,49 @@
-/*
-   Adapted from avr-libc:
+/*

+   Adapted from avr-libc:

+

+   Copyright (c) 2005, Dmitry Xmelkov

+   All rights reserved.

+

+   Redistribution and use in source and binary forms, with or without

+   modification, are permitted provided that the following conditions are met:

+

+   * Redistributions of source code must retain the above copyright

+     notice, this list of conditions and the following disclaimer.

+   * Redistributions in binary form must reproduce the above copyright

+     notice, this list of conditions and the following disclaimer in

+     the documentation and/or other materials provided with the

+     distribution.

+   * Neither the name of the copyright holders nor the names of

+     contributors may be used to endorse or promote products derived

+     from this software without specific prior written permission.

+

+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+  POSSIBILITY OF SUCH DAMAGE. */

+

+/* $Id: xtoa_fast.h 1223 2007-02-18 13:33:09Z dmix $	*/

+

+#ifndef	_XTOA_FAST_H_

+#define _XTOA_FAST_H_

+

+#ifndef	__ASSEMBLER__

+

+/* Internal function for use from `printf'.	*/

+char * __ultoa_invert (unsigned long val, char *s, int base);

+

+#endif	/* ifndef __ASSEMBLER__ */

+

+/* Next flags are to use with `base'. Unused fields are reserved.	*/

+#define XTOA_PREFIX	0x0100	/* put prefix for octal or hex	*/

+#define XTOA_UPPER	0x0200	/* use upper case letters	*/

+

+#endif	/* _XTOA_FAST_H_ */

 
-   Copyright (c) 2005, Dmitry Xmelkov
-   All rights reserved.
-
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in
-     the documentation and/or other materials provided with the
-     distribution.
-   * Neither the name of the copyright holders nor the names of
-     contributors may be used to endorse or promote products derived
-     from this software without specific prior written permission.
-
-  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-  POSSIBILITY OF SUCH DAMAGE. */
-
-/* $Id: xtoa_fast.h 1223 2007-02-18 13:33:09Z dmix $	*/
-
-#ifndef	_XTOA_FAST_H_
-#define _XTOA_FAST_H_
-
-#ifndef	__ASSEMBLER__
-
-/* Internal function for use from `printf'.	*/
-char * __ultoa_invert (unsigned long val, char *s, int base);
-
-#endif	/* ifndef __ASSEMBLER__ */
-
-/* Next flags are to use with `base'. Unused fields are reserved.	*/
-#define XTOA_PREFIX	0x0100	/* put prefix for octal or hex	*/
-#define XTOA_UPPER	0x0200	/* use upper case letters	*/
-
-#endif	/* _XTOA_FAST_H_ */
-

--- a/libraries/mavlink/README
+++ /dev/null
@@ -1,26 +1,1 @@
-MAVLink Micro Air Vehicle Message Marshalling Library
 
-This is a library for lightweight communication between
-Micro Air Vehicles (swarm) and/or ground control stations.
-
-It serializes C-structs for serial channels and can be used with
-any type of radio modem.
-
-To generate/update packets, select mavlink_standard_message.xml
-in the QGroundControl station settings view, select mavlink/include as
-the output directory and click on "Save and Generate".
-You will find the newly generated/updated message_xx.h files in
-the mavlink/include/generated folder.
-
-To use MAVLink, #include the <mavlink.h> file, not the individual
-message files. In some cases you will have to add the main folder to the include search path as well. To be safe, we recommend these flags:
-
-gcc -I mavlink/include -I mavlink/include/<your message set, e.g. common>
-
-For more information, please visit:
-
-http://pixhawk.ethz.ch/wiki/software/mavlink/
-
-(c) 2009-2011 Lorenz Meier <pixhawk@switched.com> / PIXHAWK Team
-
-

--- a/libraries/mavlink/include/ardupilotmega/ardupilotmega.h
+++ /dev/null
@@ -1,188 +1,1 @@
-/** @file
- *	@brief MAVLink comm protocol generated from ardupilotmega.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef ARDUPILOTMEGA_H
-#define ARDUPILOTMEGA_H
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// MESSAGE LENGTHS AND CRCS
-
-#ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_ARDUPILOTMEGA
-
-// ENUM DEFINITIONS
-
-
-/** @brief Enumeration of possible mount operation modes */
-#ifndef HAVE_ENUM_MAV_MOUNT_MODE
-#define HAVE_ENUM_MAV_MOUNT_MODE
-enum MAV_MOUNT_MODE
-{
-	MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */
-	MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */
-	MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */
-	MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */
-	MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */
-	MAV_MOUNT_MODE_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_CMD
-#define HAVE_ENUM_MAV_CMD
-enum MAV_CMD
-{
-	MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
-	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
-	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
-	MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude|  */
-	MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
-	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
-	MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|  */
-	MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|  */
-	MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */
-	MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
-	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
-	MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|  */
-	MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
-	MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
-	MAV_CMD_ENUM_END=401, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_FENCE_ACTION
-#define HAVE_ENUM_FENCE_ACTION
-enum FENCE_ACTION
-{
-	FENCE_ACTION_NONE=0, /* Disable fenced mode | */
-	FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */
-	FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */
-	FENCE_ACTION_ENUM_END=3, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_FENCE_BREACH
-#define HAVE_ENUM_FENCE_BREACH
-enum FENCE_BREACH
-{
-	FENCE_BREACH_NONE=0, /* No last fence breach | */
-	FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */
-	FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */
-	FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */
-	FENCE_BREACH_ENUM_END=4, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_LIMITS_STATE
-#define HAVE_ENUM_LIMITS_STATE
-enum LIMITS_STATE
-{
-	LIMITS_INIT=0, /*  pre-initialization | */
-	LIMITS_DISABLED=1, /*  disabled | */
-	LIMITS_ENABLED=2, /*  checking limits | */
-	LIMITS_TRIGGERED=3, /*  a limit has been breached | */
-	LIMITS_RECOVERING=4, /*  taking action eg. RTL | */
-	LIMITS_RECOVERED=5, /*  we're no longer in breach of a limit | */
-	LIMITS_STATE_ENUM_END=6, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_LIMIT_MODULE
-#define HAVE_ENUM_LIMIT_MODULE
-enum LIMIT_MODULE
-{
-	LIMIT_GPSLOCK=1, /*  pre-initialization | */
-	LIMIT_GEOFENCE=2, /*  disabled | */
-	LIMIT_ALTITUDE=4, /*  checking limits | */
-	LIMIT_MODULE_ENUM_END=5, /*  | */
-};
-#endif
-
-#include "../common/common.h"
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 2
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_sensor_offsets.h"
-#include "./mavlink_msg_set_mag_offsets.h"
-#include "./mavlink_msg_meminfo.h"
-#include "./mavlink_msg_ap_adc.h"
-#include "./mavlink_msg_digicam_configure.h"
-#include "./mavlink_msg_digicam_control.h"
-#include "./mavlink_msg_mount_configure.h"
-#include "./mavlink_msg_mount_control.h"
-#include "./mavlink_msg_mount_status.h"
-#include "./mavlink_msg_fence_point.h"
-#include "./mavlink_msg_fence_fetch_point.h"
-#include "./mavlink_msg_fence_status.h"
-#include "./mavlink_msg_ahrs.h"
-#include "./mavlink_msg_simstate.h"
-#include "./mavlink_msg_hwstatus.h"
-#include "./mavlink_msg_radio.h"
-#include "./mavlink_msg_limits_status.h"
-#include "./mavlink_msg_wind.h"
-#include "./mavlink_msg_data16.h"
-#include "./mavlink_msg_data32.h"
-#include "./mavlink_msg_data64.h"
-#include "./mavlink_msg_data96.h"
-#include "./mavlink_msg_rangefinder.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // ARDUPILOTMEGA_H
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink.h
+++ /dev/null
@@ -1,28 +1,1 @@
-/** @file
- *	@brief MAVLink comm protocol built from ardupilotmega.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
 
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "ardupilotmega.h"
-
-#endif // MAVLINK_H
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_ahrs.h
+++ /dev/null
@@ -1,296 +1,1 @@
-// MESSAGE AHRS PACKING
 
-#define MAVLINK_MSG_ID_AHRS 163
-
-typedef struct __mavlink_ahrs_t
-{
- float omegaIx; ///< X gyro drift estimate rad/s
- float omegaIy; ///< Y gyro drift estimate rad/s
- float omegaIz; ///< Z gyro drift estimate rad/s
- float accel_weight; ///< average accel_weight
- float renorm_val; ///< average renormalisation value
- float error_rp; ///< average error_roll_pitch value
- float error_yaw; ///< average error_yaw value
-} mavlink_ahrs_t;
-
-#define MAVLINK_MSG_ID_AHRS_LEN 28
-#define MAVLINK_MSG_ID_163_LEN 28
-
-#define MAVLINK_MSG_ID_AHRS_CRC 127
-#define MAVLINK_MSG_ID_163_CRC 127
-
-
-
-#define MAVLINK_MESSAGE_INFO_AHRS { \
-	"AHRS", \
-	7, \
-	{  { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \
-         { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \
-         { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \
-         { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \
-         { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \
-         { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \
-         { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a ahrs message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param omegaIx X gyro drift estimate rad/s
- * @param omegaIy Y gyro drift estimate rad/s
- * @param omegaIz Z gyro drift estimate rad/s
- * @param accel_weight average accel_weight
- * @param renorm_val average renormalisation value
- * @param error_rp average error_roll_pitch value
- * @param error_yaw average error_yaw value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AHRS_LEN];
-	_mav_put_float(buf, 0, omegaIx);
-	_mav_put_float(buf, 4, omegaIy);
-	_mav_put_float(buf, 8, omegaIz);
-	_mav_put_float(buf, 12, accel_weight);
-	_mav_put_float(buf, 16, renorm_val);
-	_mav_put_float(buf, 20, error_rp);
-	_mav_put_float(buf, 24, error_yaw);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
-#else
-	mavlink_ahrs_t packet;
-	packet.omegaIx = omegaIx;
-	packet.omegaIy = omegaIy;
-	packet.omegaIz = omegaIz;
-	packet.accel_weight = accel_weight;
-	packet.renorm_val = renorm_val;
-	packet.error_rp = error_rp;
-	packet.error_yaw = error_yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AHRS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a ahrs message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param omegaIx X gyro drift estimate rad/s
- * @param omegaIy Y gyro drift estimate rad/s
- * @param omegaIz Z gyro drift estimate rad/s
- * @param accel_weight average accel_weight
- * @param renorm_val average renormalisation value
- * @param error_rp average error_roll_pitch value
- * @param error_yaw average error_yaw value
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AHRS_LEN];
-	_mav_put_float(buf, 0, omegaIx);
-	_mav_put_float(buf, 4, omegaIy);
-	_mav_put_float(buf, 8, omegaIz);
-	_mav_put_float(buf, 12, accel_weight);
-	_mav_put_float(buf, 16, renorm_val);
-	_mav_put_float(buf, 20, error_rp);
-	_mav_put_float(buf, 24, error_yaw);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
-#else
-	mavlink_ahrs_t packet;
-	packet.omegaIx = omegaIx;
-	packet.omegaIy = omegaIy;
-	packet.omegaIz = omegaIz;
-	packet.accel_weight = accel_weight;
-	packet.renorm_val = renorm_val;
-	packet.error_rp = error_rp;
-	packet.error_yaw = error_yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AHRS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a ahrs struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param ahrs C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
-{
-	return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
-}
-
-/**
- * @brief Send a ahrs message
- * @param chan MAVLink channel to send the message
- *
- * @param omegaIx X gyro drift estimate rad/s
- * @param omegaIy Y gyro drift estimate rad/s
- * @param omegaIz Z gyro drift estimate rad/s
- * @param accel_weight average accel_weight
- * @param renorm_val average renormalisation value
- * @param error_rp average error_roll_pitch value
- * @param error_yaw average error_yaw value
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AHRS_LEN];
-	_mav_put_float(buf, 0, omegaIx);
-	_mav_put_float(buf, 4, omegaIy);
-	_mav_put_float(buf, 8, omegaIz);
-	_mav_put_float(buf, 12, accel_weight);
-	_mav_put_float(buf, 16, renorm_val);
-	_mav_put_float(buf, 20, error_rp);
-	_mav_put_float(buf, 24, error_yaw);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-#else
-	mavlink_ahrs_t packet;
-	packet.omegaIx = omegaIx;
-	packet.omegaIy = omegaIy;
-	packet.omegaIz = omegaIz;
-	packet.accel_weight = accel_weight;
-	packet.renorm_val = renorm_val;
-	packet.error_rp = error_rp;
-	packet.error_yaw = error_yaw;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE AHRS UNPACKING
-
-
-/**
- * @brief Get field omegaIx from ahrs message
- *
- * @return X gyro drift estimate rad/s
- */
-static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field omegaIy from ahrs message
- *
- * @return Y gyro drift estimate rad/s
- */
-static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field omegaIz from ahrs message
- *
- * @return Z gyro drift estimate rad/s
- */
-static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field accel_weight from ahrs message
- *
- * @return average accel_weight
- */
-static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field renorm_val from ahrs message
- *
- * @return average renormalisation value
- */
-static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field error_rp from ahrs message
- *
- * @return average error_roll_pitch value
- */
-static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field error_yaw from ahrs message
- *
- * @return average error_yaw value
- */
-static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a ahrs message into a struct
- *
- * @param msg The message to decode
- * @param ahrs C-struct to decode the message contents into
- */
-static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);
-	ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);
-	ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);
-	ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);
-	ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);
-	ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);
-	ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);
-#else
-	memcpy(ahrs, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_ap_adc.h
+++ /dev/null
@@ -1,274 +1,1 @@
-// MESSAGE AP_ADC PACKING
 
-#define MAVLINK_MSG_ID_AP_ADC 153
-
-typedef struct __mavlink_ap_adc_t
-{
- uint16_t adc1; ///< ADC output 1
- uint16_t adc2; ///< ADC output 2
- uint16_t adc3; ///< ADC output 3
- uint16_t adc4; ///< ADC output 4
- uint16_t adc5; ///< ADC output 5
- uint16_t adc6; ///< ADC output 6
-} mavlink_ap_adc_t;
-
-#define MAVLINK_MSG_ID_AP_ADC_LEN 12
-#define MAVLINK_MSG_ID_153_LEN 12
-
-#define MAVLINK_MSG_ID_AP_ADC_CRC 188
-#define MAVLINK_MSG_ID_153_CRC 188
-
-
-
-#define MAVLINK_MESSAGE_INFO_AP_ADC { \
-	"AP_ADC", \
-	6, \
-	{  { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \
-         { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \
-         { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \
-         { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \
-         { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \
-         { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a ap_adc message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param adc1 ADC output 1
- * @param adc2 ADC output 2
- * @param adc3 ADC output 3
- * @param adc4 ADC output 4
- * @param adc5 ADC output 5
- * @param adc6 ADC output 6
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AP_ADC_LEN];
-	_mav_put_uint16_t(buf, 0, adc1);
-	_mav_put_uint16_t(buf, 2, adc2);
-	_mav_put_uint16_t(buf, 4, adc3);
-	_mav_put_uint16_t(buf, 6, adc4);
-	_mav_put_uint16_t(buf, 8, adc5);
-	_mav_put_uint16_t(buf, 10, adc6);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN);
-#else
-	mavlink_ap_adc_t packet;
-	packet.adc1 = adc1;
-	packet.adc2 = adc2;
-	packet.adc3 = adc3;
-	packet.adc4 = adc4;
-	packet.adc5 = adc5;
-	packet.adc6 = adc6;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AP_ADC;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-}
-
-/**
- * @brief Pack a ap_adc message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param adc1 ADC output 1
- * @param adc2 ADC output 2
- * @param adc3 ADC output 3
- * @param adc4 ADC output 4
- * @param adc5 ADC output 5
- * @param adc6 ADC output 6
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AP_ADC_LEN];
-	_mav_put_uint16_t(buf, 0, adc1);
-	_mav_put_uint16_t(buf, 2, adc2);
-	_mav_put_uint16_t(buf, 4, adc3);
-	_mav_put_uint16_t(buf, 6, adc4);
-	_mav_put_uint16_t(buf, 8, adc5);
-	_mav_put_uint16_t(buf, 10, adc6);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AP_ADC_LEN);
-#else
-	mavlink_ap_adc_t packet;
-	packet.adc1 = adc1;
-	packet.adc2 = adc2;
-	packet.adc3 = adc3;
-	packet.adc4 = adc4;
-	packet.adc5 = adc5;
-	packet.adc6 = adc6;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AP_ADC;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-}
-
-/**
- * @brief Encode a ap_adc struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param ap_adc C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc)
-{
-	return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6);
-}
-
-/**
- * @brief Send a ap_adc message
- * @param chan MAVLink channel to send the message
- *
- * @param adc1 ADC output 1
- * @param adc2 ADC output 2
- * @param adc3 ADC output 3
- * @param adc4 ADC output 4
- * @param adc5 ADC output 5
- * @param adc6 ADC output 6
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_AP_ADC_LEN];
-	_mav_put_uint16_t(buf, 0, adc1);
-	_mav_put_uint16_t(buf, 2, adc2);
-	_mav_put_uint16_t(buf, 4, adc3);
-	_mav_put_uint16_t(buf, 6, adc4);
-	_mav_put_uint16_t(buf, 8, adc5);
-	_mav_put_uint16_t(buf, 10, adc6);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-#else
-	mavlink_ap_adc_t packet;
-	packet.adc1 = adc1;
-	packet.adc2 = adc2;
-	packet.adc3 = adc3;
-	packet.adc4 = adc4;
-	packet.adc5 = adc5;
-	packet.adc6 = adc6;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE AP_ADC UNPACKING
-
-
-/**
- * @brief Get field adc1 from ap_adc message
- *
- * @return ADC output 1
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field adc2 from ap_adc message
- *
- * @return ADC output 2
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Get field adc3 from ap_adc message
- *
- * @return ADC output 3
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field adc4 from ap_adc message
- *
- * @return ADC output 4
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  6);
-}
-
-/**
- * @brief Get field adc5 from ap_adc message
- *
- * @return ADC output 5
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  8);
-}
-
-/**
- * @brief Get field adc6 from ap_adc message
- *
- * @return ADC output 6
- */
-static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  10);
-}
-
-/**
- * @brief Decode a ap_adc message into a struct
- *
- * @param msg The message to decode
- * @param ap_adc C-struct to decode the message contents into
- */
-static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg);
-	ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg);
-	ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg);
-	ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg);
-	ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg);
-	ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg);
-#else
-	memcpy(ap_adc, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AP_ADC_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_data100.h
+++ /dev/null
@@ -1,183 +1,1 @@
-// MESSAGE DATA100 PACKING
 
-#define MAVLINK_MSG_ID_DATA100 171
-
-typedef struct __mavlink_data100_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[100]; ///< raw data
-} mavlink_data100_t;
-
-#define MAVLINK_MSG_ID_DATA100_LEN 102
-#define MAVLINK_MSG_ID_171_LEN 102
-
-#define MAVLINK_MSG_DATA100_FIELD_DATA_LEN 100
-
-#define MAVLINK_MESSAGE_INFO_DATA100 { \
-	"DATA100", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data100_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data100_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 100, 2, offsetof(mavlink_data100_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data100 message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data100_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[102];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 100);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 102);
-#else
-	mavlink_data100_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*100);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 102);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA100;
-	return mavlink_finalize_message(msg, system_id, component_id, 102, 179);
-}
-
-/**
- * @brief Pack a data100 message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data100_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[102];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 100);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 102);
-#else
-	mavlink_data100_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*100);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 102);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA100;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 102, 179);
-}
-
-/**
- * @brief Encode a data100 struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param data100 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data100_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data100_t* data100)
-{
-	return mavlink_msg_data100_pack(system_id, component_id, msg, data100->type, data100->len, data100->data);
-}
-
-/**
- * @brief Send a data100 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data100_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[102];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 100);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA100, buf, 102, 179);
-#else
-	mavlink_data100_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*100);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA100, (const char *)&packet, 102, 179);
-#endif
-}
-
-#endif
-
-// MESSAGE DATA100 UNPACKING
-
-
-/**
- * @brief Get field type from data100 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data100_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data100 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data100_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data100 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data100_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 100,  2);
-}
-
-/**
- * @brief Decode a data100 message into a struct
- *
- * @param msg The message to decode
- * @param data100 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data100_decode(const mavlink_message_t* msg, mavlink_data100_t* data100)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data100->type = mavlink_msg_data100_get_type(msg);
-	data100->len = mavlink_msg_data100_get_len(msg);
-	mavlink_msg_data100_get_data(msg, data100->data);
-#else
-	memcpy(data100, _MAV_PAYLOAD(msg), 102);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_data16.h
+++ /dev/null
@@ -1,202 +1,1 @@
-// MESSAGE DATA16 PACKING
 
-#define MAVLINK_MSG_ID_DATA16 169
-
-typedef struct __mavlink_data16_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[16]; ///< raw data
-} mavlink_data16_t;
-
-#define MAVLINK_MSG_ID_DATA16_LEN 18
-#define MAVLINK_MSG_ID_169_LEN 18
-
-#define MAVLINK_MSG_ID_DATA16_CRC 234
-#define MAVLINK_MSG_ID_169_CRC 234
-
-#define MAVLINK_MSG_DATA16_FIELD_DATA_LEN 16
-
-#define MAVLINK_MESSAGE_INFO_DATA16 { \
-	"DATA16", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data16_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data16_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 16, 2, offsetof(mavlink_data16_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data16 message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA16_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN);
-#else
-	mavlink_data16_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA16;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data16 message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA16_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA16_LEN);
-#else
-	mavlink_data16_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA16;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data16 struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param data16 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data16_t* data16)
-{
-	return mavlink_msg_data16_pack(system_id, component_id, msg, data16->type, data16->len, data16->data);
-}
-
-/**
- * @brief Send a data16 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA16_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 16);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-#else
-	mavlink_data16_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE DATA16 UNPACKING
-
-
-/**
- * @brief Get field type from data16 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data16_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data16 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data16_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data16 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data16_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 16,  2);
-}
-
-/**
- * @brief Decode a data16 message into a struct
- *
- * @param msg The message to decode
- * @param data16 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data16_decode(const mavlink_message_t* msg, mavlink_data16_t* data16)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data16->type = mavlink_msg_data16_get_type(msg);
-	data16->len = mavlink_msg_data16_get_len(msg);
-	mavlink_msg_data16_get_data(msg, data16->data);
-#else
-	memcpy(data16, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA16_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_data32.h
+++ /dev/null
@@ -1,202 +1,1 @@
-// MESSAGE DATA32 PACKING
 
-#define MAVLINK_MSG_ID_DATA32 170
-
-typedef struct __mavlink_data32_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[32]; ///< raw data
-} mavlink_data32_t;
-
-#define MAVLINK_MSG_ID_DATA32_LEN 34
-#define MAVLINK_MSG_ID_170_LEN 34
-
-#define MAVLINK_MSG_ID_DATA32_CRC 73
-#define MAVLINK_MSG_ID_170_CRC 73
-
-#define MAVLINK_MSG_DATA32_FIELD_DATA_LEN 32
-
-#define MAVLINK_MESSAGE_INFO_DATA32 { \
-	"DATA32", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data32_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data32_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 32, 2, offsetof(mavlink_data32_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data32 message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA32_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN);
-#else
-	mavlink_data32_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA32;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data32 message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA32_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA32_LEN);
-#else
-	mavlink_data32_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA32;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data32 struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param data32 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data32_t* data32)
-{
-	return mavlink_msg_data32_pack(system_id, component_id, msg, data32->type, data32->len, data32->data);
-}
-
-/**
- * @brief Send a data32 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA32_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 32);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-#else
-	mavlink_data32_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE DATA32 UNPACKING
-
-
-/**
- * @brief Get field type from data32 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data32_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data32 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data32_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data32 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data32_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 32,  2);
-}
-
-/**
- * @brief Decode a data32 message into a struct
- *
- * @param msg The message to decode
- * @param data32 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data32_decode(const mavlink_message_t* msg, mavlink_data32_t* data32)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data32->type = mavlink_msg_data32_get_type(msg);
-	data32->len = mavlink_msg_data32_get_len(msg);
-	mavlink_msg_data32_get_data(msg, data32->data);
-#else
-	memcpy(data32, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA32_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_data64.h
+++ /dev/null
@@ -1,202 +1,1 @@
-// MESSAGE DATA64 PACKING
 
-#define MAVLINK_MSG_ID_DATA64 171
-
-typedef struct __mavlink_data64_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[64]; ///< raw data
-} mavlink_data64_t;
-
-#define MAVLINK_MSG_ID_DATA64_LEN 66
-#define MAVLINK_MSG_ID_171_LEN 66
-
-#define MAVLINK_MSG_ID_DATA64_CRC 181
-#define MAVLINK_MSG_ID_171_CRC 181
-
-#define MAVLINK_MSG_DATA64_FIELD_DATA_LEN 64
-
-#define MAVLINK_MESSAGE_INFO_DATA64 { \
-	"DATA64", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data64_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data64_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 2, offsetof(mavlink_data64_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data64 message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA64_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN);
-#else
-	mavlink_data64_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA64;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data64 message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA64_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA64_LEN);
-#else
-	mavlink_data64_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA64;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data64 struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param data64 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data64_t* data64)
-{
-	return mavlink_msg_data64_pack(system_id, component_id, msg, data64->type, data64->len, data64->data);
-}
-
-/**
- * @brief Send a data64 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA64_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 64);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-#else
-	mavlink_data64_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)&packet, MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE DATA64 UNPACKING
-
-
-/**
- * @brief Get field type from data64 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data64_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data64 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data64_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data64 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data64_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 64,  2);
-}
-
-/**
- * @brief Decode a data64 message into a struct
- *
- * @param msg The message to decode
- * @param data64 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data64_decode(const mavlink_message_t* msg, mavlink_data64_t* data64)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data64->type = mavlink_msg_data64_get_type(msg);
-	data64->len = mavlink_msg_data64_get_len(msg);
-	mavlink_msg_data64_get_data(msg, data64->data);
-#else
-	memcpy(data64, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA64_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_data96.h
+++ /dev/null
@@ -1,202 +1,1 @@
-// MESSAGE DATA96 PACKING
 
-#define MAVLINK_MSG_ID_DATA96 172
-
-typedef struct __mavlink_data96_t
-{
- uint8_t type; ///< data type
- uint8_t len; ///< data length
- uint8_t data[96]; ///< raw data
-} mavlink_data96_t;
-
-#define MAVLINK_MSG_ID_DATA96_LEN 98
-#define MAVLINK_MSG_ID_172_LEN 98
-
-#define MAVLINK_MSG_ID_DATA96_CRC 22
-#define MAVLINK_MSG_ID_172_CRC 22
-
-#define MAVLINK_MSG_DATA96_FIELD_DATA_LEN 96
-
-#define MAVLINK_MESSAGE_INFO_DATA96 { \
-	"DATA96", \
-	3, \
-	{  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_data96_t, type) }, \
-         { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_data96_t, len) }, \
-         { "data", NULL, MAVLINK_TYPE_UINT8_T, 96, 2, offsetof(mavlink_data96_t, data) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a data96 message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA96_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN);
-#else
-	mavlink_data96_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA96;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-}
-
-/**
- * @brief Pack a data96 message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param type data type
- * @param len data length
- * @param data raw data
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t type,uint8_t len,const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA96_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA96_LEN);
-#else
-	mavlink_data96_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DATA96;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-}
-
-/**
- * @brief Encode a data96 struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param data96 C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_data96_t* data96)
-{
-	return mavlink_msg_data96_pack(system_id, component_id, msg, data96->type, data96->len, data96->data);
-}
-
-/**
- * @brief Send a data96 message
- * @param chan MAVLink channel to send the message
- *
- * @param type data type
- * @param len data length
- * @param data raw data
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DATA96_LEN];
-	_mav_put_uint8_t(buf, 0, type);
-	_mav_put_uint8_t(buf, 1, len);
-	_mav_put_uint8_t_array(buf, 2, data, 96);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-#else
-	mavlink_data96_t packet;
-	packet.type = type;
-	packet.len = len;
-	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*96);
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)&packet, MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE DATA96 UNPACKING
-
-
-/**
- * @brief Get field type from data96 message
- *
- * @return data type
- */
-static inline uint8_t mavlink_msg_data96_get_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field len from data96 message
- *
- * @return data length
- */
-static inline uint8_t mavlink_msg_data96_get_len(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field data from data96 message
- *
- * @return raw data
- */
-static inline uint16_t mavlink_msg_data96_get_data(const mavlink_message_t* msg, uint8_t *data)
-{
-	return _MAV_RETURN_uint8_t_array(msg, data, 96,  2);
-}
-
-/**
- * @brief Decode a data96 message into a struct
- *
- * @param msg The message to decode
- * @param data96 C-struct to decode the message contents into
- */
-static inline void mavlink_msg_data96_decode(const mavlink_message_t* msg, mavlink_data96_t* data96)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	data96->type = mavlink_msg_data96_get_type(msg);
-	data96->len = mavlink_msg_data96_get_len(msg);
-	mavlink_msg_data96_get_data(msg, data96->data);
-#else
-	memcpy(data96, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DATA96_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_digicam_configure.h
+++ /dev/null
@@ -1,384 +1,1 @@
-// MESSAGE DIGICAM_CONFIGURE PACKING
 
-#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154
-
-typedef struct __mavlink_digicam_configure_t
-{
- float extra_value; ///< Correspondent value to given extra_param
- uint16_t shutter_speed; ///< Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t mode; ///< Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- uint8_t aperture; ///< F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- uint8_t iso; ///< ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- uint8_t exposure_type; ///< Exposure type enumeration from 1 to N (0 means ignore)
- uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- uint8_t engine_cut_off; ///< Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore)
-} mavlink_digicam_configure_t;
-
-#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN 15
-#define MAVLINK_MSG_ID_154_LEN 15
-
-#define MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC 84
-#define MAVLINK_MSG_ID_154_CRC 84
-
-
-
-#define MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE { \
-	"DIGICAM_CONFIGURE", \
-	11, \
-	{  { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_configure_t, extra_value) }, \
-         { "shutter_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_digicam_configure_t, shutter_speed) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_configure_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_configure_t, target_component) }, \
-         { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_digicam_configure_t, mode) }, \
-         { "aperture", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_configure_t, aperture) }, \
-         { "iso", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_configure_t, iso) }, \
-         { "exposure_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_configure_t, exposure_type) }, \
-         { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_configure_t, command_id) }, \
-         { "engine_cut_off", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_digicam_configure_t, engine_cut_off) }, \
-         { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_digicam_configure_t, extra_param) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a digicam_configure message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN];
-	_mav_put_float(buf, 0, extra_value);
-	_mav_put_uint16_t(buf, 4, shutter_speed);
-	_mav_put_uint8_t(buf, 6, target_system);
-	_mav_put_uint8_t(buf, 7, target_component);
-	_mav_put_uint8_t(buf, 8, mode);
-	_mav_put_uint8_t(buf, 9, aperture);
-	_mav_put_uint8_t(buf, 10, iso);
-	_mav_put_uint8_t(buf, 11, exposure_type);
-	_mav_put_uint8_t(buf, 12, command_id);
-	_mav_put_uint8_t(buf, 13, engine_cut_off);
-	_mav_put_uint8_t(buf, 14, extra_param);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#else
-	mavlink_digicam_configure_t packet;
-	packet.extra_value = extra_value;
-	packet.shutter_speed = shutter_speed;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.mode = mode;
-	packet.aperture = aperture;
-	packet.iso = iso;
-	packet.exposure_type = exposure_type;
-	packet.command_id = command_id;
-	packet.engine_cut_off = engine_cut_off;
-	packet.extra_param = extra_param;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a digicam_configure message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t mode,uint16_t shutter_speed,uint8_t aperture,uint8_t iso,uint8_t exposure_type,uint8_t command_id,uint8_t engine_cut_off,uint8_t extra_param,float extra_value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN];
-	_mav_put_float(buf, 0, extra_value);
-	_mav_put_uint16_t(buf, 4, shutter_speed);
-	_mav_put_uint8_t(buf, 6, target_system);
-	_mav_put_uint8_t(buf, 7, target_component);
-	_mav_put_uint8_t(buf, 8, mode);
-	_mav_put_uint8_t(buf, 9, aperture);
-	_mav_put_uint8_t(buf, 10, iso);
-	_mav_put_uint8_t(buf, 11, exposure_type);
-	_mav_put_uint8_t(buf, 12, command_id);
-	_mav_put_uint8_t(buf, 13, engine_cut_off);
-	_mav_put_uint8_t(buf, 14, extra_param);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#else
-	mavlink_digicam_configure_t packet;
-	packet.extra_value = extra_value;
-	packet.shutter_speed = shutter_speed;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.mode = mode;
-	packet.aperture = aperture;
-	packet.iso = iso;
-	packet.exposure_type = exposure_type;
-	packet.command_id = command_id;
-	packet.engine_cut_off = engine_cut_off;
-	packet.extra_param = extra_param;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a digicam_configure struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param digicam_configure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure)
-{
-	return mavlink_msg_digicam_configure_pack(system_id, component_id, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value);
-}
-
-/**
- * @brief Send a digicam_configure message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mode Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- * @param shutter_speed Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- * @param aperture F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- * @param iso ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- * @param exposure_type Exposure type enumeration from 1 to N (0 means ignore)
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param engine_cut_off Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN];
-	_mav_put_float(buf, 0, extra_value);
-	_mav_put_uint16_t(buf, 4, shutter_speed);
-	_mav_put_uint8_t(buf, 6, target_system);
-	_mav_put_uint8_t(buf, 7, target_component);
-	_mav_put_uint8_t(buf, 8, mode);
-	_mav_put_uint8_t(buf, 9, aperture);
-	_mav_put_uint8_t(buf, 10, iso);
-	_mav_put_uint8_t(buf, 11, exposure_type);
-	_mav_put_uint8_t(buf, 12, command_id);
-	_mav_put_uint8_t(buf, 13, engine_cut_off);
-	_mav_put_uint8_t(buf, 14, extra_param);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-#else
-	mavlink_digicam_configure_t packet;
-	packet.extra_value = extra_value;
-	packet.shutter_speed = shutter_speed;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.mode = mode;
-	packet.aperture = aperture;
-	packet.iso = iso;
-	packet.exposure_type = exposure_type;
-	packet.command_id = command_id;
-	packet.engine_cut_off = engine_cut_off;
-	packet.extra_param = extra_param;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE DIGICAM_CONFIGURE UNPACKING
-
-
-/**
- * @brief Get field target_system from digicam_configure message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field target_component from digicam_configure message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field mode from digicam_configure message
- *
- * @return Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field shutter_speed from digicam_configure message
- *
- * @return Divisor number //e.g. 1000 means 1/1000 (0 means ignore)
- */
-static inline uint16_t mavlink_msg_digicam_configure_get_shutter_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field aperture from digicam_configure message
- *
- * @return F stop number x 10 //e.g. 28 means 2.8 (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_aperture(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field iso from digicam_configure message
- *
- * @return ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_iso(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field exposure_type from digicam_configure message
- *
- * @return Exposure type enumeration from 1 to N (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_exposure_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field command_id from digicam_configure message
- *
- * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_command_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field engine_cut_off from digicam_configure message
- *
- * @return Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_engine_cut_off(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Get field extra_param from digicam_configure message
- *
- * @return Extra parameters enumeration (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_configure_get_extra_param(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Get field extra_value from digicam_configure message
- *
- * @return Correspondent value to given extra_param
- */
-static inline float mavlink_msg_digicam_configure_get_extra_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a digicam_configure message into a struct
- *
- * @param msg The message to decode
- * @param digicam_configure C-struct to decode the message contents into
- */
-static inline void mavlink_msg_digicam_configure_decode(const mavlink_message_t* msg, mavlink_digicam_configure_t* digicam_configure)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	digicam_configure->extra_value = mavlink_msg_digicam_configure_get_extra_value(msg);
-	digicam_configure->shutter_speed = mavlink_msg_digicam_configure_get_shutter_speed(msg);
-	digicam_configure->target_system = mavlink_msg_digicam_configure_get_target_system(msg);
-	digicam_configure->target_component = mavlink_msg_digicam_configure_get_target_component(msg);
-	digicam_configure->mode = mavlink_msg_digicam_configure_get_mode(msg);
-	digicam_configure->aperture = mavlink_msg_digicam_configure_get_aperture(msg);
-	digicam_configure->iso = mavlink_msg_digicam_configure_get_iso(msg);
-	digicam_configure->exposure_type = mavlink_msg_digicam_configure_get_exposure_type(msg);
-	digicam_configure->command_id = mavlink_msg_digicam_configure_get_command_id(msg);
-	digicam_configure->engine_cut_off = mavlink_msg_digicam_configure_get_engine_cut_off(msg);
-	digicam_configure->extra_param = mavlink_msg_digicam_configure_get_extra_param(msg);
-#else
-	memcpy(digicam_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_digicam_control.h
+++ /dev/null
@@ -1,362 +1,1 @@
-// MESSAGE DIGICAM_CONTROL PACKING
 
-#define MAVLINK_MSG_ID_DIGICAM_CONTROL 155
-
-typedef struct __mavlink_digicam_control_t
-{
- float extra_value; ///< Correspondent value to given extra_param
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t session; ///< 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- uint8_t zoom_pos; ///< 1 to N //Zoom's absolute position (0 means ignore)
- int8_t zoom_step; ///< -100 to 100 //Zooming step value to offset zoom from the current position
- uint8_t focus_lock; ///< 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- uint8_t shot; ///< 0: ignore, 1: shot or start filming
- uint8_t command_id; ///< Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- uint8_t extra_param; ///< Extra parameters enumeration (0 means ignore)
-} mavlink_digicam_control_t;
-
-#define MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN 13
-#define MAVLINK_MSG_ID_155_LEN 13
-
-#define MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC 22
-#define MAVLINK_MSG_ID_155_CRC 22
-
-
-
-#define MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL { \
-	"DIGICAM_CONTROL", \
-	10, \
-	{  { "extra_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_digicam_control_t, extra_value) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_digicam_control_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_digicam_control_t, target_component) }, \
-         { "session", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_digicam_control_t, session) }, \
-         { "zoom_pos", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_digicam_control_t, zoom_pos) }, \
-         { "zoom_step", NULL, MAVLINK_TYPE_INT8_T, 0, 8, offsetof(mavlink_digicam_control_t, zoom_step) }, \
-         { "focus_lock", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_digicam_control_t, focus_lock) }, \
-         { "shot", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_digicam_control_t, shot) }, \
-         { "command_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_digicam_control_t, command_id) }, \
-         { "extra_param", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_digicam_control_t, extra_param) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a digicam_control message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)
- * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position
- * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- * @param shot 0: ignore, 1: shot or start filming
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN];
-	_mav_put_float(buf, 0, extra_value);
-	_mav_put_uint8_t(buf, 4, target_system);
-	_mav_put_uint8_t(buf, 5, target_component);
-	_mav_put_uint8_t(buf, 6, session);
-	_mav_put_uint8_t(buf, 7, zoom_pos);
-	_mav_put_int8_t(buf, 8, zoom_step);
-	_mav_put_uint8_t(buf, 9, focus_lock);
-	_mav_put_uint8_t(buf, 10, shot);
-	_mav_put_uint8_t(buf, 11, command_id);
-	_mav_put_uint8_t(buf, 12, extra_param);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#else
-	mavlink_digicam_control_t packet;
-	packet.extra_value = extra_value;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.session = session;
-	packet.zoom_pos = zoom_pos;
-	packet.zoom_step = zoom_step;
-	packet.focus_lock = focus_lock;
-	packet.shot = shot;
-	packet.command_id = command_id;
-	packet.extra_param = extra_param;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a digicam_control message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)
- * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position
- * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- * @param shot 0: ignore, 1: shot or start filming
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t session,uint8_t zoom_pos,int8_t zoom_step,uint8_t focus_lock,uint8_t shot,uint8_t command_id,uint8_t extra_param,float extra_value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN];
-	_mav_put_float(buf, 0, extra_value);
-	_mav_put_uint8_t(buf, 4, target_system);
-	_mav_put_uint8_t(buf, 5, target_component);
-	_mav_put_uint8_t(buf, 6, session);
-	_mav_put_uint8_t(buf, 7, zoom_pos);
-	_mav_put_int8_t(buf, 8, zoom_step);
-	_mav_put_uint8_t(buf, 9, focus_lock);
-	_mav_put_uint8_t(buf, 10, shot);
-	_mav_put_uint8_t(buf, 11, command_id);
-	_mav_put_uint8_t(buf, 12, extra_param);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#else
-	mavlink_digicam_control_t packet;
-	packet.extra_value = extra_value;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.session = session;
-	packet.zoom_pos = zoom_pos;
-	packet.zoom_step = zoom_step;
-	packet.focus_lock = focus_lock;
-	packet.shot = shot;
-	packet.command_id = command_id;
-	packet.extra_param = extra_param;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_DIGICAM_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a digicam_control struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param digicam_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control)
-{
-	return mavlink_msg_digicam_control_pack(system_id, component_id, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value);
-}
-
-/**
- * @brief Send a digicam_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param session 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- * @param zoom_pos 1 to N //Zoom's absolute position (0 means ignore)
- * @param zoom_step -100 to 100 //Zooming step value to offset zoom from the current position
- * @param focus_lock 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- * @param shot 0: ignore, 1: shot or start filming
- * @param command_id Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- * @param extra_param Extra parameters enumeration (0 means ignore)
- * @param extra_value Correspondent value to given extra_param
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN];
-	_mav_put_float(buf, 0, extra_value);
-	_mav_put_uint8_t(buf, 4, target_system);
-	_mav_put_uint8_t(buf, 5, target_component);
-	_mav_put_uint8_t(buf, 6, session);
-	_mav_put_uint8_t(buf, 7, zoom_pos);
-	_mav_put_int8_t(buf, 8, zoom_step);
-	_mav_put_uint8_t(buf, 9, focus_lock);
-	_mav_put_uint8_t(buf, 10, shot);
-	_mav_put_uint8_t(buf, 11, command_id);
-	_mav_put_uint8_t(buf, 12, extra_param);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-#else
-	mavlink_digicam_control_t packet;
-	packet.extra_value = extra_value;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.session = session;
-	packet.zoom_pos = zoom_pos;
-	packet.zoom_step = zoom_step;
-	packet.focus_lock = focus_lock;
-	packet.shot = shot;
-	packet.command_id = command_id;
-	packet.extra_param = extra_param;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE DIGICAM_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target_system from digicam_control message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_digicam_control_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field target_component from digicam_control message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_digicam_control_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field session from digicam_control message
- *
- * @return 0: stop, 1: start or keep it up //Session control e.g. show/hide lens
- */
-static inline uint8_t mavlink_msg_digicam_control_get_session(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field zoom_pos from digicam_control message
- *
- * @return 1 to N //Zoom's absolute position (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_control_get_zoom_pos(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field zoom_step from digicam_control message
- *
- * @return -100 to 100 //Zooming step value to offset zoom from the current position
- */
-static inline int8_t mavlink_msg_digicam_control_get_zoom_step(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int8_t(msg,  8);
-}
-
-/**
- * @brief Get field focus_lock from digicam_control message
- *
- * @return 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus
- */
-static inline uint8_t mavlink_msg_digicam_control_get_focus_lock(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field shot from digicam_control message
- *
- * @return 0: ignore, 1: shot or start filming
- */
-static inline uint8_t mavlink_msg_digicam_control_get_shot(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field command_id from digicam_control message
- *
- * @return Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once
- */
-static inline uint8_t mavlink_msg_digicam_control_get_command_id(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field extra_param from digicam_control message
- *
- * @return Extra parameters enumeration (0 means ignore)
- */
-static inline uint8_t mavlink_msg_digicam_control_get_extra_param(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field extra_value from digicam_control message
- *
- * @return Correspondent value to given extra_param
- */
-static inline float mavlink_msg_digicam_control_get_extra_value(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Decode a digicam_control message into a struct
- *
- * @param msg The message to decode
- * @param digicam_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_digicam_control_decode(const mavlink_message_t* msg, mavlink_digicam_control_t* digicam_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	digicam_control->extra_value = mavlink_msg_digicam_control_get_extra_value(msg);
-	digicam_control->target_system = mavlink_msg_digicam_control_get_target_system(msg);
-	digicam_control->target_component = mavlink_msg_digicam_control_get_target_component(msg);
-	digicam_control->session = mavlink_msg_digicam_control_get_session(msg);
-	digicam_control->zoom_pos = mavlink_msg_digicam_control_get_zoom_pos(msg);
-	digicam_control->zoom_step = mavlink_msg_digicam_control_get_zoom_step(msg);
-	digicam_control->focus_lock = mavlink_msg_digicam_control_get_focus_lock(msg);
-	digicam_control->shot = mavlink_msg_digicam_control_get_shot(msg);
-	digicam_control->command_id = mavlink_msg_digicam_control_get_command_id(msg);
-	digicam_control->extra_param = mavlink_msg_digicam_control_get_extra_param(msg);
-#else
-	memcpy(digicam_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_fence_fetch_point.h
+++ /dev/null
@@ -1,208 +1,1 @@
-// MESSAGE FENCE_FETCH_POINT PACKING
 
-#define MAVLINK_MSG_ID_FENCE_FETCH_POINT 161
-
-typedef struct __mavlink_fence_fetch_point_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t idx; ///< point index (first point is 1, 0 is for return point)
-} mavlink_fence_fetch_point_t;
-
-#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN 3
-#define MAVLINK_MSG_ID_161_LEN 3
-
-#define MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC 68
-#define MAVLINK_MSG_ID_161_CRC 68
-
-
-
-#define MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT { \
-	"FENCE_FETCH_POINT", \
-	3, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fence_fetch_point_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fence_fetch_point_t, target_component) }, \
-         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_fence_fetch_point_t, idx) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a fence_fetch_point message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t idx)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, idx);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#else
-	mavlink_fence_fetch_point_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a fence_fetch_point message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t idx)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, idx);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#else
-	mavlink_fence_fetch_point_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_FETCH_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a fence_fetch_point struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param fence_fetch_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point)
-{
-	return mavlink_msg_fence_fetch_point_pack(system_id, component_id, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx);
-}
-
-/**
- * @brief Send a fence_fetch_point message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, idx);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-#else
-	mavlink_fence_fetch_point_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE FENCE_FETCH_POINT UNPACKING
-
-
-/**
- * @brief Get field target_system from fence_fetch_point message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_fence_fetch_point_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from fence_fetch_point message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_fence_fetch_point_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field idx from fence_fetch_point message
- *
- * @return point index (first point is 1, 0 is for return point)
- */
-static inline uint8_t mavlink_msg_fence_fetch_point_get_idx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a fence_fetch_point message into a struct
- *
- * @param msg The message to decode
- * @param fence_fetch_point C-struct to decode the message contents into
- */
-static inline void mavlink_msg_fence_fetch_point_decode(const mavlink_message_t* msg, mavlink_fence_fetch_point_t* fence_fetch_point)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	fence_fetch_point->target_system = mavlink_msg_fence_fetch_point_get_target_system(msg);
-	fence_fetch_point->target_component = mavlink_msg_fence_fetch_point_get_target_component(msg);
-	fence_fetch_point->idx = mavlink_msg_fence_fetch_point_get_idx(msg);
-#else
-	memcpy(fence_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_fence_point.h
+++ /dev/null
@@ -1,274 +1,1 @@
-// MESSAGE FENCE_POINT PACKING
 
-#define MAVLINK_MSG_ID_FENCE_POINT 160
-
-typedef struct __mavlink_fence_point_t
-{
- float lat; ///< Latitude of point
- float lng; ///< Longitude of point
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t idx; ///< point index (first point is 1, 0 is for return point)
- uint8_t count; ///< total number of points (for sanity checking)
-} mavlink_fence_point_t;
-
-#define MAVLINK_MSG_ID_FENCE_POINT_LEN 12
-#define MAVLINK_MSG_ID_160_LEN 12
-
-#define MAVLINK_MSG_ID_FENCE_POINT_CRC 78
-#define MAVLINK_MSG_ID_160_CRC 78
-
-
-
-#define MAVLINK_MESSAGE_INFO_FENCE_POINT { \
-	"FENCE_POINT", \
-	6, \
-	{  { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fence_point_t, lat) }, \
-         { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fence_point_t, lng) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_point_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_fence_point_t, target_component) }, \
-         { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_fence_point_t, idx) }, \
-         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_fence_point_t, count) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a fence_point message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point
- * @param lng Longitude of point
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN];
-	_mav_put_float(buf, 0, lat);
-	_mav_put_float(buf, 4, lng);
-	_mav_put_uint8_t(buf, 8, target_system);
-	_mav_put_uint8_t(buf, 9, target_component);
-	_mav_put_uint8_t(buf, 10, idx);
-	_mav_put_uint8_t(buf, 11, count);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#else
-	mavlink_fence_point_t packet;
-	packet.lat = lat;
-	packet.lng = lng;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-	packet.count = count;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Pack a fence_point message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point
- * @param lng Longitude of point
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,float lat,float lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN];
-	_mav_put_float(buf, 0, lat);
-	_mav_put_float(buf, 4, lng);
-	_mav_put_uint8_t(buf, 8, target_system);
-	_mav_put_uint8_t(buf, 9, target_component);
-	_mav_put_uint8_t(buf, 10, idx);
-	_mav_put_uint8_t(buf, 11, count);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#else
-	mavlink_fence_point_t packet;
-	packet.lat = lat;
-	packet.lng = lng;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-	packet.count = count;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_POINT;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-}
-
-/**
- * @brief Encode a fence_point struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param fence_point C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point)
-{
-	return mavlink_msg_fence_point_pack(system_id, component_id, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng);
-}
-
-/**
- * @brief Send a fence_point message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param idx point index (first point is 1, 0 is for return point)
- * @param count total number of points (for sanity checking)
- * @param lat Latitude of point
- * @param lng Longitude of point
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_POINT_LEN];
-	_mav_put_float(buf, 0, lat);
-	_mav_put_float(buf, 4, lng);
-	_mav_put_uint8_t(buf, 8, target_system);
-	_mav_put_uint8_t(buf, 9, target_component);
-	_mav_put_uint8_t(buf, 10, idx);
-	_mav_put_uint8_t(buf, 11, count);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-#else
-	mavlink_fence_point_t packet;
-	packet.lat = lat;
-	packet.lng = lng;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.idx = idx;
-	packet.count = count;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)&packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE FENCE_POINT UNPACKING
-
-
-/**
- * @brief Get field target_system from fence_point message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_fence_point_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field target_component from fence_point message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_fence_point_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  9);
-}
-
-/**
- * @brief Get field idx from fence_point message
- *
- * @return point index (first point is 1, 0 is for return point)
- */
-static inline uint8_t mavlink_msg_fence_point_get_idx(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  10);
-}
-
-/**
- * @brief Get field count from fence_point message
- *
- * @return total number of points (for sanity checking)
- */
-static inline uint8_t mavlink_msg_fence_point_get_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  11);
-}
-
-/**
- * @brief Get field lat from fence_point message
- *
- * @return Latitude of point
- */
-static inline float mavlink_msg_fence_point_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field lng from fence_point message
- *
- * @return Longitude of point
- */
-static inline float mavlink_msg_fence_point_get_lng(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Decode a fence_point message into a struct
- *
- * @param msg The message to decode
- * @param fence_point C-struct to decode the message contents into
- */
-static inline void mavlink_msg_fence_point_decode(const mavlink_message_t* msg, mavlink_fence_point_t* fence_point)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	fence_point->lat = mavlink_msg_fence_point_get_lat(msg);
-	fence_point->lng = mavlink_msg_fence_point_get_lng(msg);
-	fence_point->target_system = mavlink_msg_fence_point_get_target_system(msg);
-	fence_point->target_component = mavlink_msg_fence_point_get_target_component(msg);
-	fence_point->idx = mavlink_msg_fence_point_get_idx(msg);
-	fence_point->count = mavlink_msg_fence_point_get_count(msg);
-#else
-	memcpy(fence_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_POINT_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_fence_status.h
+++ /dev/null
@@ -1,230 +1,1 @@
-// MESSAGE FENCE_STATUS PACKING
 
-#define MAVLINK_MSG_ID_FENCE_STATUS 162
-
-typedef struct __mavlink_fence_status_t
-{
- uint32_t breach_time; ///< time of last breach in milliseconds since boot
- uint16_t breach_count; ///< number of fence breaches
- uint8_t breach_status; ///< 0 if currently inside fence, 1 if outside
- uint8_t breach_type; ///< last breach type (see FENCE_BREACH_* enum)
-} mavlink_fence_status_t;
-
-#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8
-#define MAVLINK_MSG_ID_162_LEN 8
-
-#define MAVLINK_MSG_ID_FENCE_STATUS_CRC 189
-#define MAVLINK_MSG_ID_162_CRC 189
-
-
-
-#define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \
-	"FENCE_STATUS", \
-	4, \
-	{  { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \
-         { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \
-         { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \
-         { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a fence_status message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param breach_status 0 if currently inside fence, 1 if outside
- * @param breach_count number of fence breaches
- * @param breach_type last breach type (see FENCE_BREACH_* enum)
- * @param breach_time time of last breach in milliseconds since boot
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN];
-	_mav_put_uint32_t(buf, 0, breach_time);
-	_mav_put_uint16_t(buf, 4, breach_count);
-	_mav_put_uint8_t(buf, 6, breach_status);
-	_mav_put_uint8_t(buf, 7, breach_type);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#else
-	mavlink_fence_status_t packet;
-	packet.breach_time = breach_time;
-	packet.breach_count = breach_count;
-	packet.breach_status = breach_status;
-	packet.breach_type = breach_type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a fence_status message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param breach_status 0 if currently inside fence, 1 if outside
- * @param breach_count number of fence breaches
- * @param breach_type last breach type (see FENCE_BREACH_* enum)
- * @param breach_time time of last breach in milliseconds since boot
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN];
-	_mav_put_uint32_t(buf, 0, breach_time);
-	_mav_put_uint16_t(buf, 4, breach_count);
-	_mav_put_uint8_t(buf, 6, breach_status);
-	_mav_put_uint8_t(buf, 7, breach_type);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#else
-	mavlink_fence_status_t packet;
-	packet.breach_time = breach_time;
-	packet.breach_count = breach_count;
-	packet.breach_status = breach_status;
-	packet.breach_type = breach_type;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a fence_status struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param fence_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status)
-{
-	return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time);
-}
-
-/**
- * @brief Send a fence_status message
- * @param chan MAVLink channel to send the message
- *
- * @param breach_status 0 if currently inside fence, 1 if outside
- * @param breach_count number of fence breaches
- * @param breach_type last breach type (see FENCE_BREACH_* enum)
- * @param breach_time time of last breach in milliseconds since boot
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN];
-	_mav_put_uint32_t(buf, 0, breach_time);
-	_mav_put_uint16_t(buf, 4, breach_count);
-	_mav_put_uint8_t(buf, 6, breach_status);
-	_mav_put_uint8_t(buf, 7, breach_type);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-#else
-	mavlink_fence_status_t packet;
-	packet.breach_time = breach_time;
-	packet.breach_count = breach_count;
-	packet.breach_status = breach_status;
-	packet.breach_type = breach_type;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE FENCE_STATUS UNPACKING
-
-
-/**
- * @brief Get field breach_status from fence_status message
- *
- * @return 0 if currently inside fence, 1 if outside
- */
-static inline uint8_t mavlink_msg_fence_status_get_breach_status(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field breach_count from fence_status message
- *
- * @return number of fence breaches
- */
-static inline uint16_t mavlink_msg_fence_status_get_breach_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  4);
-}
-
-/**
- * @brief Get field breach_type from fence_status message
- *
- * @return last breach type (see FENCE_BREACH_* enum)
- */
-static inline uint8_t mavlink_msg_fence_status_get_breach_type(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field breach_time from fence_status message
- *
- * @return time of last breach in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Decode a fence_status message into a struct
- *
- * @param msg The message to decode
- * @param fence_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, mavlink_fence_status_t* fence_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	fence_status->breach_time = mavlink_msg_fence_status_get_breach_time(msg);
-	fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg);
-	fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg);
-	fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg);
-#else
-	memcpy(fence_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_FENCE_STATUS_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_hwstatus.h
+++ /dev/null
@@ -1,186 +1,1 @@
-// MESSAGE HWSTATUS PACKING
 
-#define MAVLINK_MSG_ID_HWSTATUS 165
-
-typedef struct __mavlink_hwstatus_t
-{
- uint16_t Vcc; ///< board voltage (mV)
- uint8_t I2Cerr; ///< I2C error count
-} mavlink_hwstatus_t;
-
-#define MAVLINK_MSG_ID_HWSTATUS_LEN 3
-#define MAVLINK_MSG_ID_165_LEN 3
-
-#define MAVLINK_MSG_ID_HWSTATUS_CRC 21
-#define MAVLINK_MSG_ID_165_CRC 21
-
-
-
-#define MAVLINK_MESSAGE_INFO_HWSTATUS { \
-	"HWSTATUS", \
-	2, \
-	{  { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_hwstatus_t, Vcc) }, \
-         { "I2Cerr", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_hwstatus_t, I2Cerr) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a hwstatus message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param Vcc board voltage (mV)
- * @param I2Cerr I2C error count
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t Vcc, uint8_t I2Cerr)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
-	_mav_put_uint16_t(buf, 0, Vcc);
-	_mav_put_uint8_t(buf, 2, I2Cerr);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#else
-	mavlink_hwstatus_t packet;
-	packet.Vcc = Vcc;
-	packet.I2Cerr = I2Cerr;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a hwstatus message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param Vcc board voltage (mV)
- * @param I2Cerr I2C error count
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t Vcc,uint8_t I2Cerr)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
-	_mav_put_uint16_t(buf, 0, Vcc);
-	_mav_put_uint8_t(buf, 2, I2Cerr);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#else
-	mavlink_hwstatus_t packet;
-	packet.Vcc = Vcc;
-	packet.I2Cerr = I2Cerr;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_HWSTATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a hwstatus struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param hwstatus C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus)
-{
-	return mavlink_msg_hwstatus_pack(system_id, component_id, msg, hwstatus->Vcc, hwstatus->I2Cerr);
-}
-
-/**
- * @brief Send a hwstatus message
- * @param chan MAVLink channel to send the message
- *
- * @param Vcc board voltage (mV)
- * @param I2Cerr I2C error count
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
-	_mav_put_uint16_t(buf, 0, Vcc);
-	_mav_put_uint8_t(buf, 2, I2Cerr);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-#else
-	mavlink_hwstatus_t packet;
-	packet.Vcc = Vcc;
-	packet.I2Cerr = I2Cerr;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE HWSTATUS UNPACKING
-
-
-/**
- * @brief Get field Vcc from hwstatus message
- *
- * @return board voltage (mV)
- */
-static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field I2Cerr from hwstatus message
- *
- * @return I2C error count
- */
-static inline uint8_t mavlink_msg_hwstatus_get_I2Cerr(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Decode a hwstatus message into a struct
- *
- * @param msg The message to decode
- * @param hwstatus C-struct to decode the message contents into
- */
-static inline void mavlink_msg_hwstatus_decode(const mavlink_message_t* msg, mavlink_hwstatus_t* hwstatus)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	hwstatus->Vcc = mavlink_msg_hwstatus_get_Vcc(msg);
-	hwstatus->I2Cerr = mavlink_msg_hwstatus_get_I2Cerr(msg);
-#else
-	memcpy(hwstatus, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HWSTATUS_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_limits_status.h
+++ /dev/null
@@ -1,340 +1,1 @@
-// MESSAGE LIMITS_STATUS PACKING
 
-#define MAVLINK_MSG_ID_LIMITS_STATUS 167
-
-typedef struct __mavlink_limits_status_t
-{
- uint32_t last_trigger; ///< time of last breach in milliseconds since boot
- uint32_t last_action; ///< time of last recovery action in milliseconds since boot
- uint32_t last_recovery; ///< time of last successful recovery in milliseconds since boot
- uint32_t last_clear; ///< time of last all-clear in milliseconds since boot
- uint16_t breach_count; ///< number of fence breaches
- uint8_t limits_state; ///< state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- uint8_t mods_enabled; ///< AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- uint8_t mods_required; ///< AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- uint8_t mods_triggered; ///< AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
-} mavlink_limits_status_t;
-
-#define MAVLINK_MSG_ID_LIMITS_STATUS_LEN 22
-#define MAVLINK_MSG_ID_167_LEN 22
-
-#define MAVLINK_MSG_ID_LIMITS_STATUS_CRC 144
-#define MAVLINK_MSG_ID_167_CRC 144
-
-
-
-#define MAVLINK_MESSAGE_INFO_LIMITS_STATUS { \
-	"LIMITS_STATUS", \
-	9, \
-	{  { "last_trigger", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_limits_status_t, last_trigger) }, \
-         { "last_action", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_limits_status_t, last_action) }, \
-         { "last_recovery", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_limits_status_t, last_recovery) }, \
-         { "last_clear", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_limits_status_t, last_clear) }, \
-         { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_limits_status_t, breach_count) }, \
-         { "limits_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_limits_status_t, limits_state) }, \
-         { "mods_enabled", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_limits_status_t, mods_enabled) }, \
-         { "mods_required", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_limits_status_t, mods_required) }, \
-         { "mods_triggered", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_limits_status_t, mods_triggered) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a limits_status message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- * @param last_trigger time of last breach in milliseconds since boot
- * @param last_action time of last recovery action in milliseconds since boot
- * @param last_recovery time of last successful recovery in milliseconds since boot
- * @param last_clear time of last all-clear in milliseconds since boot
- * @param breach_count number of fence breaches
- * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN];
-	_mav_put_uint32_t(buf, 0, last_trigger);
-	_mav_put_uint32_t(buf, 4, last_action);
-	_mav_put_uint32_t(buf, 8, last_recovery);
-	_mav_put_uint32_t(buf, 12, last_clear);
-	_mav_put_uint16_t(buf, 16, breach_count);
-	_mav_put_uint8_t(buf, 18, limits_state);
-	_mav_put_uint8_t(buf, 19, mods_enabled);
-	_mav_put_uint8_t(buf, 20, mods_required);
-	_mav_put_uint8_t(buf, 21, mods_triggered);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#else
-	mavlink_limits_status_t packet;
-	packet.last_trigger = last_trigger;
-	packet.last_action = last_action;
-	packet.last_recovery = last_recovery;
-	packet.last_clear = last_clear;
-	packet.breach_count = breach_count;
-	packet.limits_state = limits_state;
-	packet.mods_enabled = mods_enabled;
-	packet.mods_required = mods_required;
-	packet.mods_triggered = mods_triggered;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a limits_status message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- * @param last_trigger time of last breach in milliseconds since boot
- * @param last_action time of last recovery action in milliseconds since boot
- * @param last_recovery time of last successful recovery in milliseconds since boot
- * @param last_clear time of last all-clear in milliseconds since boot
- * @param breach_count number of fence breaches
- * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t limits_state,uint32_t last_trigger,uint32_t last_action,uint32_t last_recovery,uint32_t last_clear,uint16_t breach_count,uint8_t mods_enabled,uint8_t mods_required,uint8_t mods_triggered)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN];
-	_mav_put_uint32_t(buf, 0, last_trigger);
-	_mav_put_uint32_t(buf, 4, last_action);
-	_mav_put_uint32_t(buf, 8, last_recovery);
-	_mav_put_uint32_t(buf, 12, last_clear);
-	_mav_put_uint16_t(buf, 16, breach_count);
-	_mav_put_uint8_t(buf, 18, limits_state);
-	_mav_put_uint8_t(buf, 19, mods_enabled);
-	_mav_put_uint8_t(buf, 20, mods_required);
-	_mav_put_uint8_t(buf, 21, mods_triggered);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#else
-	mavlink_limits_status_t packet;
-	packet.last_trigger = last_trigger;
-	packet.last_action = last_action;
-	packet.last_recovery = last_recovery;
-	packet.last_clear = last_clear;
-	packet.breach_count = breach_count;
-	packet.limits_state = limits_state;
-	packet.mods_enabled = mods_enabled;
-	packet.mods_required = mods_required;
-	packet.mods_triggered = mods_triggered;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a limits_status struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param limits_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status)
-{
-	return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered);
-}
-
-/**
- * @brief Send a limits_status message
- * @param chan MAVLink channel to send the message
- *
- * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- * @param last_trigger time of last breach in milliseconds since boot
- * @param last_action time of last recovery action in milliseconds since boot
- * @param last_recovery time of last successful recovery in milliseconds since boot
- * @param last_clear time of last all-clear in milliseconds since boot
- * @param breach_count number of fence breaches
- * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_LIMITS_STATUS_LEN];
-	_mav_put_uint32_t(buf, 0, last_trigger);
-	_mav_put_uint32_t(buf, 4, last_action);
-	_mav_put_uint32_t(buf, 8, last_recovery);
-	_mav_put_uint32_t(buf, 12, last_clear);
-	_mav_put_uint16_t(buf, 16, breach_count);
-	_mav_put_uint8_t(buf, 18, limits_state);
-	_mav_put_uint8_t(buf, 19, mods_enabled);
-	_mav_put_uint8_t(buf, 20, mods_required);
-	_mav_put_uint8_t(buf, 21, mods_triggered);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-#else
-	mavlink_limits_status_t packet;
-	packet.last_trigger = last_trigger;
-	packet.last_action = last_action;
-	packet.last_recovery = last_recovery;
-	packet.last_clear = last_clear;
-	packet.breach_count = breach_count;
-	packet.limits_state = limits_state;
-	packet.mods_enabled = mods_enabled;
-	packet.mods_required = mods_required;
-	packet.mods_triggered = mods_triggered;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE LIMITS_STATUS UNPACKING
-
-
-/**
- * @brief Get field limits_state from limits_status message
- *
- * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  18);
-}
-
-/**
- * @brief Get field last_trigger from limits_status message
- *
- * @return time of last breach in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field last_action from limits_status message
- *
- * @return time of last recovery action in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  4);
-}
-
-/**
- * @brief Get field last_recovery from limits_status message
- *
- * @return time of last successful recovery in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  8);
-}
-
-/**
- * @brief Get field last_clear from limits_status message
- *
- * @return time of last all-clear in milliseconds since boot
- */
-static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  12);
-}
-
-/**
- * @brief Get field breach_count from limits_status message
- *
- * @return number of fence breaches
- */
-static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  16);
-}
-
-/**
- * @brief Get field mods_enabled from limits_status message
- *
- * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  19);
-}
-
-/**
- * @brief Get field mods_required from limits_status message
- *
- * @return AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_mods_required(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  20);
-}
-
-/**
- * @brief Get field mods_triggered from limits_status message
- *
- * @return AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE)
- */
-static inline uint8_t mavlink_msg_limits_status_get_mods_triggered(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  21);
-}
-
-/**
- * @brief Decode a limits_status message into a struct
- *
- * @param msg The message to decode
- * @param limits_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* msg, mavlink_limits_status_t* limits_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	limits_status->last_trigger = mavlink_msg_limits_status_get_last_trigger(msg);
-	limits_status->last_action = mavlink_msg_limits_status_get_last_action(msg);
-	limits_status->last_recovery = mavlink_msg_limits_status_get_last_recovery(msg);
-	limits_status->last_clear = mavlink_msg_limits_status_get_last_clear(msg);
-	limits_status->breach_count = mavlink_msg_limits_status_get_breach_count(msg);
-	limits_status->limits_state = mavlink_msg_limits_status_get_limits_state(msg);
-	limits_status->mods_enabled = mavlink_msg_limits_status_get_mods_enabled(msg);
-	limits_status->mods_required = mavlink_msg_limits_status_get_mods_required(msg);
-	limits_status->mods_triggered = mavlink_msg_limits_status_get_mods_triggered(msg);
-#else
-	memcpy(limits_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_meminfo.h
+++ /dev/null
@@ -1,186 +1,1 @@
-// MESSAGE MEMINFO PACKING
 
-#define MAVLINK_MSG_ID_MEMINFO 152
-
-typedef struct __mavlink_meminfo_t
-{
- uint16_t brkval; ///< heap top
- uint16_t freemem; ///< free memory
-} mavlink_meminfo_t;
-
-#define MAVLINK_MSG_ID_MEMINFO_LEN 4
-#define MAVLINK_MSG_ID_152_LEN 4
-
-#define MAVLINK_MSG_ID_MEMINFO_CRC 208
-#define MAVLINK_MSG_ID_152_CRC 208
-
-
-
-#define MAVLINK_MESSAGE_INFO_MEMINFO { \
-	"MEMINFO", \
-	2, \
-	{  { "brkval", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_meminfo_t, brkval) }, \
-         { "freemem", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_meminfo_t, freemem) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a meminfo message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param brkval heap top
- * @param freemem free memory
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint16_t brkval, uint16_t freemem)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMINFO_LEN];
-	_mav_put_uint16_t(buf, 0, brkval);
-	_mav_put_uint16_t(buf, 2, freemem);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN);
-#else
-	mavlink_meminfo_t packet;
-	packet.brkval = brkval;
-	packet.freemem = freemem;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MEMINFO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-}
-
-/**
- * @brief Pack a meminfo message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param brkval heap top
- * @param freemem free memory
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint16_t brkval,uint16_t freemem)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMINFO_LEN];
-	_mav_put_uint16_t(buf, 0, brkval);
-	_mav_put_uint16_t(buf, 2, freemem);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMINFO_LEN);
-#else
-	mavlink_meminfo_t packet;
-	packet.brkval = brkval;
-	packet.freemem = freemem;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MEMINFO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-}
-
-/**
- * @brief Encode a meminfo struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param meminfo C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo)
-{
-	return mavlink_msg_meminfo_pack(system_id, component_id, msg, meminfo->brkval, meminfo->freemem);
-}
-
-/**
- * @brief Send a meminfo message
- * @param chan MAVLink channel to send the message
- *
- * @param brkval heap top
- * @param freemem free memory
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brkval, uint16_t freemem)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MEMINFO_LEN];
-	_mav_put_uint16_t(buf, 0, brkval);
-	_mav_put_uint16_t(buf, 2, freemem);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-#else
-	mavlink_meminfo_t packet;
-	packet.brkval = brkval;
-	packet.freemem = freemem;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)&packet, MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE MEMINFO UNPACKING
-
-
-/**
- * @brief Get field brkval from meminfo message
- *
- * @return heap top
- */
-static inline uint16_t mavlink_msg_meminfo_get_brkval(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field freemem from meminfo message
- *
- * @return free memory
- */
-static inline uint16_t mavlink_msg_meminfo_get_freemem(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a meminfo message into a struct
- *
- * @param msg The message to decode
- * @param meminfo C-struct to decode the message contents into
- */
-static inline void mavlink_msg_meminfo_decode(const mavlink_message_t* msg, mavlink_meminfo_t* meminfo)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	meminfo->brkval = mavlink_msg_meminfo_get_brkval(msg);
-	meminfo->freemem = mavlink_msg_meminfo_get_freemem(msg);
-#else
-	memcpy(meminfo, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MEMINFO_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_mount_configure.h
+++ /dev/null
@@ -1,274 +1,1 @@
-// MESSAGE MOUNT_CONFIGURE PACKING
 
-#define MAVLINK_MSG_ID_MOUNT_CONFIGURE 156
-
-typedef struct __mavlink_mount_configure_t
-{
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t mount_mode; ///< mount operating mode (see MAV_MOUNT_MODE enum)
- uint8_t stab_roll; ///< (1 = yes, 0 = no)
- uint8_t stab_pitch; ///< (1 = yes, 0 = no)
- uint8_t stab_yaw; ///< (1 = yes, 0 = no)
-} mavlink_mount_configure_t;
-
-#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN 6
-#define MAVLINK_MSG_ID_156_LEN 6
-
-#define MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC 19
-#define MAVLINK_MSG_ID_156_CRC 19
-
-
-
-#define MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE { \
-	"MOUNT_CONFIGURE", \
-	6, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mount_configure_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mount_configure_t, target_component) }, \
-         { "mount_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mount_configure_t, mount_mode) }, \
-         { "stab_roll", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mount_configure_t, stab_roll) }, \
-         { "stab_pitch", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mount_configure_t, stab_pitch) }, \
-         { "stab_yaw", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mount_configure_t, stab_yaw) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mount_configure message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum)
- * @param stab_roll (1 = yes, 0 = no)
- * @param stab_pitch (1 = yes, 0 = no)
- * @param stab_yaw (1 = yes, 0 = no)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, mount_mode);
-	_mav_put_uint8_t(buf, 3, stab_roll);
-	_mav_put_uint8_t(buf, 4, stab_pitch);
-	_mav_put_uint8_t(buf, 5, stab_yaw);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#else
-	mavlink_mount_configure_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.mount_mode = mount_mode;
-	packet.stab_roll = stab_roll;
-	packet.stab_pitch = stab_pitch;
-	packet.stab_yaw = stab_yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mount_configure message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum)
- * @param stab_roll (1 = yes, 0 = no)
- * @param stab_pitch (1 = yes, 0 = no)
- * @param stab_yaw (1 = yes, 0 = no)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,uint8_t mount_mode,uint8_t stab_roll,uint8_t stab_pitch,uint8_t stab_yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, mount_mode);
-	_mav_put_uint8_t(buf, 3, stab_roll);
-	_mav_put_uint8_t(buf, 4, stab_pitch);
-	_mav_put_uint8_t(buf, 5, stab_yaw);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#else
-	mavlink_mount_configure_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.mount_mode = mount_mode;
-	packet.stab_roll = stab_roll;
-	packet.stab_pitch = stab_pitch;
-	packet.stab_yaw = stab_yaw;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONFIGURE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mount_configure struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param mount_configure C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure)
-{
-	return mavlink_msg_mount_configure_pack(system_id, component_id, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw);
-}
-
-/**
- * @brief Send a mount_configure message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mount_mode mount operating mode (see MAV_MOUNT_MODE enum)
- * @param stab_roll (1 = yes, 0 = no)
- * @param stab_pitch (1 = yes, 0 = no)
- * @param stab_yaw (1 = yes, 0 = no)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, target_component);
-	_mav_put_uint8_t(buf, 2, mount_mode);
-	_mav_put_uint8_t(buf, 3, stab_roll);
-	_mav_put_uint8_t(buf, 4, stab_pitch);
-	_mav_put_uint8_t(buf, 5, stab_yaw);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-#else
-	mavlink_mount_configure_t packet;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.mount_mode = mount_mode;
-	packet.stab_roll = stab_roll;
-	packet.stab_pitch = stab_pitch;
-	packet.stab_yaw = stab_yaw;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE MOUNT_CONFIGURE UNPACKING
-
-
-/**
- * @brief Get field target_system from mount_configure message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mount_configure_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field target_component from mount_configure message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mount_configure_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field mount_mode from mount_configure message
- *
- * @return mount operating mode (see MAV_MOUNT_MODE enum)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_mount_mode(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
-
-/**
- * @brief Get field stab_roll from mount_configure message
- *
- * @return (1 = yes, 0 = no)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_stab_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  3);
-}
-
-/**
- * @brief Get field stab_pitch from mount_configure message
- *
- * @return (1 = yes, 0 = no)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_stab_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field stab_yaw from mount_configure message
- *
- * @return (1 = yes, 0 = no)
- */
-static inline uint8_t mavlink_msg_mount_configure_get_stab_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Decode a mount_configure message into a struct
- *
- * @param msg The message to decode
- * @param mount_configure C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mount_configure_decode(const mavlink_message_t* msg, mavlink_mount_configure_t* mount_configure)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mount_configure->target_system = mavlink_msg_mount_configure_get_target_system(msg);
-	mount_configure->target_component = mavlink_msg_mount_configure_get_target_component(msg);
-	mount_configure->mount_mode = mavlink_msg_mount_configure_get_mount_mode(msg);
-	mount_configure->stab_roll = mavlink_msg_mount_configure_get_stab_roll(msg);
-	mount_configure->stab_pitch = mavlink_msg_mount_configure_get_stab_pitch(msg);
-	mount_configure->stab_yaw = mavlink_msg_mount_configure_get_stab_yaw(msg);
-#else
-	memcpy(mount_configure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_mount_control.h
+++ /dev/null
@@ -1,274 +1,1 @@
-// MESSAGE MOUNT_CONTROL PACKING
 
-#define MAVLINK_MSG_ID_MOUNT_CONTROL 157
-
-typedef struct __mavlink_mount_control_t
-{
- int32_t input_a; ///< pitch(deg*100) or lat, depending on mount mode
- int32_t input_b; ///< roll(deg*100) or lon depending on mount mode
- int32_t input_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
- uint8_t save_position; ///< if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
-} mavlink_mount_control_t;
-
-#define MAVLINK_MSG_ID_MOUNT_CONTROL_LEN 15
-#define MAVLINK_MSG_ID_157_LEN 15
-
-#define MAVLINK_MSG_ID_MOUNT_CONTROL_CRC 21
-#define MAVLINK_MSG_ID_157_CRC 21
-
-
-
-#define MAVLINK_MESSAGE_INFO_MOUNT_CONTROL { \
-	"MOUNT_CONTROL", \
-	6, \
-	{  { "input_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_control_t, input_a) }, \
-         { "input_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_control_t, input_b) }, \
-         { "input_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_control_t, input_c) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_control_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_control_t, target_component) }, \
-         { "save_position", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mount_control_t, save_position) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mount_control message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param input_a pitch(deg*100) or lat, depending on mount mode
- * @param input_b roll(deg*100) or lon depending on mount mode
- * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN];
-	_mav_put_int32_t(buf, 0, input_a);
-	_mav_put_int32_t(buf, 4, input_b);
-	_mav_put_int32_t(buf, 8, input_c);
-	_mav_put_uint8_t(buf, 12, target_system);
-	_mav_put_uint8_t(buf, 13, target_component);
-	_mav_put_uint8_t(buf, 14, save_position);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#else
-	mavlink_mount_control_t packet;
-	packet.input_a = input_a;
-	packet.input_b = input_b;
-	packet.input_c = input_c;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.save_position = save_position;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mount_control message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param input_a pitch(deg*100) or lat, depending on mount mode
- * @param input_b roll(deg*100) or lon depending on mount mode
- * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,int32_t input_a,int32_t input_b,int32_t input_c,uint8_t save_position)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN];
-	_mav_put_int32_t(buf, 0, input_a);
-	_mav_put_int32_t(buf, 4, input_b);
-	_mav_put_int32_t(buf, 8, input_c);
-	_mav_put_uint8_t(buf, 12, target_system);
-	_mav_put_uint8_t(buf, 13, target_component);
-	_mav_put_uint8_t(buf, 14, save_position);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#else
-	mavlink_mount_control_t packet;
-	packet.input_a = input_a;
-	packet.input_b = input_b;
-	packet.input_c = input_c;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.save_position = save_position;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_CONTROL;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mount_control struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param mount_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control)
-{
-	return mavlink_msg_mount_control_pack(system_id, component_id, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position);
-}
-
-/**
- * @brief Send a mount_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param input_a pitch(deg*100) or lat, depending on mount mode
- * @param input_b roll(deg*100) or lon depending on mount mode
- * @param input_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @param save_position if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_CONTROL_LEN];
-	_mav_put_int32_t(buf, 0, input_a);
-	_mav_put_int32_t(buf, 4, input_b);
-	_mav_put_int32_t(buf, 8, input_c);
-	_mav_put_uint8_t(buf, 12, target_system);
-	_mav_put_uint8_t(buf, 13, target_component);
-	_mav_put_uint8_t(buf, 14, save_position);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-#else
-	mavlink_mount_control_t packet;
-	packet.input_a = input_a;
-	packet.input_b = input_b;
-	packet.input_c = input_c;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-	packet.save_position = save_position;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE MOUNT_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target_system from mount_control message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mount_control_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field target_component from mount_control message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mount_control_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Get field input_a from mount_control message
- *
- * @return pitch(deg*100) or lat, depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_control_get_input_a(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field input_b from mount_control message
- *
- * @return roll(deg*100) or lon depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_control_get_input_b(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field input_c from mount_control message
- *
- * @return yaw(deg*100) or alt (in cm) depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_control_get_input_c(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field save_position from mount_control message
- *
- * @return if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING)
- */
-static inline uint8_t mavlink_msg_mount_control_get_save_position(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  14);
-}
-
-/**
- * @brief Decode a mount_control message into a struct
- *
- * @param msg The message to decode
- * @param mount_control C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mount_control_decode(const mavlink_message_t* msg, mavlink_mount_control_t* mount_control)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mount_control->input_a = mavlink_msg_mount_control_get_input_a(msg);
-	mount_control->input_b = mavlink_msg_mount_control_get_input_b(msg);
-	mount_control->input_c = mavlink_msg_mount_control_get_input_c(msg);
-	mount_control->target_system = mavlink_msg_mount_control_get_target_system(msg);
-	mount_control->target_component = mavlink_msg_mount_control_get_target_component(msg);
-	mount_control->save_position = mavlink_msg_mount_control_get_save_position(msg);
-#else
-	memcpy(mount_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_mount_status.h
+++ /dev/null
@@ -1,252 +1,1 @@
-// MESSAGE MOUNT_STATUS PACKING
 
-#define MAVLINK_MSG_ID_MOUNT_STATUS 158
-
-typedef struct __mavlink_mount_status_t
-{
- int32_t pointing_a; ///< pitch(deg*100) or lat, depending on mount mode
- int32_t pointing_b; ///< roll(deg*100) or lon depending on mount mode
- int32_t pointing_c; ///< yaw(deg*100) or alt (in cm) depending on mount mode
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_mount_status_t;
-
-#define MAVLINK_MSG_ID_MOUNT_STATUS_LEN 14
-#define MAVLINK_MSG_ID_158_LEN 14
-
-#define MAVLINK_MSG_ID_MOUNT_STATUS_CRC 134
-#define MAVLINK_MSG_ID_158_CRC 134
-
-
-
-#define MAVLINK_MESSAGE_INFO_MOUNT_STATUS { \
-	"MOUNT_STATUS", \
-	5, \
-	{  { "pointing_a", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_mount_status_t, pointing_a) }, \
-         { "pointing_b", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_mount_status_t, pointing_b) }, \
-         { "pointing_c", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_mount_status_t, pointing_c) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mount_status_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mount_status_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a mount_status message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param pointing_a pitch(deg*100) or lat, depending on mount mode
- * @param pointing_b roll(deg*100) or lon depending on mount mode
- * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN];
-	_mav_put_int32_t(buf, 0, pointing_a);
-	_mav_put_int32_t(buf, 4, pointing_b);
-	_mav_put_int32_t(buf, 8, pointing_c);
-	_mav_put_uint8_t(buf, 12, target_system);
-	_mav_put_uint8_t(buf, 13, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#else
-	mavlink_mount_status_t packet;
-	packet.pointing_a = pointing_a;
-	packet.pointing_b = pointing_b;
-	packet.pointing_c = pointing_c;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a mount_status message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param pointing_a pitch(deg*100) or lat, depending on mount mode
- * @param pointing_b roll(deg*100) or lon depending on mount mode
- * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,int32_t pointing_a,int32_t pointing_b,int32_t pointing_c)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN];
-	_mav_put_int32_t(buf, 0, pointing_a);
-	_mav_put_int32_t(buf, 4, pointing_b);
-	_mav_put_int32_t(buf, 8, pointing_c);
-	_mav_put_uint8_t(buf, 12, target_system);
-	_mav_put_uint8_t(buf, 13, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#else
-	mavlink_mount_status_t packet;
-	packet.pointing_a = pointing_a;
-	packet.pointing_b = pointing_b;
-	packet.pointing_c = pointing_c;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_MOUNT_STATUS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a mount_status struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param mount_status C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status)
-{
-	return mavlink_msg_mount_status_pack(system_id, component_id, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c);
-}
-
-/**
- * @brief Send a mount_status message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param pointing_a pitch(deg*100) or lat, depending on mount mode
- * @param pointing_b roll(deg*100) or lon depending on mount mode
- * @param pointing_c yaw(deg*100) or alt (in cm) depending on mount mode
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_MOUNT_STATUS_LEN];
-	_mav_put_int32_t(buf, 0, pointing_a);
-	_mav_put_int32_t(buf, 4, pointing_b);
-	_mav_put_int32_t(buf, 8, pointing_c);
-	_mav_put_uint8_t(buf, 12, target_system);
-	_mav_put_uint8_t(buf, 13, target_component);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-#else
-	mavlink_mount_status_t packet;
-	packet.pointing_a = pointing_a;
-	packet.pointing_b = pointing_b;
-	packet.pointing_c = pointing_c;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE MOUNT_STATUS UNPACKING
-
-
-/**
- * @brief Get field target_system from mount_status message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_mount_status_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  12);
-}
-
-/**
- * @brief Get field target_component from mount_status message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_mount_status_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  13);
-}
-
-/**
- * @brief Get field pointing_a from mount_status message
- *
- * @return pitch(deg*100) or lat, depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_status_get_pointing_a(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  0);
-}
-
-/**
- * @brief Get field pointing_b from mount_status message
- *
- * @return roll(deg*100) or lon depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_status_get_pointing_b(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field pointing_c from mount_status message
- *
- * @return yaw(deg*100) or alt (in cm) depending on mount mode
- */
-static inline int32_t mavlink_msg_mount_status_get_pointing_c(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Decode a mount_status message into a struct
- *
- * @param msg The message to decode
- * @param mount_status C-struct to decode the message contents into
- */
-static inline void mavlink_msg_mount_status_decode(const mavlink_message_t* msg, mavlink_mount_status_t* mount_status)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mount_status->pointing_a = mavlink_msg_mount_status_get_pointing_a(msg);
-	mount_status->pointing_b = mavlink_msg_mount_status_get_pointing_b(msg);
-	mount_status->pointing_c = mavlink_msg_mount_status_get_pointing_c(msg);
-	mount_status->target_system = mavlink_msg_mount_status_get_target_system(msg);
-	mount_status->target_component = mavlink_msg_mount_status_get_target_component(msg);
-#else
-	memcpy(mount_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_radio.h
+++ /dev/null
@@ -1,296 +1,1 @@
-// MESSAGE RADIO PACKING
 
-#define MAVLINK_MSG_ID_RADIO 166
-
-typedef struct __mavlink_radio_t
-{
- uint16_t rxerrors; ///< receive errors
- uint16_t fixed; ///< count of error corrected packets
- uint8_t rssi; ///< local signal strength
- uint8_t remrssi; ///< remote signal strength
- uint8_t txbuf; ///< how full the tx buffer is as a percentage
- uint8_t noise; ///< background noise level
- uint8_t remnoise; ///< remote background noise level
-} mavlink_radio_t;
-
-#define MAVLINK_MSG_ID_RADIO_LEN 9
-#define MAVLINK_MSG_ID_166_LEN 9
-
-#define MAVLINK_MSG_ID_RADIO_CRC 21
-#define MAVLINK_MSG_ID_166_CRC 21
-
-
-
-#define MAVLINK_MESSAGE_INFO_RADIO { \
-	"RADIO", \
-	7, \
-	{  { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_t, rxerrors) }, \
-         { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, fixed) }, \
-         { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_t, rssi) }, \
-         { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_t, remrssi) }, \
-         { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_t, txbuf) }, \
-         { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_t, noise) }, \
-         { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_t, remnoise) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a radio message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RADIO_LEN];
-	_mav_put_uint16_t(buf, 0, rxerrors);
-	_mav_put_uint16_t(buf, 2, fixed);
-	_mav_put_uint8_t(buf, 4, rssi);
-	_mav_put_uint8_t(buf, 5, remrssi);
-	_mav_put_uint8_t(buf, 6, txbuf);
-	_mav_put_uint8_t(buf, 7, noise);
-	_mav_put_uint8_t(buf, 8, remnoise);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN);
-#else
-	mavlink_radio_t packet;
-	packet.rxerrors = rxerrors;
-	packet.fixed = fixed;
-	packet.rssi = rssi;
-	packet.remrssi = remrssi;
-	packet.txbuf = txbuf;
-	packet.noise = noise;
-	packet.remnoise = remnoise;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RADIO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-}
-
-/**
- * @brief Pack a radio message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint8_t noise,uint8_t remnoise,uint16_t rxerrors,uint16_t fixed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RADIO_LEN];
-	_mav_put_uint16_t(buf, 0, rxerrors);
-	_mav_put_uint16_t(buf, 2, fixed);
-	_mav_put_uint8_t(buf, 4, rssi);
-	_mav_put_uint8_t(buf, 5, remrssi);
-	_mav_put_uint8_t(buf, 6, txbuf);
-	_mav_put_uint8_t(buf, 7, noise);
-	_mav_put_uint8_t(buf, 8, remnoise);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_LEN);
-#else
-	mavlink_radio_t packet;
-	packet.rxerrors = rxerrors;
-	packet.fixed = fixed;
-	packet.rssi = rssi;
-	packet.remrssi = remrssi;
-	packet.txbuf = txbuf;
-	packet.noise = noise;
-	packet.remnoise = remnoise;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RADIO;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-}
-
-/**
- * @brief Encode a radio struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param radio C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio)
-{
-	return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed);
-}
-
-/**
- * @brief Send a radio message
- * @param chan MAVLink channel to send the message
- *
- * @param rssi local signal strength
- * @param remrssi remote signal strength
- * @param txbuf how full the tx buffer is as a percentage
- * @param noise background noise level
- * @param remnoise remote background noise level
- * @param rxerrors receive errors
- * @param fixed count of error corrected packets
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RADIO_LEN];
-	_mav_put_uint16_t(buf, 0, rxerrors);
-	_mav_put_uint16_t(buf, 2, fixed);
-	_mav_put_uint8_t(buf, 4, rssi);
-	_mav_put_uint8_t(buf, 5, remrssi);
-	_mav_put_uint8_t(buf, 6, txbuf);
-	_mav_put_uint8_t(buf, 7, noise);
-	_mav_put_uint8_t(buf, 8, remnoise);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-#else
-	mavlink_radio_t packet;
-	packet.rxerrors = rxerrors;
-	packet.fixed = fixed;
-	packet.rssi = rssi;
-	packet.remrssi = remrssi;
-	packet.txbuf = txbuf;
-	packet.noise = noise;
-	packet.remnoise = remnoise;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE RADIO UNPACKING
-
-
-/**
- * @brief Get field rssi from radio message
- *
- * @return local signal strength
- */
-static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  4);
-}
-
-/**
- * @brief Get field remrssi from radio message
- *
- * @return remote signal strength
- */
-static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  5);
-}
-
-/**
- * @brief Get field txbuf from radio message
- *
- * @return how full the tx buffer is as a percentage
- */
-static inline uint8_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field noise from radio message
- *
- * @return background noise level
- */
-static inline uint8_t mavlink_msg_radio_get_noise(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field remnoise from radio message
- *
- * @return remote background noise level
- */
-static inline uint8_t mavlink_msg_radio_get_remnoise(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  8);
-}
-
-/**
- * @brief Get field rxerrors from radio message
- *
- * @return receive errors
- */
-static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  0);
-}
-
-/**
- * @brief Get field fixed from radio message
- *
- * @return count of error corrected packets
- */
-static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint16_t(msg,  2);
-}
-
-/**
- * @brief Decode a radio message into a struct
- *
- * @param msg The message to decode
- * @param radio C-struct to decode the message contents into
- */
-static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlink_radio_t* radio)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg);
-	radio->fixed = mavlink_msg_radio_get_fixed(msg);
-	radio->rssi = mavlink_msg_radio_get_rssi(msg);
-	radio->remrssi = mavlink_msg_radio_get_remrssi(msg);
-	radio->txbuf = mavlink_msg_radio_get_txbuf(msg);
-	radio->noise = mavlink_msg_radio_get_noise(msg);
-	radio->remnoise = mavlink_msg_radio_get_remnoise(msg);
-#else
-	memcpy(radio, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_rangefinder.h
+++ /dev/null
@@ -1,186 +1,1 @@
-// MESSAGE RANGEFINDER PACKING
 
-#define MAVLINK_MSG_ID_RANGEFINDER 173
-
-typedef struct __mavlink_rangefinder_t
-{
- float distance; ///< distance in meters
- float voltage; ///< raw voltage if available, zero otherwise
-} mavlink_rangefinder_t;
-
-#define MAVLINK_MSG_ID_RANGEFINDER_LEN 8
-#define MAVLINK_MSG_ID_173_LEN 8
-
-#define MAVLINK_MSG_ID_RANGEFINDER_CRC 83
-#define MAVLINK_MSG_ID_173_CRC 83
-
-
-
-#define MAVLINK_MESSAGE_INFO_RANGEFINDER { \
-	"RANGEFINDER", \
-	2, \
-	{  { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rangefinder_t, distance) }, \
-         { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rangefinder_t, voltage) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a rangefinder message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param distance distance in meters
- * @param voltage raw voltage if available, zero otherwise
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float distance, float voltage)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN];
-	_mav_put_float(buf, 0, distance);
-	_mav_put_float(buf, 4, voltage);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#else
-	mavlink_rangefinder_t packet;
-	packet.distance = distance;
-	packet.voltage = voltage;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RANGEFINDER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-}
-
-/**
- * @brief Pack a rangefinder message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param distance distance in meters
- * @param voltage raw voltage if available, zero otherwise
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float distance,float voltage)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN];
-	_mav_put_float(buf, 0, distance);
-	_mav_put_float(buf, 4, voltage);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#else
-	mavlink_rangefinder_t packet;
-	packet.distance = distance;
-	packet.voltage = voltage;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_RANGEFINDER;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-}
-
-/**
- * @brief Encode a rangefinder struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param rangefinder C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder)
-{
-	return mavlink_msg_rangefinder_pack(system_id, component_id, msg, rangefinder->distance, rangefinder->voltage);
-}
-
-/**
- * @brief Send a rangefinder message
- * @param chan MAVLink channel to send the message
- *
- * @param distance distance in meters
- * @param voltage raw voltage if available, zero otherwise
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float distance, float voltage)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_RANGEFINDER_LEN];
-	_mav_put_float(buf, 0, distance);
-	_mav_put_float(buf, 4, voltage);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-#else
-	mavlink_rangefinder_t packet;
-	packet.distance = distance;
-	packet.voltage = voltage;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)&packet, MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE RANGEFINDER UNPACKING
-
-
-/**
- * @brief Get field distance from rangefinder message
- *
- * @return distance in meters
- */
-static inline float mavlink_msg_rangefinder_get_distance(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field voltage from rangefinder message
- *
- * @return raw voltage if available, zero otherwise
- */
-static inline float mavlink_msg_rangefinder_get_voltage(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Decode a rangefinder message into a struct
- *
- * @param msg The message to decode
- * @param rangefinder C-struct to decode the message contents into
- */
-static inline void mavlink_msg_rangefinder_decode(const mavlink_message_t* msg, mavlink_rangefinder_t* rangefinder)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	rangefinder->distance = mavlink_msg_rangefinder_get_distance(msg);
-	rangefinder->voltage = mavlink_msg_rangefinder_get_voltage(msg);
-#else
-	memcpy(rangefinder, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RANGEFINDER_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h
+++ /dev/null
@@ -1,406 +1,1 @@
-// MESSAGE SENSOR_OFFSETS PACKING
 
-#define MAVLINK_MSG_ID_SENSOR_OFFSETS 150
-
-typedef struct __mavlink_sensor_offsets_t
-{
- float mag_declination; ///< magnetic declination (radians)
- int32_t raw_press; ///< raw pressure from barometer
- int32_t raw_temp; ///< raw temperature from barometer
- float gyro_cal_x; ///< gyro X calibration
- float gyro_cal_y; ///< gyro Y calibration
- float gyro_cal_z; ///< gyro Z calibration
- float accel_cal_x; ///< accel X calibration
- float accel_cal_y; ///< accel Y calibration
- float accel_cal_z; ///< accel Z calibration
- int16_t mag_ofs_x; ///< magnetometer X offset
- int16_t mag_ofs_y; ///< magnetometer Y offset
- int16_t mag_ofs_z; ///< magnetometer Z offset
-} mavlink_sensor_offsets_t;
-
-#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42
-#define MAVLINK_MSG_ID_150_LEN 42
-
-#define MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC 134
-#define MAVLINK_MSG_ID_150_CRC 134
-
-
-
-#define MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS { \
-	"SENSOR_OFFSETS", \
-	12, \
-	{  { "mag_declination", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_offsets_t, mag_declination) }, \
-         { "raw_press", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_sensor_offsets_t, raw_press) }, \
-         { "raw_temp", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_sensor_offsets_t, raw_temp) }, \
-         { "gyro_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_offsets_t, gyro_cal_x) }, \
-         { "gyro_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_offsets_t, gyro_cal_y) }, \
-         { "gyro_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_offsets_t, gyro_cal_z) }, \
-         { "accel_cal_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_sensor_offsets_t, accel_cal_x) }, \
-         { "accel_cal_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_sensor_offsets_t, accel_cal_y) }, \
-         { "accel_cal_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_sensor_offsets_t, accel_cal_z) }, \
-         { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_sensor_offsets_t, mag_ofs_x) }, \
-         { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_sensor_offsets_t, mag_ofs_y) }, \
-         { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_sensor_offsets_t, mag_ofs_z) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a sensor_offsets message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @param mag_declination magnetic declination (radians)
- * @param raw_press raw pressure from barometer
- * @param raw_temp raw temperature from barometer
- * @param gyro_cal_x gyro X calibration
- * @param gyro_cal_y gyro Y calibration
- * @param gyro_cal_z gyro Z calibration
- * @param accel_cal_x accel X calibration
- * @param accel_cal_y accel Y calibration
- * @param accel_cal_z accel Z calibration
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN];
-	_mav_put_float(buf, 0, mag_declination);
-	_mav_put_int32_t(buf, 4, raw_press);
-	_mav_put_int32_t(buf, 8, raw_temp);
-	_mav_put_float(buf, 12, gyro_cal_x);
-	_mav_put_float(buf, 16, gyro_cal_y);
-	_mav_put_float(buf, 20, gyro_cal_z);
-	_mav_put_float(buf, 24, accel_cal_x);
-	_mav_put_float(buf, 28, accel_cal_y);
-	_mav_put_float(buf, 32, accel_cal_z);
-	_mav_put_int16_t(buf, 36, mag_ofs_x);
-	_mav_put_int16_t(buf, 38, mag_ofs_y);
-	_mav_put_int16_t(buf, 40, mag_ofs_z);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#else
-	mavlink_sensor_offsets_t packet;
-	packet.mag_declination = mag_declination;
-	packet.raw_press = raw_press;
-	packet.raw_temp = raw_temp;
-	packet.gyro_cal_x = gyro_cal_x;
-	packet.gyro_cal_y = gyro_cal_y;
-	packet.gyro_cal_z = gyro_cal_z;
-	packet.accel_cal_x = accel_cal_x;
-	packet.accel_cal_y = accel_cal_y;
-	packet.accel_cal_z = accel_cal_z;
-	packet.mag_ofs_x = mag_ofs_x;
-	packet.mag_ofs_y = mag_ofs_y;
-	packet.mag_ofs_z = mag_ofs_z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a sensor_offsets message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @param mag_declination magnetic declination (radians)
- * @param raw_press raw pressure from barometer
- * @param raw_temp raw temperature from barometer
- * @param gyro_cal_x gyro X calibration
- * @param gyro_cal_y gyro Y calibration
- * @param gyro_cal_z gyro Z calibration
- * @param accel_cal_x accel X calibration
- * @param accel_cal_y accel Y calibration
- * @param accel_cal_z accel Z calibration
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN];
-	_mav_put_float(buf, 0, mag_declination);
-	_mav_put_int32_t(buf, 4, raw_press);
-	_mav_put_int32_t(buf, 8, raw_temp);
-	_mav_put_float(buf, 12, gyro_cal_x);
-	_mav_put_float(buf, 16, gyro_cal_y);
-	_mav_put_float(buf, 20, gyro_cal_z);
-	_mav_put_float(buf, 24, accel_cal_x);
-	_mav_put_float(buf, 28, accel_cal_y);
-	_mav_put_float(buf, 32, accel_cal_z);
-	_mav_put_int16_t(buf, 36, mag_ofs_x);
-	_mav_put_int16_t(buf, 38, mag_ofs_y);
-	_mav_put_int16_t(buf, 40, mag_ofs_z);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#else
-	mavlink_sensor_offsets_t packet;
-	packet.mag_declination = mag_declination;
-	packet.raw_press = raw_press;
-	packet.raw_temp = raw_temp;
-	packet.gyro_cal_x = gyro_cal_x;
-	packet.gyro_cal_y = gyro_cal_y;
-	packet.gyro_cal_z = gyro_cal_z;
-	packet.accel_cal_x = accel_cal_x;
-	packet.accel_cal_y = accel_cal_y;
-	packet.accel_cal_z = accel_cal_z;
-	packet.mag_ofs_x = mag_ofs_x;
-	packet.mag_ofs_y = mag_ofs_y;
-	packet.mag_ofs_z = mag_ofs_z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a sensor_offsets struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param sensor_offsets C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets)
-{
-	return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z);
-}
-
-/**
- * @brief Send a sensor_offsets message
- * @param chan MAVLink channel to send the message
- *
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @param mag_declination magnetic declination (radians)
- * @param raw_press raw pressure from barometer
- * @param raw_temp raw temperature from barometer
- * @param gyro_cal_x gyro X calibration
- * @param gyro_cal_y gyro Y calibration
- * @param gyro_cal_z gyro Z calibration
- * @param accel_cal_x accel X calibration
- * @param accel_cal_y accel Y calibration
- * @param accel_cal_z accel Z calibration
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN];
-	_mav_put_float(buf, 0, mag_declination);
-	_mav_put_int32_t(buf, 4, raw_press);
-	_mav_put_int32_t(buf, 8, raw_temp);
-	_mav_put_float(buf, 12, gyro_cal_x);
-	_mav_put_float(buf, 16, gyro_cal_y);
-	_mav_put_float(buf, 20, gyro_cal_z);
-	_mav_put_float(buf, 24, accel_cal_x);
-	_mav_put_float(buf, 28, accel_cal_y);
-	_mav_put_float(buf, 32, accel_cal_z);
-	_mav_put_int16_t(buf, 36, mag_ofs_x);
-	_mav_put_int16_t(buf, 38, mag_ofs_y);
-	_mav_put_int16_t(buf, 40, mag_ofs_z);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-#else
-	mavlink_sensor_offsets_t packet;
-	packet.mag_declination = mag_declination;
-	packet.raw_press = raw_press;
-	packet.raw_temp = raw_temp;
-	packet.gyro_cal_x = gyro_cal_x;
-	packet.gyro_cal_y = gyro_cal_y;
-	packet.gyro_cal_z = gyro_cal_z;
-	packet.accel_cal_x = accel_cal_x;
-	packet.accel_cal_y = accel_cal_y;
-	packet.accel_cal_z = accel_cal_z;
-	packet.mag_ofs_x = mag_ofs_x;
-	packet.mag_ofs_y = mag_ofs_y;
-	packet.mag_ofs_z = mag_ofs_z;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE SENSOR_OFFSETS UNPACKING
-
-
-/**
- * @brief Get field mag_ofs_x from sensor_offsets message
- *
- * @return magnetometer X offset
- */
-static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  36);
-}
-
-/**
- * @brief Get field mag_ofs_y from sensor_offsets message
- *
- * @return magnetometer Y offset
- */
-static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  38);
-}
-
-/**
- * @brief Get field mag_ofs_z from sensor_offsets message
- *
- * @return magnetometer Z offset
- */
-static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  40);
-}
-
-/**
- * @brief Get field mag_declination from sensor_offsets message
- *
- * @return magnetic declination (radians)
- */
-static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field raw_press from sensor_offsets message
- *
- * @return raw pressure from barometer
- */
-static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  4);
-}
-
-/**
- * @brief Get field raw_temp from sensor_offsets message
- *
- * @return raw temperature from barometer
- */
-static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  8);
-}
-
-/**
- * @brief Get field gyro_cal_x from sensor_offsets message
- *
- * @return gyro X calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field gyro_cal_y from sensor_offsets message
- *
- * @return gyro Y calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field gyro_cal_z from sensor_offsets message
- *
- * @return gyro Z calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field accel_cal_x from sensor_offsets message
- *
- * @return accel X calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field accel_cal_y from sensor_offsets message
- *
- * @return accel Y calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field accel_cal_z from sensor_offsets message
- *
- * @return accel Z calibration
- */
-static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Decode a sensor_offsets message into a struct
- *
- * @param msg The message to decode
- * @param sensor_offsets C-struct to decode the message contents into
- */
-static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg);
-	sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg);
-	sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg);
-	sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg);
-	sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg);
-	sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg);
-	sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg);
-	sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg);
-	sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg);
-	sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg);
-	sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg);
-	sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg);
-#else
-	memcpy(sensor_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h
+++ /dev/null
@@ -1,252 +1,1 @@
-// MESSAGE SET_MAG_OFFSETS PACKING
 
-#define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151
-
-typedef struct __mavlink_set_mag_offsets_t
-{
- int16_t mag_ofs_x; ///< magnetometer X offset
- int16_t mag_ofs_y; ///< magnetometer Y offset
- int16_t mag_ofs_z; ///< magnetometer Z offset
- uint8_t target_system; ///< System ID
- uint8_t target_component; ///< Component ID
-} mavlink_set_mag_offsets_t;
-
-#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8
-#define MAVLINK_MSG_ID_151_LEN 8
-
-#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC 219
-#define MAVLINK_MSG_ID_151_CRC 219
-
-
-
-#define MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS { \
-	"SET_MAG_OFFSETS", \
-	5, \
-	{  { "mag_ofs_x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_set_mag_offsets_t, mag_ofs_x) }, \
-         { "mag_ofs_y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_set_mag_offsets_t, mag_ofs_y) }, \
-         { "mag_ofs_z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_set_mag_offsets_t, mag_ofs_z) }, \
-         { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_set_mag_offsets_t, target_system) }, \
-         { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_set_mag_offsets_t, target_component) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a set_mag_offsets message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN];
-	_mav_put_int16_t(buf, 0, mag_ofs_x);
-	_mav_put_int16_t(buf, 2, mag_ofs_y);
-	_mav_put_int16_t(buf, 4, mag_ofs_z);
-	_mav_put_uint8_t(buf, 6, target_system);
-	_mav_put_uint8_t(buf, 7, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#else
-	mavlink_set_mag_offsets_t packet;
-	packet.mag_ofs_x = mag_ofs_x;
-	packet.mag_ofs_y = mag_ofs_y;
-	packet.mag_ofs_z = mag_ofs_z;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Pack a set_mag_offsets message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System ID
- * @param target_component Component ID
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN];
-	_mav_put_int16_t(buf, 0, mag_ofs_x);
-	_mav_put_int16_t(buf, 2, mag_ofs_y);
-	_mav_put_int16_t(buf, 4, mag_ofs_z);
-	_mav_put_uint8_t(buf, 6, target_system);
-	_mav_put_uint8_t(buf, 7, target_component);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#else
-	mavlink_set_mag_offsets_t packet;
-	packet.mag_ofs_x = mag_ofs_x;
-	packet.mag_ofs_y = mag_ofs_y;
-	packet.mag_ofs_z = mag_ofs_z;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-}
-
-/**
- * @brief Encode a set_mag_offsets struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param set_mag_offsets C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets)
-{
-	return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z);
-}
-
-/**
- * @brief Send a set_mag_offsets message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System ID
- * @param target_component Component ID
- * @param mag_ofs_x magnetometer X offset
- * @param mag_ofs_y magnetometer Y offset
- * @param mag_ofs_z magnetometer Z offset
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN];
-	_mav_put_int16_t(buf, 0, mag_ofs_x);
-	_mav_put_int16_t(buf, 2, mag_ofs_y);
-	_mav_put_int16_t(buf, 4, mag_ofs_z);
-	_mav_put_uint8_t(buf, 6, target_system);
-	_mav_put_uint8_t(buf, 7, target_component);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-#else
-	mavlink_set_mag_offsets_t packet;
-	packet.mag_ofs_x = mag_ofs_x;
-	packet.mag_ofs_y = mag_ofs_y;
-	packet.mag_ofs_z = mag_ofs_z;
-	packet.target_system = target_system;
-	packet.target_component = target_component;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE SET_MAG_OFFSETS UNPACKING
-
-
-/**
- * @brief Get field target_system from set_mag_offsets message
- *
- * @return System ID
- */
-static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  6);
-}
-
-/**
- * @brief Get field target_component from set_mag_offsets message
- *
- * @return Component ID
- */
-static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  7);
-}
-
-/**
- * @brief Get field mag_ofs_x from set_mag_offsets message
- *
- * @return magnetometer X offset
- */
-static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  0);
-}
-
-/**
- * @brief Get field mag_ofs_y from set_mag_offsets message
- *
- * @return magnetometer Y offset
- */
-static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  2);
-}
-
-/**
- * @brief Get field mag_ofs_z from set_mag_offsets message
- *
- * @return magnetometer Z offset
- */
-static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int16_t(msg,  4);
-}
-
-/**
- * @brief Decode a set_mag_offsets message into a struct
- *
- * @param msg The message to decode
- * @param set_mag_offsets C-struct to decode the message contents into
- */
-static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg);
-	set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg);
-	set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg);
-	set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg);
-	set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg);
-#else
-	memcpy(set_mag_offsets, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_simstate.h
+++ /dev/null
@@ -1,384 +1,1 @@
-// MESSAGE SIMSTATE PACKING
 
-#define MAVLINK_MSG_ID_SIMSTATE 164
-
-typedef struct __mavlink_simstate_t
-{
- float roll; ///< Roll angle (rad)
- float pitch; ///< Pitch angle (rad)
- float yaw; ///< Yaw angle (rad)
- float xacc; ///< X acceleration m/s/s
- float yacc; ///< Y acceleration m/s/s
- float zacc; ///< Z acceleration m/s/s
- float xgyro; ///< Angular speed around X axis rad/s
- float ygyro; ///< Angular speed around Y axis rad/s
- float zgyro; ///< Angular speed around Z axis rad/s
- int32_t lat; ///< Latitude in degrees * 1E7
- int32_t lng; ///< Longitude in degrees * 1E7
-} mavlink_simstate_t;
-
-#define MAVLINK_MSG_ID_SIMSTATE_LEN 44
-#define MAVLINK_MSG_ID_164_LEN 44
-
-#define MAVLINK_MSG_ID_SIMSTATE_CRC 154
-#define MAVLINK_MSG_ID_164_CRC 154
-
-
-
-#define MAVLINK_MESSAGE_INFO_SIMSTATE { \
-	"SIMSTATE", \
-	11, \
-	{  { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_simstate_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_simstate_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_simstate_t, yaw) }, \
-         { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_simstate_t, xacc) }, \
-         { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_simstate_t, yacc) }, \
-         { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_simstate_t, zacc) }, \
-         { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \
-         { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \
-         { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \
-         { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \
-         { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a simstate message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SIMSTATE_LEN];
-	_mav_put_float(buf, 0, roll);
-	_mav_put_float(buf, 4, pitch);
-	_mav_put_float(buf, 8, yaw);
-	_mav_put_float(buf, 12, xacc);
-	_mav_put_float(buf, 16, yacc);
-	_mav_put_float(buf, 20, zacc);
-	_mav_put_float(buf, 24, xgyro);
-	_mav_put_float(buf, 28, ygyro);
-	_mav_put_float(buf, 32, zgyro);
-	_mav_put_int32_t(buf, 36, lat);
-	_mav_put_int32_t(buf, 40, lng);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#else
-	mavlink_simstate_t packet;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.xacc = xacc;
-	packet.yacc = yacc;
-	packet.zacc = zacc;
-	packet.xgyro = xgyro;
-	packet.ygyro = ygyro;
-	packet.zgyro = zgyro;
-	packet.lat = lat;
-	packet.lng = lng;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SIMSTATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-}
-
-/**
- * @brief Pack a simstate message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,int32_t lat,int32_t lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SIMSTATE_LEN];
-	_mav_put_float(buf, 0, roll);
-	_mav_put_float(buf, 4, pitch);
-	_mav_put_float(buf, 8, yaw);
-	_mav_put_float(buf, 12, xacc);
-	_mav_put_float(buf, 16, yacc);
-	_mav_put_float(buf, 20, zacc);
-	_mav_put_float(buf, 24, xgyro);
-	_mav_put_float(buf, 28, ygyro);
-	_mav_put_float(buf, 32, zgyro);
-	_mav_put_int32_t(buf, 36, lat);
-	_mav_put_int32_t(buf, 40, lng);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#else
-	mavlink_simstate_t packet;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.xacc = xacc;
-	packet.yacc = yacc;
-	packet.zacc = zacc;
-	packet.xgyro = xgyro;
-	packet.ygyro = ygyro;
-	packet.zgyro = zgyro;
-	packet.lat = lat;
-	packet.lng = lng;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_SIMSTATE;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-}
-
-/**
- * @brief Encode a simstate struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param simstate C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_simstate_t* simstate)
-{
-	return mavlink_msg_simstate_pack(system_id, component_id, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng);
-}
-
-/**
- * @brief Send a simstate message
- * @param chan MAVLink channel to send the message
- *
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param xacc X acceleration m/s/s
- * @param yacc Y acceleration m/s/s
- * @param zacc Z acceleration m/s/s
- * @param xgyro Angular speed around X axis rad/s
- * @param ygyro Angular speed around Y axis rad/s
- * @param zgyro Angular speed around Z axis rad/s
- * @param lat Latitude in degrees * 1E7
- * @param lng Longitude in degrees * 1E7
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_SIMSTATE_LEN];
-	_mav_put_float(buf, 0, roll);
-	_mav_put_float(buf, 4, pitch);
-	_mav_put_float(buf, 8, yaw);
-	_mav_put_float(buf, 12, xacc);
-	_mav_put_float(buf, 16, yacc);
-	_mav_put_float(buf, 20, zacc);
-	_mav_put_float(buf, 24, xgyro);
-	_mav_put_float(buf, 28, ygyro);
-	_mav_put_float(buf, 32, zgyro);
-	_mav_put_int32_t(buf, 36, lat);
-	_mav_put_int32_t(buf, 40, lng);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-#else
-	mavlink_simstate_t packet;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.xacc = xacc;
-	packet.yacc = yacc;
-	packet.zacc = zacc;
-	packet.xgyro = xgyro;
-	packet.ygyro = ygyro;
-	packet.zgyro = zgyro;
-	packet.lat = lat;
-	packet.lng = lng;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)&packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE SIMSTATE UNPACKING
-
-
-/**
- * @brief Get field roll from simstate message
- *
- * @return Roll angle (rad)
- */
-static inline float mavlink_msg_simstate_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field pitch from simstate message
- *
- * @return Pitch angle (rad)
- */
-static inline float mavlink_msg_simstate_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field yaw from simstate message
- *
- * @return Yaw angle (rad)
- */
-static inline float mavlink_msg_simstate_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field xacc from simstate message
- *
- * @return X acceleration m/s/s
- */
-static inline float mavlink_msg_simstate_get_xacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field yacc from simstate message
- *
- * @return Y acceleration m/s/s
- */
-static inline float mavlink_msg_simstate_get_yacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field zacc from simstate message
- *
- * @return Z acceleration m/s/s
- */
-static inline float mavlink_msg_simstate_get_zacc(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field xgyro from simstate message
- *
- * @return Angular speed around X axis rad/s
- */
-static inline float mavlink_msg_simstate_get_xgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field ygyro from simstate message
- *
- * @return Angular speed around Y axis rad/s
- */
-static inline float mavlink_msg_simstate_get_ygyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Get field zgyro from simstate message
- *
- * @return Angular speed around Z axis rad/s
- */
-static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  32);
-}
-
-/**
- * @brief Get field lat from simstate message
- *
- * @return Latitude in degrees * 1E7
- */
-static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  36);
-}
-
-/**
- * @brief Get field lng from simstate message
- *
- * @return Longitude in degrees * 1E7
- */
-static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_int32_t(msg,  40);
-}
-
-/**
- * @brief Decode a simstate message into a struct
- *
- * @param msg The message to decode
- * @param simstate C-struct to decode the message contents into
- */
-static inline void mavlink_msg_simstate_decode(const mavlink_message_t* msg, mavlink_simstate_t* simstate)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	simstate->roll = mavlink_msg_simstate_get_roll(msg);
-	simstate->pitch = mavlink_msg_simstate_get_pitch(msg);
-	simstate->yaw = mavlink_msg_simstate_get_yaw(msg);
-	simstate->xacc = mavlink_msg_simstate_get_xacc(msg);
-	simstate->yacc = mavlink_msg_simstate_get_yacc(msg);
-	simstate->zacc = mavlink_msg_simstate_get_zacc(msg);
-	simstate->xgyro = mavlink_msg_simstate_get_xgyro(msg);
-	simstate->ygyro = mavlink_msg_simstate_get_ygyro(msg);
-	simstate->zgyro = mavlink_msg_simstate_get_zgyro(msg);
-	simstate->lat = mavlink_msg_simstate_get_lat(msg);
-	simstate->lng = mavlink_msg_simstate_get_lng(msg);
-#else
-	memcpy(simstate, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SIMSTATE_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/mavlink_msg_wind.h
+++ /dev/null
@@ -1,208 +1,1 @@
-// MESSAGE WIND PACKING
 
-#define MAVLINK_MSG_ID_WIND 168
-
-typedef struct __mavlink_wind_t
-{
- float direction; ///< wind direction that wind is coming from (degrees)
- float speed; ///< wind speed in ground plane (m/s)
- float speed_z; ///< vertical wind speed (m/s)
-} mavlink_wind_t;
-
-#define MAVLINK_MSG_ID_WIND_LEN 12
-#define MAVLINK_MSG_ID_168_LEN 12
-
-#define MAVLINK_MSG_ID_WIND_CRC 1
-#define MAVLINK_MSG_ID_168_CRC 1
-
-
-
-#define MAVLINK_MESSAGE_INFO_WIND { \
-	"WIND", \
-	3, \
-	{  { "direction", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_wind_t, direction) }, \
-         { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_wind_t, speed) }, \
-         { "speed_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_wind_t, speed_z) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a wind message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param direction wind direction that wind is coming from (degrees)
- * @param speed wind speed in ground plane (m/s)
- * @param speed_z vertical wind speed (m/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       float direction, float speed, float speed_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WIND_LEN];
-	_mav_put_float(buf, 0, direction);
-	_mav_put_float(buf, 4, speed);
-	_mav_put_float(buf, 8, speed_z);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN);
-#else
-	mavlink_wind_t packet;
-	packet.direction = direction;
-	packet.speed = speed;
-	packet.speed_z = speed_z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WIND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
-#else
-    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-}
-
-/**
- * @brief Pack a wind message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param direction wind direction that wind is coming from (degrees)
- * @param speed wind speed in ground plane (m/s)
- * @param speed_z vertical wind speed (m/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           float direction,float speed,float speed_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WIND_LEN];
-	_mav_put_float(buf, 0, direction);
-	_mav_put_float(buf, 4, speed);
-	_mav_put_float(buf, 8, speed_z);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_LEN);
-#else
-	mavlink_wind_t packet;
-	packet.direction = direction;
-	packet.speed = speed;
-	packet.speed_z = speed_z;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_WIND;
-#if MAVLINK_CRC_EXTRA
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
-#else
-    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-}
-
-/**
- * @brief Encode a wind struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param wind C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wind_t* wind)
-{
-	return mavlink_msg_wind_pack(system_id, component_id, msg, wind->direction, wind->speed, wind->speed_z);
-}
-
-/**
- * @brief Send a wind message
- * @param chan MAVLink channel to send the message
- *
- * @param direction wind direction that wind is coming from (degrees)
- * @param speed wind speed in ground plane (m/s)
- * @param speed_z vertical wind speed (m/s)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction, float speed, float speed_z)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[MAVLINK_MSG_ID_WIND_LEN];
-	_mav_put_float(buf, 0, direction);
-	_mav_put_float(buf, 4, speed);
-	_mav_put_float(buf, 8, speed_z);
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-#else
-	mavlink_wind_t packet;
-	packet.direction = direction;
-	packet.speed = speed;
-	packet.speed_z = speed_z;
-
-#if MAVLINK_CRC_EXTRA
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
-#else
-    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)&packet, MAVLINK_MSG_ID_WIND_LEN);
-#endif
-#endif
-}
-
-#endif
-
-// MESSAGE WIND UNPACKING
-
-
-/**
- * @brief Get field direction from wind message
- *
- * @return wind direction that wind is coming from (degrees)
- */
-static inline float mavlink_msg_wind_get_direction(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  0);
-}
-
-/**
- * @brief Get field speed from wind message
- *
- * @return wind speed in ground plane (m/s)
- */
-static inline float mavlink_msg_wind_get_speed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field speed_z from wind message
- *
- * @return vertical wind speed (m/s)
- */
-static inline float mavlink_msg_wind_get_speed_z(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Decode a wind message into a struct
- *
- * @param msg The message to decode
- * @param wind C-struct to decode the message contents into
- */
-static inline void mavlink_msg_wind_decode(const mavlink_message_t* msg, mavlink_wind_t* wind)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	wind->direction = mavlink_msg_wind_get_direction(msg);
-	wind->speed = mavlink_msg_wind_get_speed(msg);
-	wind->speed_z = mavlink_msg_wind_get_speed_z(msg);
-#else
-	memcpy(wind, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_WIND_LEN);
-#endif
-}
-

--- a/libraries/mavlink/include/ardupilotmega/testsuite.h
+++ /dev/null
@@ -1,1259 +1,1 @@
-/** @file
- *	@brief MAVLink comm protocol testsuite generated from ardupilotmega.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef ARDUPILOTMEGA_TESTSUITE_H
-#define ARDUPILOTMEGA_TESTSUITE_H
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef MAVLINK_TEST_ALL
-#define MAVLINK_TEST_ALL
-static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
-static void mavlink_test_ardupilotmega(uint8_t, uint8_t, mavlink_message_t *last_msg);
-
-static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_common(system_id, component_id, last_msg);
-	mavlink_test_ardupilotmega(system_id, component_id, last_msg);
-}
-#endif
-
-#include "../common/testsuite.h"
-
-
-static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_sensor_offsets_t packet_in = {
-		17.0,
-	963497672,
-	963497880,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
-	213.0,
-	241.0,
-	19107,
-	19211,
-	19315,
-	};
-	mavlink_sensor_offsets_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.mag_declination = packet_in.mag_declination;
-        	packet1.raw_press = packet_in.raw_press;
-        	packet1.raw_temp = packet_in.raw_temp;
-        	packet1.gyro_cal_x = packet_in.gyro_cal_x;
-        	packet1.gyro_cal_y = packet_in.gyro_cal_y;
-        	packet1.gyro_cal_z = packet_in.gyro_cal_z;
-        	packet1.accel_cal_x = packet_in.accel_cal_x;
-        	packet1.accel_cal_y = packet_in.accel_cal_y;
-        	packet1.accel_cal_z = packet_in.accel_cal_z;
-        	packet1.mag_ofs_x = packet_in.mag_ofs_x;
-        	packet1.mag_ofs_y = packet_in.mag_ofs_y;
-        	packet1.mag_ofs_z = packet_in.mag_ofs_z;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_sensor_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z );
-	mavlink_msg_sensor_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z );
-	mavlink_msg_sensor_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_sensor_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_sensor_offsets_send(MAVLINK_COMM_1 , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z );
-	mavlink_msg_sensor_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_set_mag_offsets(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_set_mag_offsets_t packet_in = {
-		17235,
-	17339,
-	17443,
-	151,
-	218,
-	};
-	mavlink_set_mag_offsets_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.mag_ofs_x = packet_in.mag_ofs_x;
-        	packet1.mag_ofs_y = packet_in.mag_ofs_y;
-        	packet1.mag_ofs_z = packet_in.mag_ofs_z;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_set_mag_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
-	mavlink_msg_set_mag_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
-	mavlink_msg_set_mag_offsets_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_set_mag_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_set_mag_offsets_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z );
-	mavlink_msg_set_mag_offsets_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_meminfo_t packet_in = {
-		17235,
-	17339,
-	};
-	mavlink_meminfo_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.brkval = packet_in.brkval;
-        	packet1.freemem = packet_in.freemem;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_meminfo_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_pack(system_id, component_id, &msg , packet1.brkval , packet1.freemem );
-	mavlink_msg_meminfo_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.brkval , packet1.freemem );
-	mavlink_msg_meminfo_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_meminfo_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_meminfo_send(MAVLINK_COMM_1 , packet1.brkval , packet1.freemem );
-	mavlink_msg_meminfo_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_ap_adc_t packet_in = {
-		17235,
-	17339,
-	17443,
-	17547,
-	17651,
-	17755,
-	};
-	mavlink_ap_adc_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.adc1 = packet_in.adc1;
-        	packet1.adc2 = packet_in.adc2;
-        	packet1.adc3 = packet_in.adc3;
-        	packet1.adc4 = packet_in.adc4;
-        	packet1.adc5 = packet_in.adc5;
-        	packet1.adc6 = packet_in.adc6;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_ap_adc_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 );
-	mavlink_msg_ap_adc_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 );
-	mavlink_msg_ap_adc_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_ap_adc_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ap_adc_send(MAVLINK_COMM_1 , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 );
-	mavlink_msg_ap_adc_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_digicam_configure(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_digicam_configure_t packet_in = {
-		17.0,
-	17443,
-	151,
-	218,
-	29,
-	96,
-	163,
-	230,
-	41,
-	108,
-	175,
-	};
-	mavlink_digicam_configure_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.extra_value = packet_in.extra_value;
-        	packet1.shutter_speed = packet_in.shutter_speed;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.mode = packet_in.mode;
-        	packet1.aperture = packet_in.aperture;
-        	packet1.iso = packet_in.iso;
-        	packet1.exposure_type = packet_in.exposure_type;
-        	packet1.command_id = packet_in.command_id;
-        	packet1.engine_cut_off = packet_in.engine_cut_off;
-        	packet1.extra_param = packet_in.extra_param;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_digicam_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_digicam_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_configure_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mode , packet1.shutter_speed , packet1.aperture , packet1.iso , packet1.exposure_type , packet1.command_id , packet1.engine_cut_off , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_digicam_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_digicam_control_t packet_in = {
-		17.0,
-	17,
-	84,
-	151,
-	218,
-	29,
-	96,
-	163,
-	230,
-	41,
-	};
-	mavlink_digicam_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.extra_value = packet_in.extra_value;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.session = packet_in.session;
-        	packet1.zoom_pos = packet_in.zoom_pos;
-        	packet1.zoom_step = packet_in.zoom_step;
-        	packet1.focus_lock = packet_in.focus_lock;
-        	packet1.shot = packet_in.shot;
-        	packet1.command_id = packet_in.command_id;
-        	packet1.extra_param = packet_in.extra_param;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_digicam_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.session , packet1.zoom_pos , packet1.zoom_step , packet1.focus_lock , packet1.shot , packet1.command_id , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.session , packet1.zoom_pos , packet1.zoom_step , packet1.focus_lock , packet1.shot , packet1.command_id , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_digicam_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_digicam_control_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.session , packet1.zoom_pos , packet1.zoom_step , packet1.focus_lock , packet1.shot , packet1.command_id , packet1.extra_param , packet1.extra_value );
-	mavlink_msg_digicam_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mount_configure(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_mount_configure_t packet_in = {
-		5,
-	72,
-	139,
-	206,
-	17,
-	84,
-	};
-	mavlink_mount_configure_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.mount_mode = packet_in.mount_mode;
-        	packet1.stab_roll = packet_in.stab_roll;
-        	packet1.stab_pitch = packet_in.stab_pitch;
-        	packet1.stab_yaw = packet_in.stab_yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mount_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mount_mode , packet1.stab_roll , packet1.stab_pitch , packet1.stab_yaw );
-	mavlink_msg_mount_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mount_mode , packet1.stab_roll , packet1.stab_pitch , packet1.stab_yaw );
-	mavlink_msg_mount_configure_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_mount_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_configure_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mount_mode , packet1.stab_roll , packet1.stab_pitch , packet1.stab_yaw );
-	mavlink_msg_mount_configure_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mount_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_mount_control_t packet_in = {
-		963497464,
-	963497672,
-	963497880,
-	41,
-	108,
-	175,
-	};
-	mavlink_mount_control_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.input_a = packet_in.input_a;
-        	packet1.input_b = packet_in.input_b;
-        	packet1.input_c = packet_in.input_c;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.save_position = packet_in.save_position;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mount_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.input_a , packet1.input_b , packet1.input_c , packet1.save_position );
-	mavlink_msg_mount_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.input_a , packet1.input_b , packet1.input_c , packet1.save_position );
-	mavlink_msg_mount_control_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_mount_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_control_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.input_a , packet1.input_b , packet1.input_c , packet1.save_position );
-	mavlink_msg_mount_control_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_mount_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_mount_status_t packet_in = {
-		963497464,
-	963497672,
-	963497880,
-	41,
-	108,
-	};
-	mavlink_mount_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.pointing_a = packet_in.pointing_a;
-        	packet1.pointing_b = packet_in.pointing_b;
-        	packet1.pointing_c = packet_in.pointing_c;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_mount_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.pointing_a , packet1.pointing_b , packet1.pointing_c );
-	mavlink_msg_mount_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.pointing_a , packet1.pointing_b , packet1.pointing_c );
-	mavlink_msg_mount_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_mount_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_mount_status_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.pointing_a , packet1.pointing_b , packet1.pointing_c );
-	mavlink_msg_mount_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_fence_point(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_fence_point_t packet_in = {
-		17.0,
-	45.0,
-	29,
-	96,
-	163,
-	230,
-	};
-	mavlink_fence_point_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.lat = packet_in.lat;
-        	packet1.lng = packet_in.lng;
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.idx = packet_in.idx;
-        	packet1.count = packet_in.count;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_fence_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng );
-	mavlink_msg_fence_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng );
-	mavlink_msg_fence_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_fence_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng );
-	mavlink_msg_fence_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_fence_fetch_point(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_fence_fetch_point_t packet_in = {
-		5,
-	72,
-	139,
-	};
-	mavlink_fence_fetch_point_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.target_system = packet_in.target_system;
-        	packet1.target_component = packet_in.target_component;
-        	packet1.idx = packet_in.idx;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_fence_fetch_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_fence_fetch_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_fence_fetch_point_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_fence_fetch_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_fetch_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx );
-	mavlink_msg_fence_fetch_point_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_fence_status_t packet_in = {
-		963497464,
-	17443,
-	151,
-	218,
-	};
-	mavlink_fence_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.breach_time = packet_in.breach_time;
-        	packet1.breach_count = packet_in.breach_count;
-        	packet1.breach_status = packet_in.breach_status;
-        	packet1.breach_type = packet_in.breach_type;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_fence_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_pack(system_id, component_id, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time );
-	mavlink_msg_fence_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time );
-	mavlink_msg_fence_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_fence_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_fence_status_send(MAVLINK_COMM_1 , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time );
-	mavlink_msg_fence_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_ahrs_t packet_in = {
-		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
-	};
-	mavlink_ahrs_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.omegaIx = packet_in.omegaIx;
-        	packet1.omegaIy = packet_in.omegaIy;
-        	packet1.omegaIz = packet_in.omegaIz;
-        	packet1.accel_weight = packet_in.accel_weight;
-        	packet1.renorm_val = packet_in.renorm_val;
-        	packet1.error_rp = packet_in.error_rp;
-        	packet1.error_yaw = packet_in.error_yaw;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_ahrs_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_ahrs_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_ahrs_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_ahrs_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_ahrs_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_ahrs_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_simstate(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_simstate_t packet_in = {
-		17.0,
-	45.0,
-	73.0,
-	101.0,
-	129.0,
-	157.0,
-	185.0,
-	213.0,
-	241.0,
-	963499336,
-	963499544,
-	};
-	mavlink_simstate_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.roll = packet_in.roll;
-        	packet1.pitch = packet_in.pitch;
-        	packet1.yaw = packet_in.yaw;
-        	packet1.xacc = packet_in.xacc;
-        	packet1.yacc = packet_in.yacc;
-        	packet1.zacc = packet_in.zacc;
-        	packet1.xgyro = packet_in.xgyro;
-        	packet1.ygyro = packet_in.ygyro;
-        	packet1.zgyro = packet_in.zgyro;
-        	packet1.lat = packet_in.lat;
-        	packet1.lng = packet_in.lng;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_simstate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_pack(system_id, component_id, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lng );
-	mavlink_msg_simstate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lng );
-	mavlink_msg_simstate_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_simstate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_simstate_send(MAVLINK_COMM_1 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lng );
-	mavlink_msg_simstate_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_hwstatus(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_hwstatus_t packet_in = {
-		17235,
-	139,
-	};
-	mavlink_hwstatus_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.Vcc = packet_in.Vcc;
-        	packet1.I2Cerr = packet_in.I2Cerr;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_hwstatus_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_pack(system_id, component_id, &msg , packet1.Vcc , packet1.I2Cerr );
-	mavlink_msg_hwstatus_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Vcc , packet1.I2Cerr );
-	mavlink_msg_hwstatus_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_hwstatus_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_hwstatus_send(MAVLINK_COMM_1 , packet1.Vcc , packet1.I2Cerr );
-	mavlink_msg_hwstatus_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_radio_t packet_in = {
-		17235,
-	17339,
-	17,
-	84,
-	151,
-	218,
-	29,
-	};
-	mavlink_radio_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.rxerrors = packet_in.rxerrors;
-        	packet1.fixed = packet_in.fixed;
-        	packet1.rssi = packet_in.rssi;
-        	packet1.remrssi = packet_in.remrssi;
-        	packet1.txbuf = packet_in.txbuf;
-        	packet1.noise = packet_in.noise;
-        	packet1.remnoise = packet_in.remnoise;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_radio_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_radio_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_radio_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed );
-	mavlink_msg_radio_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_limits_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_limits_status_t packet_in = {
-		963497464,
-	963497672,
-	963497880,
-	963498088,
-	18067,
-	187,
-	254,
-	65,
-	132,
-	};
-	mavlink_limits_status_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.last_trigger = packet_in.last_trigger;
-        	packet1.last_action = packet_in.last_action;
-        	packet1.last_recovery = packet_in.last_recovery;
-        	packet1.last_clear = packet_in.last_clear;
-        	packet1.breach_count = packet_in.breach_count;
-        	packet1.limits_state = packet_in.limits_state;
-        	packet1.mods_enabled = packet_in.mods_enabled;
-        	packet1.mods_required = packet_in.mods_required;
-        	packet1.mods_triggered = packet_in.mods_triggered;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_limits_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_pack(system_id, component_id, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered );
-	mavlink_msg_limits_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered );
-	mavlink_msg_limits_status_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_limits_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_limits_status_send(MAVLINK_COMM_1 , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered );
-	mavlink_msg_limits_status_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_wind(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_wind_t packet_in = {
-		17.0,
-	45.0,
-	73.0,
-	};
-	mavlink_wind_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.direction = packet_in.direction;
-        	packet1.speed = packet_in.speed;
-        	packet1.speed_z = packet_in.speed_z;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_wind_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_pack(system_id, component_id, &msg , packet1.direction , packet1.speed , packet1.speed_z );
-	mavlink_msg_wind_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.direction , packet1.speed , packet1.speed_z );
-	mavlink_msg_wind_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_wind_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_wind_send(MAVLINK_COMM_1 , packet1.direction , packet1.speed , packet1.speed_z );
-	mavlink_msg_wind_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data16(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_data16_t packet_in = {
-		5,
-	72,
-	{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154 },
-	};
-	mavlink_data16_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*16);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data16_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data16_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data16_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_data16_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data16_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data16_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data32(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_data32_t packet_in = {
-		5,
-	72,
-	{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170 },
-	};
-	mavlink_data32_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*32);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data32_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data32_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data32_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_data32_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data32_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data32_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data64(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_data64_t packet_in = {
-		5,
-	72,
-	{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 },
-	};
-	mavlink_data64_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*64);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data64_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data64_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data64_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_data64_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data64_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data64_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_data96(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_data96_t packet_in = {
-		5,
-	72,
-	{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234 },
-	};
-	mavlink_data96_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.type = packet_in.type;
-        	packet1.len = packet_in.len;
-        
-        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*96);
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_data96_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_pack(system_id, component_id, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data96_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data96_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_data96_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data96_send(MAVLINK_COMM_1 , packet1.type , packet1.len , packet1.data );
-	mavlink_msg_data96_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_rangefinder(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_message_t msg;
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        uint16_t i;
-	mavlink_rangefinder_t packet_in = {
-		17.0,
-	45.0,
-	};
-	mavlink_rangefinder_t packet1, packet2;
-        memset(&packet1, 0, sizeof(packet1));
-        	packet1.distance = packet_in.distance;
-        	packet1.voltage = packet_in.voltage;
-        
-        
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_rangefinder_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_pack(system_id, component_id, &msg , packet1.distance , packet1.voltage );
-	mavlink_msg_rangefinder_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.distance , packet1.voltage );
-	mavlink_msg_rangefinder_decode(&msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-
-        memset(&packet2, 0, sizeof(packet2));
-        mavlink_msg_to_send_buffer(buffer, &msg);
-        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
-        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
-        }
-	mavlink_msg_rangefinder_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-        
-        memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_rangefinder_send(MAVLINK_COMM_1 , packet1.distance , packet1.voltage );
-	mavlink_msg_rangefinder_decode(last_msg, &packet2);
-        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
-}
-
-static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
-{
-	mavlink_test_sensor_offsets(system_id, component_id, last_msg);
-	mavlink_test_set_mag_offsets(system_id, component_id, last_msg);
-	mavlink_test_meminfo(system_id, component_id, last_msg);
-	mavlink_test_ap_adc(system_id, component_id, last_msg);
-	mavlink_test_digicam_configure(system_id, component_id, last_msg);
-	mavlink_test_digicam_control(system_id, component_id, last_msg);
-	mavlink_test_mount_configure(system_id, component_id, last_msg);
-	mavlink_test_mount_control(system_id, component_id, last_msg);
-	mavlink_test_mount_status(system_id, component_id, last_msg);
-	mavlink_test_fence_point(system_id, component_id, last_msg);
-	mavlink_test_fence_fetch_point(system_id, component_id, last_msg);
-	mavlink_test_fence_status(system_id, component_id, last_msg);
-	mavlink_test_ahrs(system_id, component_id, last_msg);
-	mavlink_test_simstate(system_id, component_id, last_msg);
-	mavlink_test_hwstatus(system_id, component_id, last_msg);
-	mavlink_test_radio(system_id, component_id, last_msg);
-	mavlink_test_limits_status(system_id, component_id, last_msg);
-	mavlink_test_wind(system_id, component_id, last_msg);
-	mavlink_test_data16(system_id, component_id, last_msg);
-	mavlink_test_data32(system_id, component_id, last_msg);
-	mavlink_test_data64(system_id, component_id, last_msg);
-	mavlink_test_data96(system_id, component_id, last_msg);
-	mavlink_test_rangefinder(system_id, component_id, last_msg);
-}
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // ARDUPILOTMEGA_TESTSUITE_H
-

--- a/libraries/mavlink/include/ardupilotmega/version.h
+++ /dev/null
@@ -1,13 +1,1 @@
-/** @file
- *	@brief MAVLink comm protocol built from ardupilotmega.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_VERSION_H
-#define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Aug 14 13:52:03 2013"
-#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
- 
-#endif // MAVLINK_VERSION_H
-

--- a/libraries/mavlink/include/checksum.h
+++ /dev/null
@@ -1,92 +1,1 @@
-#ifdef __cplusplus
-extern "C" {
-#endif
 
-#ifndef _CHECKSUM_H_
-#define _CHECKSUM_H_
-
-
-/**
- *
- *  CALCULATE THE CHECKSUM
- *
- */
-
-#define X25_INIT_CRC 0xffff
-#define X25_VALIDATE_CRC 0xf0b8
-
-/**
- * @brief Accumulate the X.25 CRC by adding one char at a time.
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new char to hash
- * @param crcAccum the already accumulated checksum
- **/
-#ifndef HAVE_CRC_ACCUMULATE
-static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
-{
-        /*Accumulate one byte of data into the CRC*/
-        uint8_t tmp;
-
-        tmp = data ^ (uint8_t)(*crcAccum &0xff);
-        tmp ^= (tmp<<4);
-        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
-}
-#endif
-
-/**
- * @brief Initiliaze the buffer for the X.25 CRC
- *
- * @param crcAccum the 16 bit X.25 CRC
- */
-static inline void crc_init(uint16_t* crcAccum)
-{
-        *crcAccum = X25_INIT_CRC;
-}
-
-
-/**
- * @brief Calculates the X.25 checksum on a byte buffer
- *
- * @param  pBuffer buffer containing the byte array to hash
- * @param  length  length of the byte array
- * @return the checksum over the buffer bytes
- **/
-static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
-{
-        uint16_t crcTmp;
-        crc_init(&crcTmp);
-	while (length--) {
-                crc_accumulate(*pBuffer++, &crcTmp);
-        }
-        return crcTmp;
-}
-
-/**
- * @brief Accumulate the X.25 CRC by adding an array of bytes
- *
- * The checksum function adds the hash of one char at a time to the
- * 16 bit checksum (uint16_t).
- *
- * @param data new bytes to hash
- * @param crcAccum the already accumulated checksum
- **/
-static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
-{
-	const uint8_t *p = (const uint8_t *)pBuffer;
-	while (length--) {
-                crc_accumulate(*p++, crcAccum);
-        }
-}
-
-
-
-
-#endif /* _CHECKSUM_H_ */
-
-#ifdef __cplusplus
-}
-#endif
-

--- a/libraries/mavlink/include/common/common.h
+++ /dev/null
@@ -1,489 +1,1 @@
-/** @file
- *	@brief MAVLink comm protocol generated from common.xml
- *	@see http://qgroundcontrol.org/mavlink/
- */
-#ifndef COMMON_H
-#define COMMON_H
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// MESSAGE LENGTHS AND CRCS
-
-#ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 54, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 200, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
-#endif
-
-#ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
-#endif
-
-#include "../protocol.h"
-
-#define MAVLINK_ENABLED_COMMON
-
-
-
-// MAVLINK VERSION
-
-#ifndef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-#if (MAVLINK_VERSION == 0)
-#undef MAVLINK_VERSION
-#define MAVLINK_VERSION 3
-#endif
-
-// ENUM DEFINITIONS
-
-
-/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */
-#ifndef HAVE_ENUM_MAV_AUTOPILOT
-#define HAVE_ENUM_MAV_AUTOPILOT
-enum MAV_AUTOPILOT
-{
-	MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */
-	MAV_AUTOPILOT_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch | */
-	MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */
-	MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com | */
-	MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */
-	MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */
-	MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */
-	MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */
-	MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */
-	MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */
-	MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */
-	MAV_AUTOPILOT_FP=11, /* FlexiPilot | */
-	MAV_AUTOPILOT_ENUM_END=12, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_TYPE
-#define HAVE_ENUM_MAV_TYPE
-enum MAV_TYPE
-{
-	MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */
-	MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */
-	MAV_TYPE_QUADROTOR=2, /* Quadrotor | */
-	MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */
-	MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */
-	MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */
-	MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */
-	MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */
-	MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */
-	MAV_TYPE_ROCKET=9, /* Rocket | */
-	MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */
-	MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */
-	MAV_TYPE_SUBMARINE=12, /* Submarine | */
-	MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */
-	MAV_TYPE_OCTOROTOR=14, /* Octorotor | */
-	MAV_TYPE_TRICOPTER=15, /* Octorotor | */
-	MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */
-	MAV_TYPE_ENUM_END=17, /*  | */
-};
-#endif
-
-/** @brief These flags encode the MAV mode. */
-#ifndef HAVE_ENUM_MAV_MODE_FLAG
-#define HAVE_ENUM_MAV_MODE_FLAG
-enum MAV_MODE_FLAG
-{
-	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */
-	MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
-	MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
-	MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
-	MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
-	MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
-	MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */
-	MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
-	MAV_MODE_FLAG_ENUM_END=129, /*  | */
-};
-#endif
-
-/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */
-#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION
-#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION
-enum MAV_MODE_FLAG_DECODE_POSITION
-{
-	MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */
-	MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */
-	MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixt bit:   00000100 | */
-	MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit:  00001000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit:  00100000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit:  10000000 | */
-	MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /*  | */
-};
-#endif
-
-/** @brief Override command, pauses current mission execution and moves immediately to a position */
-#ifndef HAVE_ENUM_MAV_GOTO
-#define HAVE_ENUM_MAV_GOTO
-enum MAV_GOTO
-{
-	MAV_GOTO_DO_HOLD=0, /* Hold at the current position. | */
-	MAV_GOTO_DO_CONTINUE=1, /* Continue with the next item in mission execution. | */
-	MAV_GOTO_HOLD_AT_CURRENT_POSITION=2, /* Hold at the current position of the system | */
-	MAV_GOTO_HOLD_AT_SPECIFIED_POSITION=3, /* Hold at the position specified in the parameters of the DO_HOLD action | */
-	MAV_GOTO_ENUM_END=4, /*  | */
-};
-#endif
-
-/** @brief These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it
-               simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. */
-#ifndef HAVE_ENUM_MAV_MODE
-#define HAVE_ENUM_MAV_MODE
-enum MAV_MODE
-{
-	MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. No flag is set. | */
-	MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization | */
-	MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */
-	MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control. | */
-	MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint | */
-	MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */
-	MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization | */
-	MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */
-	MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control. | */
-	MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint | */
-	MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) | */
-	MAV_MODE_ENUM_END=221, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_STATE
-#define HAVE_ENUM_MAV_STATE
-enum MAV_STATE
-{
-	MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */
-	MAV_STATE_BOOT=1, /* System is booting up. | */
-	MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */
-	MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */
-	MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */
-	MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */
-	MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */
-	MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */
-	MAV_STATE_ENUM_END=8, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_COMPONENT
-#define HAVE_ENUM_MAV_COMPONENT
-enum MAV_COMPONENT
-{
-	MAV_COMP_ID_ALL=0, /*  | */
-	MAV_COMP_ID_CAMERA=100, /*  | */
-	MAV_COMP_ID_SERVO1=140, /*  | */
-	MAV_COMP_ID_SERVO2=141, /*  | */
-	MAV_COMP_ID_SERVO3=142, /*  | */
-	MAV_COMP_ID_SERVO4=143, /*  | */
-	MAV_COMP_ID_SERVO5=144, /*  | */
-	MAV_COMP_ID_SERVO6=145, /*  | */
-	MAV_COMP_ID_SERVO7=146, /*  | */
-	MAV_COMP_ID_SERVO8=147, /*  | */
-	MAV_COMP_ID_SERVO9=148, /*  | */
-	MAV_COMP_ID_SERVO10=149, /*  | */
-	MAV_COMP_ID_SERVO11=150, /*  | */
-	MAV_COMP_ID_SERVO12=151, /*  | */
-	MAV_COMP_ID_SERVO13=152, /*  | */
-	MAV_COMP_ID_SERVO14=153, /*  | */
-	MAV_COMP_ID_MAPPER=180, /*  | */
-	MAV_COMP_ID_MISSIONPLANNER=190, /*  | */
-	MAV_COMP_ID_PATHPLANNER=195, /*  | */
-	MAV_COMP_ID_IMU=200, /*  | */
-	MAV_COMP_ID_IMU_2=201, /*  | */
-	MAV_COMP_ID_IMU_3=202, /*  | */
-	MAV_COMP_ID_GPS=220, /*  | */
-	MAV_COMP_ID_UDP_BRIDGE=240, /*  | */
-	MAV_COMP_ID_UART_BRIDGE=241, /*  | */
-	MAV_COMP_ID_SYSTEM_CONTROL=250, /*  | */
-	MAV_COMPONENT_ENUM_END=251, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_FRAME
-#define HAVE_ENUM_MAV_FRAME
-enum MAV_FRAME
-{
-	MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) | */
-	MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */
-	MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */
-	MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */
-	MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */
-	MAV_FRAME_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE
-#define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE
-enum MAVLINK_DATA_STREAM_TYPE
-{
-	MAVLINK_DATA_STREAM_IMG_JPEG=1, /*  | */
-	MAVLINK_DATA_STREAM_IMG_BMP=2, /*  | */
-	MAVLINK_DATA_STREAM_IMG_RAW8U=3, /*  | */
-	MAVLINK_DATA_STREAM_IMG_RAW32U=4, /*  | */
-	MAVLINK_DATA_STREAM_IMG_PGM=5, /*  | */
-	MAVLINK_DATA_STREAM_IMG_PNG=6, /*  | */
-	MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /*  | */
-};
-#endif
-
-/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */
-#ifndef HAVE_ENUM_MAV_CMD
-#define HAVE_ENUM_MAV_CMD
-enum MAV_CMD
-{
-	MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
-	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
-	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
-	MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude|  */
-	MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
-	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
-	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
-	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
-	MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|  */
-	MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
-	MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
-	MAV_CMD_ENUM_END=401, /*  | */
-};
-#endif
-
-/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a
-     recommendation to the autopilot software. Individual autopilots may or may not obey
-     the recommended messages. */
-#ifndef HAVE_ENUM_MAV_DATA_STREAM
-#define HAVE_ENUM_MAV_DATA_STREAM
-enum MAV_DATA_STREAM
-{
-	MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */
-	MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */
-	MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */
-	MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */
-	MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */
-	MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */
-	MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */
-	MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */
-	MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */
-	MAV_DATA_STREAM_ENUM_END=13, /*  | */
-};
-#endif
-
-/** @brief  The ROI (region of interest) for the vehicle. This can be
-                be used by the vehicle for camera/vehicle attitude alignment (see
-                MAV_CMD_NAV_ROI). */
-#ifndef HAVE_ENUM_MAV_ROI
-#define HAVE_ENUM_MAV_ROI
-enum MAV_ROI
-{
-	MAV_ROI_NONE=0, /* No region of interest. | */
-	MAV_ROI_WPNEXT=1, /* Point toward next MISSION. | */
-	MAV_ROI_WPINDEX=2, /* Point toward given MISSION. | */
-	MAV_ROI_LOCATION=3, /* Point toward fixed location. | */
-	MAV_ROI_TARGET=4, /* Point toward of given id. | */
-	MAV_ROI_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */
-#ifndef HAVE_ENUM_MAV_CMD_ACK
-#define HAVE_ENUM_MAV_CMD_ACK
-enum MAV_CMD_ACK
-{
-	MAV_CMD_ACK_OK=1, /* Command / mission item is ok. | */
-	MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. | */
-	MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. | */
-	MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. | */
-	MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. | */
-	MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. | */
-	MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. | */
-	MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. | */
-	MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. | */
-	MAV_CMD_ACK_ENUM_END=10, /*  | */
-};
-#endif
-
-/** @brief type of a mavlink parameter */
-#ifndef HAVE_ENUM_MAV_VAR
-#define HAVE_ENUM_MAV_VAR
-enum MAV_VAR
-{
-	MAV_VAR_FLOAT=0, /* 32 bit float | */
-	MAV_VAR_UINT8=1, /* 8 bit unsigned integer | */
-	MAV_VAR_INT8=2, /* 8 bit signed integer | */
-	MAV_VAR_UINT16=3, /* 16 bit unsigned integer | */
-	MAV_VAR_INT16=4, /* 16 bit signed integer | */
-	MAV_VAR_UINT32=5, /* 32 bit unsigned integer | */
-	MAV_VAR_INT32=6, /* 32 bit signed integer | */
-	MAV_VAR_ENUM_END=7, /*  | */
-};
-#endif
-
-/** @brief result from a mavlink command */
-#ifndef HAVE_ENUM_MAV_RESULT
-#define HAVE_ENUM_MAV_RESULT
-enum MAV_RESULT
-{
-	MAV_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */
-	MAV_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */
-	MAV_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */
-	MAV_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */
-	MAV_RESULT_FAILED=4, /* Command executed, but failed | */
-	MAV_RESULT_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief result in a mavlink mission ack */
-#ifndef HAVE_ENUM_MAV_MISSION_RESULT
-#define HAVE_ENUM_MAV_MISSION_RESULT
-enum MAV_MISSION_RESULT
-{
-	MAV_MISSION_ACCEPTED=0, /* mission accepted OK | */
-	MAV_MISSION_ERROR=1, /* generic error / not accepting mission commands at all right now | */
-	MAV_MISSION_UNSUPPORTED_FRAME=2, /* coordinate frame is not supported | */
-	MAV_MISSION_UNSUPPORTED=3, /* command is not supported | */
-	MAV_MISSION_NO_SPACE=4, /* mission item exceeds storage space | */
-	MAV_MISSION_INVALID=5, /* one of the parameters has an invalid value | */
-	MAV_MISSION_INVALID_PARAM1=6, /* param1 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM2=7, /* param2 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM3=8, /* param3 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM4=9, /* param4 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM5_X=10, /* x/param5 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM6_Y=11, /* y/param6 has an invalid value | */
-	MAV_MISSION_INVALID_PARAM7=12, /* param7 has an invalid value | */
-	MAV_MISSION_INVALID_SEQUENCE=13, /* received waypoint out of sequence | */
-	MAV_MISSION_DENIED=14, /* not accepting any mission commands from this communication partner | */
-	MAV_MISSION_RESULT_ENUM_END=15, /*  | */
-};
-#endif
-
-/** @brief Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. */
-#ifndef HAVE_ENUM_MAV_SEVERITY
-#define HAVE_ENUM_MAV_SEVERITY
-enum MAV_SEVERITY
-{
-	MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */
-	MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */
-	MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */
-	MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */
-	MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */
-	MAV_SEVERITY_NOTICE=5, /* An unusual event has occured, though not an error condition. This should be investigated for the root cause. | */
-	MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */
-	MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */
-	MAV_SEVERITY_ENUM_END=8, /*  | */
-};
-#endif
-
-// MESSAGE DEFINITIONS
-#include "./mavlink_msg_heartbeat.h"
-#include "./mavlink_msg_sys_status.h"
-#include "./mavlink_msg_system_time.h"
-#include "./mavlink_msg_ping.h"
-#include "./mavlink_msg_change_operator_control.h"
-#include "./mavlink_msg_change_operator_control_ack.h"
-#include "./mavlink_msg_auth_key.h"
-#include "./mavlink_msg_set_mode.h"
-#include "./mavlink_msg_param_request_read.h"
-#include "./mavlink_msg_param_request_list.h"
-#include "./mavlink_msg_param_value.h"
-#include "./mavlink_msg_param_set.h"
-#include "./mavlink_msg_gps_raw_int.h"
-#include "./mavlink_msg_gps_status.h"
-#include "./mavlink_msg_scaled_imu.h"
-#include "./mavlink_msg_raw_imu.h"
-#include "./mavlink_msg_raw_pressure.h"
-#include "./mavlink_msg_scaled_pressure.h"
-#include "./mavlink_msg_attitude.h"
-#include "./mavlink_msg_attitude_quaternion.h"
-#include "./mavlink_msg_local_position_ned.h"
-#include "./mavlink_msg_global_position_int.h"
-#include "./mavlink_msg_rc_channels_scaled.h"
-#include "./mavlink_msg_rc_channels_raw.h"
-#include "./mavlink_msg_servo_output_raw.h"
-#include "./mavlink_msg_mission_request_partial_list.h"
-#include "./mavlink_msg_mission_write_partial_list.h"
-#include "./mavlink_msg_mission_item.h"
-#include "./mavlink_msg_mission_request.h"
-#include "./mavlink_msg_mission_set_current.h"
-#include "./mavlink_msg_mission_current.h"
-#include "./mavlink_msg_mission_request_list.h"
-#include "./mavlink_msg_mission_count.h"
-#include "./mavlink_msg_mission_clear_all.h"
-#include "./mavlink_msg_mission_item_reached.h"
-#include "./mavlink_msg_mission_ack.h"
-#include "./mavlink_msg_set_gps_global_origin.h"
-#include "./mavlink_msg_gps_global_origin.h"
-#include "./mavlink_msg_set_local_position_setpoint.h"
-#include "./mavlink_msg_local_position_setpoint.h"
-#include "./mavlink_msg_global_position_setpoint_int.h"
-#include "./mavlink_msg_set_global_position_setpoint_int.h"
-#include "./mavlink_msg_safety_set_allowed_area.h"
-#include "./mavlink_msg_safety_allowed_area.h"
-#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h"
-#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h"
-#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h"
-#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h"
-#include "./mavlink_msg_set_quad_motors_setpoint.h"
-#include "./mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h"
-#include "./mavlink_msg_nav_controller_output.h"
-#include "./mavlink_msg_state_correction.h"
-#include "./mavlink_msg_request_data_stream.h"
-#include "./mavlink_msg_data_stream.h"
-#include "./mavlink_msg_manual_control.h"
-#include "./mavlink_msg_rc_channels_override.h"
-#include "./mavlink_msg_vfr_hud.h"
-#include "./mavlink_msg_command_long.h"
-#include "./mavlink_msg_command_ack.h"
-#include "./mavlink_msg_local_position_ned_system_global_offset.h"
-#include "./mavlink_msg_hil_state.h"
-#include "./mavlink_msg_hil_controls.h"
-#include "./mavlink_msg_hil_rc_inputs_raw.h"
-#include "./mavlink_msg_optical_flow.h"
-#include "./mavlink_msg_global_vision_position_estimate.h"
-#include "./mavlink_msg_vision_position_estimate.h"
-#include "./mavlink_msg_vision_speed_estimate.h"
-#include "./mavlink_msg_vicon_position_estimate.h"
-#include "./mavlink_msg_memory_vect.h"
-#include "./mavlink_msg_debug_vect.h"
-#include "./mavlink_msg_named_value_float.h"
-#include "./mavlink_msg_named_value_int.h"
-#include "./mavlink_msg_statustext.h"
-#include "./mavlink_msg_debug.h"
-
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-#endif // COMMON_H
-

--- a/libraries/mavlink/include/common/mavlink.h
+++ /dev/null
@@ -1,28 +1,1 @@
-/** @file
- *	@brief MAVLink comm protocol built from common.xml
- *	@see http://pixhawk.ethz.ch/software/mavlink
- */
-#ifndef MAVLINK_H
-#define MAVLINK_H
 
-#ifndef MAVLINK_STX
-#define MAVLINK_STX 254
-#endif
-
-#ifndef MAVLINK_ENDIAN
-#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
-#endif
-
-#ifndef MAVLINK_ALIGNED_FIELDS
-#define MAVLINK_ALIGNED_FIELDS 1
-#endif
-
-#ifndef MAVLINK_CRC_EXTRA
-#define MAVLINK_CRC_EXTRA 1
-#endif
-
-#include "version.h"
-#include "common.h"
-
-#endif // MAVLINK_H
-

--- a/libraries/mavlink/include/common/mavlink_msg_attitude.h
+++ /dev/null
@@ -1,277 +1,1 @@
-// MESSAGE ATTITUDE PACKING
 
-#define MAVLINK_MSG_ID_ATTITUDE 30
-
-typedef struct __mavlink_attitude_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float roll; ///< Roll angle (rad)
- float pitch; ///< Pitch angle (rad)
- float yaw; ///< Yaw angle (rad)
- float rollspeed; ///< Roll angular speed (rad/s)
- float pitchspeed; ///< Pitch angular speed (rad/s)
- float yawspeed; ///< Yaw angular speed (rad/s)
-} mavlink_attitude_t;
-
-#define MAVLINK_MSG_ID_ATTITUDE_LEN 28
-#define MAVLINK_MSG_ID_30_LEN 28
-
-
-
-#define MAVLINK_MESSAGE_INFO_ATTITUDE { \
-	"ATTITUDE", \
-	7, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_t, time_boot_ms) }, \
-         { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_t, roll) }, \
-         { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_t, pitch) }, \
-         { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_t, yaw) }, \
-         { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_t, rollspeed) }, \
-         { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_t, pitchspeed) }, \
-         { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_t, yawspeed) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a attitude message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, roll);
-	_mav_put_float(buf, 8, pitch);
-	_mav_put_float(buf, 12, yaw);
-	_mav_put_float(buf, 16, rollspeed);
-	_mav_put_float(buf, 20, pitchspeed);
-	_mav_put_float(buf, 24, yawspeed);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
-#else
-	mavlink_attitude_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.rollspeed = rollspeed;
-	packet.pitchspeed = pitchspeed;
-	packet.yawspeed = yawspeed;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 39);
-}
-
-/**
- * @brief Pack a attitude message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint32_t time_boot_ms,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, roll);
-	_mav_put_float(buf, 8, pitch);
-	_mav_put_float(buf, 12, yaw);
-	_mav_put_float(buf, 16, rollspeed);
-	_mav_put_float(buf, 20, pitchspeed);
-	_mav_put_float(buf, 24, yawspeed);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
-#else
-	mavlink_attitude_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.rollspeed = rollspeed;
-	packet.pitchspeed = pitchspeed;
-	packet.yawspeed = yawspeed;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 39);
-}
-
-/**
- * @brief Encode a attitude struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param attitude C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_t* attitude)
-{
-	return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed);
-}
-
-/**
- * @brief Send a attitude message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param roll Roll angle (rad)
- * @param pitch Pitch angle (rad)
- * @param yaw Yaw angle (rad)
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, roll);
-	_mav_put_float(buf, 8, pitch);
-	_mav_put_float(buf, 12, yaw);
-	_mav_put_float(buf, 16, rollspeed);
-	_mav_put_float(buf, 20, pitchspeed);
-	_mav_put_float(buf, 24, yawspeed);
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, 28, 39);
-#else
-	mavlink_attitude_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.roll = roll;
-	packet.pitch = pitch;
-	packet.yaw = yaw;
-	packet.rollspeed = rollspeed;
-	packet.pitchspeed = pitchspeed;
-	packet.yawspeed = yawspeed;
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, 28, 39);
-#endif
-}
-
-#endif
-
-// MESSAGE ATTITUDE UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from attitude message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_attitude_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field roll from attitude message
- *
- * @return Roll angle (rad)
- */
-static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field pitch from attitude message
- *
- * @return Pitch angle (rad)
- */
-static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field yaw from attitude message
- *
- * @return Yaw angle (rad)
- */
-static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field rollspeed from attitude message
- *
- * @return Roll angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field pitchspeed from attitude message
- *
- * @return Pitch angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field yawspeed from attitude message
- *
- * @return Yaw angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Decode a attitude message into a struct
- *
- * @param msg The message to decode
- * @param attitude C-struct to decode the message contents into
- */
-static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	attitude->time_boot_ms = mavlink_msg_attitude_get_time_boot_ms(msg);
-	attitude->roll = mavlink_msg_attitude_get_roll(msg);
-	attitude->pitch = mavlink_msg_attitude_get_pitch(msg);
-	attitude->yaw = mavlink_msg_attitude_get_yaw(msg);
-	attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg);
-	attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg);
-	attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg);
-#else
-	memcpy(attitude, _MAV_PAYLOAD(msg), 28);
-#endif
-}
-

--- a/libraries/mavlink/include/common/mavlink_msg_attitude_quaternion.h
+++ /dev/null
@@ -1,299 +1,1 @@
-// MESSAGE ATTITUDE_QUATERNION PACKING
 
-#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION 31
-
-typedef struct __mavlink_attitude_quaternion_t
-{
- uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
- float q1; ///< Quaternion component 1
- float q2; ///< Quaternion component 2
- float q3; ///< Quaternion component 3
- float q4; ///< Quaternion component 4
- float rollspeed; ///< Roll angular speed (rad/s)
- float pitchspeed; ///< Pitch angular speed (rad/s)
- float yawspeed; ///< Yaw angular speed (rad/s)
-} mavlink_attitude_quaternion_t;
-
-#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 32
-#define MAVLINK_MSG_ID_31_LEN 32
-
-
-
-#define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \
-	"ATTITUDE_QUATERNION", \
-	8, \
-	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \
-         { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \
-         { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \
-         { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_quaternion_t, q3) }, \
-         { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_quaternion_t, q4) }, \
-         { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \
-         { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \
-         { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a attitude_quaternion message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param q1 Quaternion component 1
- * @param q2 Quaternion component 2
- * @param q3 Quaternion component 3
- * @param q4 Quaternion component 4
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, q1);
-	_mav_put_float(buf, 8, q2);
-	_mav_put_float(buf, 12, q3);
-	_mav_put_float(buf, 16, q4);
-	_mav_put_float(buf, 20, rollspeed);
-	_mav_put_float(buf, 24, pitchspeed);
-	_mav_put_float(buf, 28, yawspeed);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
-#else
-	mavlink_attitude_quaternion_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.q1 = q1;
-	packet.q2 = q2;
-	packet.q3 = q3;
-	packet.q4 = q4;
-	packet.rollspeed = rollspeed;
-	packet.pitchspeed = pitchspeed;
-	packet.yawspeed = yawspeed;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 246);
-}
-
-/**
- * @brief Pack a attitude_quaternion message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param q1 Quaternion component 1
- * @param q2 Quaternion component 2
- * @param q3 Quaternion component 3
- * @param q4 Quaternion component 4
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, q1);
-	_mav_put_float(buf, 8, q2);
-	_mav_put_float(buf, 12, q3);
-	_mav_put_float(buf, 16, q4);
-	_mav_put_float(buf, 20, rollspeed);
-	_mav_put_float(buf, 24, pitchspeed);
-	_mav_put_float(buf, 28, yawspeed);
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
-#else
-	mavlink_attitude_quaternion_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.q1 = q1;
-	packet.q2 = q2;
-	packet.q3 = q3;
-	packet.q4 = q4;
-	packet.rollspeed = rollspeed;
-	packet.pitchspeed = pitchspeed;
-	packet.yawspeed = yawspeed;
-
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 246);
-}
-
-/**
- * @brief Encode a attitude_quaternion struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param attitude_quaternion C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion)
-{
-	return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed);
-}
-
-/**
- * @brief Send a attitude_quaternion message
- * @param chan MAVLink channel to send the message
- *
- * @param time_boot_ms Timestamp (milliseconds since system boot)
- * @param q1 Quaternion component 1
- * @param q2 Quaternion component 2
- * @param q3 Quaternion component 3
- * @param q4 Quaternion component 4
- * @param rollspeed Roll angular speed (rad/s)
- * @param pitchspeed Pitch angular speed (rad/s)
- * @param yawspeed Yaw angular speed (rad/s)
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
-	_mav_put_uint32_t(buf, 0, time_boot_ms);
-	_mav_put_float(buf, 4, q1);
-	_mav_put_float(buf, 8, q2);
-	_mav_put_float(buf, 12, q3);
-	_mav_put_float(buf, 16, q4);
-	_mav_put_float(buf, 20, rollspeed);
-	_mav_put_float(buf, 24, pitchspeed);
-	_mav_put_float(buf, 28, yawspeed);
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, 32, 246);
-#else
-	mavlink_attitude_quaternion_t packet;
-	packet.time_boot_ms = time_boot_ms;
-	packet.q1 = q1;
-	packet.q2 = q2;
-	packet.q3 = q3;
-	packet.q4 = q4;
-	packet.rollspeed = rollspeed;
-	packet.pitchspeed = pitchspeed;
-	packet.yawspeed = yawspeed;
-
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, 32, 246);
-#endif
-}
-
-#endif
-
-// MESSAGE ATTITUDE_QUATERNION UNPACKING
-
-
-/**
- * @brief Get field time_boot_ms from attitude_quaternion message
- *
- * @return Timestamp (milliseconds since system boot)
- */
-static inline uint32_t mavlink_msg_attitude_quaternion_get_time_boot_ms(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint32_t(msg,  0);
-}
-
-/**
- * @brief Get field q1 from attitude_quaternion message
- *
- * @return Quaternion component 1
- */
-static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  4);
-}
-
-/**
- * @brief Get field q2 from attitude_quaternion message
- *
- * @return Quaternion component 2
- */
-static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  8);
-}
-
-/**
- * @brief Get field q3 from attitude_quaternion message
- *
- * @return Quaternion component 3
- */
-static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  12);
-}
-
-/**
- * @brief Get field q4 from attitude_quaternion message
- *
- * @return Quaternion component 4
- */
-static inline float mavlink_msg_attitude_quaternion_get_q4(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  16);
-}
-
-/**
- * @brief Get field rollspeed from attitude_quaternion message
- *
- * @return Roll angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_quaternion_get_rollspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  20);
-}
-
-/**
- * @brief Get field pitchspeed from attitude_quaternion message
- *
- * @return Pitch angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_quaternion_get_pitchspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  24);
-}
-
-/**
- * @brief Get field yawspeed from attitude_quaternion message
- *
- * @return Yaw angular speed (rad/s)
- */
-static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_float(msg,  28);
-}
-
-/**
- * @brief Decode a attitude_quaternion message into a struct
- *
- * @param msg The message to decode
- * @param attitude_quaternion C-struct to decode the message contents into
- */
-static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_t* msg, mavlink_attitude_quaternion_t* attitude_quaternion)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	attitude_quaternion->time_boot_ms = mavlink_msg_attitude_quaternion_get_time_boot_ms(msg);
-	attitude_quaternion->q1 = mavlink_msg_attitude_quaternion_get_q1(msg);
-	attitude_quaternion->q2 = mavlink_msg_attitude_quaternion_get_q2(msg);
-	attitude_quaternion->q3 = mavlink_msg_attitude_quaternion_get_q3(msg);
-	attitude_quaternion->q4 = mavlink_msg_attitude_quaternion_get_q4(msg);
-	attitude_quaternion->rollspeed = mavlink_msg_attitude_quaternion_get_rollspeed(msg);
-	attitude_quaternion->pitchspeed = mavlink_msg_attitude_quaternion_get_pitchspeed(msg);
-	attitude_quaternion->yawspeed = mavlink_msg_attitude_quaternion_get_yawspeed(msg);
-#else
-	memcpy(attitude_quaternion, _MAV_PAYLOAD(msg), 32);
-#endif
-}
-

--- a/libraries/mavlink/include/common/mavlink_msg_auth_key.h
+++ /dev/null
@@ -1,145 +1,1 @@
-// MESSAGE AUTH_KEY PACKING
 
-#define MAVLINK_MSG_ID_AUTH_KEY 7
-
-typedef struct __mavlink_auth_key_t
-{
- char key[32]; ///< key
-} mavlink_auth_key_t;
-
-#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32
-#define MAVLINK_MSG_ID_7_LEN 32
-
-#define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32
-
-#define MAVLINK_MESSAGE_INFO_AUTH_KEY { \
-	"AUTH_KEY", \
-	1, \
-	{  { "key", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a auth_key message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param key key
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       const char *key)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
-
-	_mav_put_char_array(buf, 0, key, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
-#else
-	mavlink_auth_key_t packet;
-
-	mav_array_memcpy(packet.key, key, sizeof(char)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AUTH_KEY;
-	return mavlink_finalize_message(msg, system_id, component_id, 32, 119);
-}
-
-/**
- * @brief Pack a auth_key message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param key key
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           const char *key)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
-
-	_mav_put_char_array(buf, 0, key, 32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32);
-#else
-	mavlink_auth_key_t packet;
-
-	mav_array_memcpy(packet.key, key, sizeof(char)*32);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_AUTH_KEY;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 119);
-}
-
-/**
- * @brief Encode a auth_key struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param auth_key C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key)
-{
-	return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key);
-}
-
-/**
- * @brief Send a auth_key message
- * @param chan MAVLink channel to send the message
- *
- * @param key key
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[32];
-
-	_mav_put_char_array(buf, 0, key, 32);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, 32, 119);
-#else
-	mavlink_auth_key_t packet;
-
-	mav_array_memcpy(packet.key, key, sizeof(char)*32);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, 32, 119);
-#endif
-}
-
-#endif
-
-// MESSAGE AUTH_KEY UNPACKING
-
-
-/**
- * @brief Get field key from auth_key message
- *
- * @return key
- */
-static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key)
-{
-	return _MAV_RETURN_char_array(msg, key, 32,  0);
-}
-
-/**
- * @brief Decode a auth_key message into a struct
- *
- * @param msg The message to decode
- * @param auth_key C-struct to decode the message contents into
- */
-static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key)
-{
-#if MAVLINK_NEED_BYTE_SWAP
-	mavlink_msg_auth_key_get_key(msg, auth_key->key);
-#else
-	memcpy(auth_key, _MAV_PAYLOAD(msg), 32);
-#endif
-}
-

--- a/libraries/mavlink/include/common/mavlink_msg_change_operator_control.h
+++ /dev/null
@@ -1,205 +1,1 @@
-// MESSAGE CHANGE_OPERATOR_CONTROL PACKING
 
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5
-
-typedef struct __mavlink_change_operator_control_t
-{
- uint8_t target_system; ///< System the GCS requests control for
- uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV
- uint8_t version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
-} mavlink_change_operator_control_t;
-
-#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28
-#define MAVLINK_MSG_ID_5_LEN 28
-
-#define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25
-
-#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL { \
-	"CHANGE_OPERATOR_CONTROL", \
-	4, \
-	{  { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_t, target_system) }, \
-         { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_t, control_request) }, \
-         { "version", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_t, version) }, \
-         { "passkey", NULL, MAVLINK_TYPE_CHAR, 25, 3, offsetof(mavlink_change_operator_control_t, passkey) }, \
-         } \
-}
-
-
-/**
- * @brief Pack a change_operator_control message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param target_system System the GCS requests control for
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
-						       uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, control_request);
-	_mav_put_uint8_t(buf, 2, version);
-	_mav_put_char_array(buf, 3, passkey, 25);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
-#else
-	mavlink_change_operator_control_t packet;
-	packet.target_system = target_system;
-	packet.control_request = control_request;
-	packet.version = version;
-	mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL;
-	return mavlink_finalize_message(msg, system_id, component_id, 28, 217);
-}
-
-/**
- * @brief Pack a change_operator_control message on a channel
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param chan The MAVLink channel this message was sent over
- * @param msg The MAVLink message to compress the data into
- * @param target_system System the GCS requests control for
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
- * @return length of the message in bytes (excluding serial stream start sign)
- */
-static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
-							   mavlink_message_t* msg,
-						           uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, control_request);
-	_mav_put_uint8_t(buf, 2, version);
-	_mav_put_char_array(buf, 3, passkey, 25);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28);
-#else
-	mavlink_change_operator_control_t packet;
-	packet.target_system = target_system;
-	packet.control_request = control_request;
-	packet.version = version;
-	mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25);
-        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28);
-#endif
-
-	msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL;
-	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 217);
-}
-
-/**
- * @brief Encode a change_operator_control struct into a message
- *
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- * @param change_operator_control C-struct to read the message contents from
- */
-static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control)
-{
-	return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey);
-}
-
-/**
- * @brief Send a change_operator_control message
- * @param chan MAVLink channel to send the message
- *
- * @param target_system System the GCS requests control for
- * @param control_request 0: request control of this MAV, 1: Release control of this MAV
- * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
- */
-#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey)
-{
-#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
-	char buf[28];
-	_mav_put_uint8_t(buf, 0, target_system);
-	_mav_put_uint8_t(buf, 1, control_request);
-	_mav_put_uint8_t(buf, 2, version);
-	_mav_put_char_array(buf, 3, passkey, 25);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, 28, 217);
-#else
-	mavlink_change_operator_control_t packet;
-	packet.target_system = target_system;
-	packet.control_request = control_request;
-	packet.version = version;
-	mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25);
-	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, 28, 217);
-#endif
-}
-
-#endif
-
-// MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING
-
-
-/**
- * @brief Get field target_system from change_operator_control message
- *
- * @return System the GCS requests control for
- */
-static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  0);
-}
-
-/**
- * @brief Get field control_request from change_operator_control message
- *
- * @return 0: request control of this MAV, 1: Release control of this MAV
- */
-static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  1);
-}
-
-/**
- * @brief Get field version from change_operator_control message
- *
- * @return 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
- */
-static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg)
-{
-	return _MAV_RETURN_uint8_t(msg,  2);
-}
<