Updated MAVLink to most recent

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

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Apr 17 13:14:54 2012"
+#define MAVLINK_BUILD_DATE "Tue Apr 17 14:16:31 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

mavlink/include/v1.0/common/common.h

@@ -64,6 +64,32 @@ enum MAV_AUTOPILOT
 };
 #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

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

@@ -5,8 +5,8 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Apr 17 13:42:19 2012"
+#define MAVLINK_BUILD_DATE "Tue Apr 17 14:16:35 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
-#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
+#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
 #endif // MAVLINK_VERSION_H

mavlink/include/v1.0/mavlink_protobuf_manager.hpp

+#ifndef MAVLINKPROTOBUFMANAGER_HPP
+#define MAVLINKPROTOBUFMANAGER_HPP
+
+#include <deque>
+#include <google/protobuf/message.h>
+#include <iostream>
+#include <tr1/memory>
+
+#include <checksum.h>
+#include <common/mavlink.h>
+#include <mavlink_types.h>
+#include <pixhawk/pixhawk.pb.h>
+
+namespace mavlink
+{
+
+class ProtobufManager
+{
+public:
+	ProtobufManager()
+	 : mRegisteredTypeCount(0)
+	 , mStreamID(0)
+	 , mVerbose(false)
+	 , kExtendedHeaderSize(MAVLINK_EXTENDED_HEADER_LEN)
+	 , kExtendedPayloadMaxSize(MAVLINK_MAX_EXTENDED_PAYLOAD_LEN)
+	{
+		// register GLOverlay
+		{
+			std::tr1::shared_ptr<px::GLOverlay> msg(new px::GLOverlay);
+			registerType(msg);
+		}
+
+		// register ObstacleList
+		{
+			std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
+			registerType(msg);
+		}
+
+		// register ObstacleMap
+		{
+			std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
+			registerType(msg);
+		}
+
+		// register Path
+		{
+			std::tr1::shared_ptr<px::Path> msg(new px::Path);
+                        registerType(msg);
+		}
+
+		// register PointCloudXYZI
+		{
+			std::tr1::shared_ptr<px::PointCloudXYZI> msg(new px::PointCloudXYZI);
+			registerType(msg);
+		}
+
+		// register PointCloudXYZRGB
+		{
+			std::tr1::shared_ptr<px::PointCloudXYZRGB> msg(new px::PointCloudXYZRGB);
+			registerType(msg);
+		}
+
+		// register RGBDImage
+		{
+			std::tr1::shared_ptr<px::RGBDImage> msg(new px::RGBDImage);
+			registerType(msg);
+		}
+
+		srand(time(NULL));
+		mStreamID = rand() + 1;
+	}
+
+	bool fragmentMessage(uint8_t system_id, uint8_t component_id,
+						 uint8_t target_system, uint8_t target_component,
+						 const google::protobuf::Message& protobuf_msg,
+						 std::vector<mavlink_extended_message_t>& fragments) const
+	{
+		TypeMap::const_iterator it = mTypeMap.find(protobuf_msg.GetTypeName());
+		if (it == mTypeMap.end())
+		{
+			std::cout << "# WARNING: Protobuf message with type "
+					  << protobuf_msg.GetTypeName() << " is not registered."
+					  << std::endl;
+			return false;
+		}
+
+		uint8_t typecode = it->second;
+
+		std::string data = protobuf_msg.SerializeAsString();
+
+		int fragmentCount = (protobuf_msg.ByteSize() + kExtendedPayloadMaxSize - 1) / kExtendedPayloadMaxSize;
+		unsigned int offset = 0;
+
+		for (int i = 0; i < fragmentCount; ++i)
+		{
+			mavlink_extended_message_t fragment;			
+
+			// write extended header data
+			uint8_t* payload = reinterpret_cast<uint8_t*>(fragment.base_msg.payload64);
+			unsigned int length = 0;
+			uint8_t flags = 0;
+
+			if (i < fragmentCount - 1)
+			{
+				length = kExtendedPayloadMaxSize;
+				flags |= 0x1;
+			}
+			else
+			{
+				length = protobuf_msg.ByteSize() - kExtendedPayloadMaxSize * (fragmentCount - 1);
+			}
+
+			memcpy(payload, &target_system, 1);
+			memcpy(payload + 1, &target_component, 1);
+			memcpy(payload + 2, &typecode, 1);
+			memcpy(payload + 3, &length, 4);
+			memcpy(payload + 7, &mStreamID, 2);
+			memcpy(payload + 9, &offset, 4);
+			memcpy(payload + 13, &flags, 1);
+
+			fragment.base_msg.msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE;
+			mavlink_finalize_message(&fragment.base_msg, system_id, component_id, kExtendedHeaderSize, 0);
+			
+			// write extended payload data
+			fragment.extended_payload_len = length;
+			memcpy(fragment.extended_payload, &data[offset], length);
+
+			fragments.push_back(fragment);
+			offset += length;
+		}
+
+		if (mVerbose)
+		{
+			std::cerr << "# INFO: Split extended message with size "
+					  << protobuf_msg.ByteSize() << " into "
+					  << fragmentCount << " fragments." << std::endl;
+		}
+
+		return true;
+	}
+
+	bool cacheFragment(mavlink_extended_message_t& msg)
+	{
+		if (!validFragment(msg))
+		{
+			if (mVerbose)
+			{
+				std::cerr << "# WARNING: Message is not a valid fragment. "
+						  << "Dropping message..." << std::endl;
+			}
+			return false;
+		}
+
+		// read extended header
+		uint8_t* payload = reinterpret_cast<uint8_t*>(msg.base_msg.payload64);
+		uint8_t typecode = 0;
+		unsigned int length = 0;
+		unsigned short streamID = 0;
+		unsigned int offset = 0;
+		uint8_t flags = 0;
+
+		memcpy(&typecode, payload + 2, 1);
+		memcpy(&length, payload + 3, 4);
+		memcpy(&streamID, payload + 7, 2);
+		memcpy(&offset, payload + 9, 4);
+		memcpy(&flags, payload + 13, 1);
+
+		if (typecode >= mTypeMap.size())
+		{
+			std::cout << "# WARNING: Protobuf message with type code "
+					  << static_cast<int>(typecode) << " is not registered." << std::endl;
+			return false;
+		}
+
+		bool reassemble = false;
+
+		FragmentQueue::iterator it = mFragmentQueue.find(streamID);
+		if (it == mFragmentQueue.end())
+		{
+			if (offset == 0)
+			{
+				mFragmentQueue[streamID].push_back(msg);
+
+				if ((flags & 0x1) != 0x1)
+				{
+					reassemble = true;
+				}
+
+				if (mVerbose)
+				{
+					std::cerr << "# INFO: Added fragment to new queue."
+							  << std::endl;
+				}
+			}
+			else
+			{
+				if (mVerbose)
+				{
+					std::cerr << "# WARNING: Message is not a valid fragment. "
+							  << "Dropping message..." << std::endl;
+				}
+			}
+		}
+		else
+		{
+			std::deque<mavlink_extended_message_t>& queue = it->second;
+
+			if (queue.empty())
+			{
+				if (offset == 0)
+				{
+					queue.push_back(msg);
+
+					if ((flags & 0x1) != 0x1)
+					{
+						reassemble = true;
+					}
+				}
+				else
+				{
+					if (mVerbose)
+					{
+						std::cerr << "# WARNING: Message is not a valid fragment. "
+								  << "Dropping message..." << std::endl;
+					}
+				}
+			}
+			else
+			{
+				if (fragmentDataSize(queue.back()) + fragmentOffset(queue.back()) != offset)
+				{
+					if (mVerbose)
+					{
+						std::cerr << "# WARNING: Previous fragment(s) have been lost. "
+								  << "Dropping message and clearing queue..." << std::endl;
+					}
+					queue.clear();
+				}
+				else
+				{
+					queue.push_back(msg);
+
+					if ((flags & 0x1) != 0x1)
+					{
+						reassemble = true;
+					}
+				}
+			}
+		}
+
+		if (reassemble)
+		{
+			std::deque<mavlink_extended_message_t>& queue = mFragmentQueue[streamID];
+
+			std::string data;
+			for (size_t i = 0; i < queue.size(); ++i)
+			{
+				mavlink_extended_message_t& mavlink_msg = queue.at(i);
+
+				data.append(reinterpret_cast<char*>(&mavlink_msg.extended_payload[0]),
+							static_cast<size_t>(mavlink_msg.extended_payload_len));
+			}
+
+			mMessages.at(typecode)->ParseFromString(data);
+
+			mMessageAvailable.at(typecode) = true;
+
+			queue.clear();
+
+			if (mVerbose)
+			{
+				std::cerr << "# INFO: Reassembled fragments for message with typename "
+						  << mMessages.at(typecode)->GetTypeName() << " and size "
+						  << mMessages.at(typecode)->ByteSize()
+						  << "." << std::endl;
+			}
+		}
+
+		return true;
+	}
+
+	bool getMessage(std::tr1::shared_ptr<google::protobuf::Message>& msg)
+	{
+		for (size_t i = 0; i < mMessageAvailable.size(); ++i)
+		{
+			if (mMessageAvailable.at(i))
+			{
+				msg = mMessages.at(i);
+				mMessageAvailable.at(i) = false;
+
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+private:
+	void registerType(const std::tr1::shared_ptr<google::protobuf::Message>& msg)
+	{
+		mTypeMap[msg->GetTypeName()] = mRegisteredTypeCount;
+		++mRegisteredTypeCount;
+		mMessages.push_back(msg);
+		mMessageAvailable.push_back(false);
+	}
+
+	bool validFragment(const mavlink_extended_message_t& msg) const
+	{
+		if (msg.base_msg.magic != MAVLINK_STX ||
+			msg.base_msg.len != kExtendedHeaderSize ||
+			msg.base_msg.msgid != MAVLINK_MSG_ID_EXTENDED_MESSAGE)
+		{
+			return false;
+		}
+
+		uint16_t checksum;
+		checksum = crc_calculate(reinterpret_cast<const uint8_t*>(&msg.base_msg.len), MAVLINK_CORE_HEADER_LEN);
+		crc_accumulate_buffer(&checksum, reinterpret_cast<const char*>(&msg.base_msg.payload64), kExtendedHeaderSize);
+#if MAVLINK_CRC_EXTRA
+		static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+		crc_accumulate(mavlink_message_crcs[msg.base_msg.msgid], &checksum);
+#endif
+
+		if (mavlink_ck_a(&(msg.base_msg)) != (uint8_t)(checksum & 0xFF) &&
+		    mavlink_ck_b(&(msg.base_msg)) != (uint8_t)(checksum >> 8))
+		{
+			return false;
+		}
+
+		return true;
+	}
+
+	unsigned int fragmentDataSize(const mavlink_extended_message_t& msg) const
+	{
+		const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
+
+		return *(reinterpret_cast<const unsigned int*>(payload + 3));
+	}
+
+	unsigned int fragmentOffset(const mavlink_extended_message_t& msg) const
+	{
+		const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
+
+		return *(reinterpret_cast<const unsigned int*>(payload + 9));
+	}
+
+	int mRegisteredTypeCount;
+	unsigned short mStreamID;
+	bool mVerbose;
+
+	typedef std::map<std::string, uint8_t> TypeMap;
+	TypeMap mTypeMap;
+	std::vector< std::tr1::shared_ptr<google::protobuf::Message> > mMessages;
+	std::vector<bool> mMessageAvailable;
+
+	typedef std::map<unsigned short, std::deque<mavlink_extended_message_t> > FragmentQueue;
+	FragmentQueue mFragmentQueue;
+
+	const int kExtendedHeaderSize;
+	/**
+	 * Extended header structure
+	 * =========================
+	 *   byte 0 - target_system
+	 *   byte 1 - target_component
+	 *   byte 2 - extended message id (type code)
+	 *   bytes 3-6 - extended payload size in bytes
+	 *   byte 7-8 - stream ID
+	 *   byte 9-12 - fragment offset
+	 *   byte 13 - fragment flags (bit 0 - 1=more fragments, 0=last fragment)
+	 */
+
+	const int kExtendedPayloadMaxSize;
+};
+
+}
+
+#endif

mavlink/include/v1.0/pixhawk/pixhawk.h

@@ -12,11 +12,11 @@ extern "C" {
 // 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, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 8, 255, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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}
+#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, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 12, 255, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 148, 223, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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}
+#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, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 23, 223, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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

mavlink/include/v1.0/pixhawk/testsuite.h

@@ -860,14 +860,18 @@ static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t
         uint16_t i;
 	mavlink_data_transmission_handshake_t packet_in = {
 		963497464,
-	17,
-	84,
-	151,
-	218,
+	17443,
+	17547,
+	29,
+	96,
+	163,
+	230,
 	};
 	mavlink_data_transmission_handshake_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
         	packet1.size = packet_in.size;
+        	packet1.width = packet_in.width;
+        	packet1.height = packet_in.height;
         	packet1.type = packet_in.type;
         	packet1.packets = packet_in.packets;
         	packet1.payload = packet_in.payload;
@@ -881,12 +885,12 @@ static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_transmission_handshake_pack(system_id, component_id, &msg , packet1.type , packet1.size , packet1.packets , packet1.payload , packet1.jpg_quality );
+	mavlink_msg_data_transmission_handshake_pack(system_id, component_id, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
 	mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.size , packet1.packets , packet1.payload , packet1.jpg_quality );
+	mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
 	mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
@@ -899,7 +903,7 @@ static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
         
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.packets , packet1.payload , packet1.jpg_quality );
+	mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
 	mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }

mavlink/include/v1.0/pixhawk/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Apr 17 13:14:38 2012"
+#define MAVLINK_BUILD_DATE "Tue Apr 17 14:16:35 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  

mavlink/include/v1.0/sensesoar/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Apr 17 13:42:19 2012"
+#define MAVLINK_BUILD_DATE "Tue Apr 17 14:16:29 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

mavlink/include/v1.0/test/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Apr 17 13:45:37 2012"
+#define MAVLINK_BUILD_DATE "Tue Apr 17 14:13:26 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179
  

src/comm/MAVLinkProtocol.cc

@@ -217,8 +217,18 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 
                 // read extended header
                 uint8_t* payload = reinterpret_cast<uint8_t*>(message.payload64);
+
                 memcpy(&extended_message.extended_payload_len, payload + 3, 4);
 
+                // Check if message is valid
+                if
+                 (b.size() != MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_EXTENDED_HEADER_LEN+ extended_message.extended_payload_len)
+                {
+                    //invalid message
+                    qDebug() << "GOT INVALID EXTENDED MESSAGE, ABORTING";
+                    return;
+                }
+
                 const uint8_t* extended_payload = reinterpret_cast<const uint8_t*>(b.constData()) + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_EXTENDED_HEADER_LEN;
 
                 // copy extended payload data