GCS_MAVLink: Regenerate mavlink1.0 implementation

created using:
mavgen.py --lang=C --wire-protocol=1.0 --output=./include/mavlink/v1.0 ./message_definitions/ardupilotmega.xml
mavlink repo commit 79cabfe956464c8f745807a6e65062ac3fc09193

libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h

@@ -0,0 +1,182 @@
+// 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_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[18];
+	_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, 18);
+#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, 18);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_DATA16;
+	return mavlink_finalize_message(msg, system_id, component_id, 18, 234);
+}
+
+/**
+ * @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[18];
+	_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, 18);
+#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, 18);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_DATA16;
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 234);
+}
+
+/**
+ * @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[18];
+	_mav_put_uint8_t(buf, 0, type);
+	_mav_put_uint8_t(buf, 1, len);
+	_mav_put_uint8_t_array(buf, 2, data, 16);
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, 18, 234);
+#else
+	mavlink_data16_t packet;
+	packet.type = type;
+	packet.len = len;
+	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*16);
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)&packet, 18, 234);
+#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), 18);
+#endif
+}

libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h

@@ -0,0 +1,182 @@
+// 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_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[34];
+	_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, 34);
+#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, 34);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_DATA32;
+	return mavlink_finalize_message(msg, system_id, component_id, 34, 73);
+}
+
+/**
+ * @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[34];
+	_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, 34);
+#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, 34);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_DATA32;
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 34, 73);
+}
+
+/**
+ * @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[34];
+	_mav_put_uint8_t(buf, 0, type);
+	_mav_put_uint8_t(buf, 1, len);
+	_mav_put_uint8_t_array(buf, 2, data, 32);
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, 34, 73);
+#else
+	mavlink_data32_t packet;
+	packet.type = type;
+	packet.len = len;
+	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*32);
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)&packet, 34, 73);
+#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), 34);
+#endif
+}

libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h

@@ -992,6 +992,100 @@ static void mavlink_test_wind(uint8_t system_id, uint8_t component_id, mavlink_m
         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_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_test_sensor_offsets(system_id, component_id, last_msg);
@@ -1012,6 +1106,8 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
 	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);
 }
 
 #ifdef __cplusplus

libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Aug 15 07:29:40 2012"
+#define MAVLINK_BUILD_DATE "Tue Sep 18 11:41:40 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Wed Aug 15 07:29:40 2012"
+#define MAVLINK_BUILD_DATE "Tue Sep 18 11:41:40 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_helpers.h

@@ -9,8 +9,17 @@
 #define MAVLINK_HELPER
 #endif
 
+/**
+ * Reset the status of a channel
+ */
+MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
+{
+	mavlink_status_t *status = mavlink_get_channel_status(chan);
+	status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+}
+
 /*
-  internal function to give access to the channel status for each channel
+ * Internal function to give access to the channel status for each channel
  */
 MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
 {
@@ -19,7 +28,7 @@ MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
 }
 
 /*
- internal function to give access to the channel buffer for each channel
+ * Internal function to give access to the channel buffer for each channel
  */
 MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
 {
@@ -227,6 +236,19 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
 	static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
 #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
 #endif
+#endif
+
+/* Enable this option to check the length of each message.
+ This allows invalid messages to be caught much sooner. Use if the transmission
+ medium is prone to missing (or extra) characters (e.g. a radio that fades in
+ and out). Only use if the channel will only contain messages types listed in
+ the headers.
+*/
+#if MAVLINK_CHECK_MESSAGE_LENGTH
+#ifndef MAVLINK_MESSAGE_LENGTH
+	static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
+#endif
 #endif
 
 	mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message
@@ -291,6 +313,19 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
 		break;
 
 	case MAVLINK_PARSE_STATE_GOT_COMPID:
+#if MAVLINK_CHECK_MESSAGE_LENGTH
+	        if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
+		{
+			status->parse_error++;
+			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+			break;
+			if (c == MAVLINK_STX)
+			{
+				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
+				mavlink_start_checksum(rxmsg);
+			}
+	        }
+#endif
 		rxmsg->msgid = c;
 		mavlink_update_checksum(rxmsg, c);
 		if (rxmsg->len == 0)

libraries/GCS_MAVLink/include/mavlink/v1.0/protocol.h

@@ -43,6 +43,7 @@
 
 /* always include the prototypes to ensure we don't get out of sync */
 MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
+MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
 #if MAVLINK_CRC_EXTRA
 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
 						      uint8_t chan, uint8_t length, uint8_t crc_extra);