diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h
index d684bd6a17..eafeec6217 100644
--- a/libraries/GCS_MAVLink/include/common/common.h
+++ b/libraries/GCS_MAVLink/include/common/common.h
@@ -102,7 +102,6 @@ enum MAV_DATA_STREAM
 #include "./mavlink_msg_raw_imu.h"
 #include "./mavlink_msg_raw_pressure.h"
 #include "./mavlink_msg_attitude.h"
-#include "./mavlink_msg_attitude_new.h"
 #include "./mavlink_msg_local_position.h"
 #include "./mavlink_msg_global_position.h"
 #include "./mavlink_msg_gps_raw.h"
diff --git a/libraries/GCS_MAVLink/include/mavlink_types.h b/libraries/GCS_MAVLink/include/mavlink_types.h
index 1c92af3762..609cb7071f 100644
--- a/libraries/GCS_MAVLink/include/mavlink_types.h
+++ b/libraries/GCS_MAVLink/include/mavlink_types.h
@@ -179,21 +179,10 @@ typedef enum {
     MAVLINK_COMM_0,
     MAVLINK_COMM_1,
     MAVLINK_COMM_2,
-    MAVLINK_COMM_3
-} mavlink_channel_t;
-
-/*
- * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
- * of buffers they will use. If more are used, then the result will be
- * a stack overrun
- */
-#ifndef MAVLINK_COMM_NUM_BUFFERS
-#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
-# define MAVLINK_COMM_NUM_BUFFERS 16
-#else
-# define MAVLINK_COMM_NUM_BUFFERS 4
-#endif
-#endif
+    MAVLINK_COMM_3,
+    MAVLINK_COMM_NB,
+    MAVLINK_COMM_NB_HIGH = 16
+                       } mavlink_channel_t;
 
 typedef enum {
     MAVLINK_PARSE_STATE_UNINIT=0,
diff --git a/libraries/GCS_MAVLink/include/protocol.h b/libraries/GCS_MAVLink/include/protocol.h
index ed1f9546b4..c6aace92e8 100644
--- a/libraries/GCS_MAVLink/include/protocol.h
+++ b/libraries/GCS_MAVLink/include/protocol.h
@@ -34,7 +34,12 @@ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus)
 
 static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
 {
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
+#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
+	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
+#else
+	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
+#endif
+
 	return &m_mavlink_status[chan];
 }
 
@@ -68,7 +73,7 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t
 	msg->compid = component_id;
 	// One sequence number per component
 	msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq;
-    mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1;
+        mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1;
 	checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN);
 	msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
 	msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
@@ -197,8 +202,11 @@ static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
  */
 static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
 {
-	static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
-
+#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
+	static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
+#else
+	static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
+#endif
 	// Initializes only once, values keep unchanged after first initialization
 	mavlink_parse_state_initialize(mavlink_get_channel_status(chan));
 
@@ -383,13 +391,23 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag
 /*
 static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
 {
-	    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	    static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2];
-	    static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS];
-	    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
-	    static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES];
-	    static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS];
-	    static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS];
+	#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
+	    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
+	    static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2];
+	    static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH];
+	    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
+	    static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES];
+	    static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH];
+	    static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH];
+	#else
+	    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
+	    static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2];
+	    static uint8_t m_msgbuf_index[MAVLINK_COMM_NB];
+	    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
+	    static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES];
+	    static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB];
+	    static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB];
+	#endif
 
 	    // Set a packet start candidate index if sign is start sign
 	    if (c == MAVLINK_STX)
@@ -399,8 +417,13 @@ static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_me
 
 	    // Parse normally, if a CRC mismatch occurs retry with the next packet index
 }
-//    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-//    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
+//#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
+//    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
+//    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
+//#else
+//    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
+//    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
+//#endif
 //// Initializes only once, values keep unchanged after first initialization
 //    mavlink_parse_state_initialize(&m_mavlink_status[chan]);
 //
@@ -552,7 +575,6 @@ typedef union __generic_64bit
 {
 	uint8_t b[8];
 	int64_t ll; ///< Long long (64 bit)
-	double  d;  ///< IEEE-754 double precision floating point
 } generic_64bit;
 
 /**
@@ -861,103 +883,6 @@ void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
 }
  */
 
-static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum)
-{
-	crc_accumulate(b, checksum);
-	comm_send_ch(chan, b);
-}
-
-static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum)
-{
-	crc_accumulate(b, checksum);
-	comm_send_ch(chan, b);
-}
-
-static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum)
-{
-	char s;
-	s = (b>>8)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b & 0xff);
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-}
-
-static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum)
-{
-	mavlink_send_uart_uint16_t(chan, b, checksum);
-}
-
-static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum)
-{
-	char s;
-	s = (b>>24)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>16)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>8)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b & 0xff);
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-}
-
-static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum)
-{
-	mavlink_send_uart_uint32_t(chan, b, checksum);
-}
-
-static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum)
-{
-	char s;
-	s = (b>>56)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>48)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>40)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>32)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>24)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>16)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b>>8)&0xff;
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-	s = (b & 0xff);
-	comm_send_ch(chan, s);
-	crc_accumulate(s, checksum);
-}
-
-static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum)
-{
-	mavlink_send_uart_uint64_t(chan, b, checksum);
-}
-
-static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum)
-{
-	generic_32bit g;
-	g.f = b;
-	mavlink_send_uart_uint32_t(chan, g.i, checksum);
-}
-
-static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum)
-{
-	generic_64bit g;
-	g.d = b;
-	mavlink_send_uart_uint32_t(chan, g.ll, checksum);
-}
 
 static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg)
 {