diff --git a/files/mavlink_generator/C/include_v1.0/mavlink_helpers.h b/files/mavlink_generator/C/include_v1.0/mavlink_helpers.h
index 6eb8f0ba2a554bf3a220ffcbbb922cbf9274dbdf..6799b6077db86f7793d2743a4a8778a6ba4f74fb 100644
--- a/files/mavlink_generator/C/include_v1.0/mavlink_helpers.h
+++ b/files/mavlink_generator/C/include_v1.0/mavlink_helpers.h
@@ -120,7 +120,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
  */
 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
 {
-	memcpy(buffer, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
+	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
 	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
 }
 
@@ -182,7 +182,15 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
  */
 MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
 {
+#if MAVLINK_EXTERNAL_RX_BUFFER
+	// No m_mavlink_message array defined in function,
+	// has to be defined externally
+#ifndef m_mavlink_message
+#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION
+#endif
+#else
 	static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
+#endif
 
         /*
 	  default message crc function. You can override this per-system to
@@ -209,6 +217,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
 		{
 			status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
 			rxmsg->len = 0;
+			rxmsg->magic = c;
 			mavlink_start_checksum(rxmsg);
 		}
 		break;
@@ -269,7 +278,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
 		break;
 
 	case MAVLINK_PARSE_STATE_GOT_MSGID:
-		_MAV_PAYLOAD(rxmsg)[status->packet_idx++] = (char)c;
+		_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
 		mavlink_update_checksum(rxmsg, c);
 		if (status->packet_idx == rxmsg->len)
 		{
@@ -296,6 +305,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
 		else
 		{
 			status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
+			_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
 		}
 		break;
 
@@ -317,6 +327,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
 			// Successfully got message
 			status->msg_received = 1;
 			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
+			_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
 			memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
 		}
 		break;
diff --git a/files/mavlink_generator/C/include_v1.0/mavlink_types.h b/files/mavlink_generator/C/include_v1.0/mavlink_types.h
index 571dc50872af5eefeebd98546cfdeae2b6240e35..3827834078b2dc8c12c692bfa35b7e4610f9cc27 100644
--- a/files/mavlink_generator/C/include_v1.0/mavlink_types.h
+++ b/files/mavlink_generator/C/include_v1.0/mavlink_types.h
@@ -1,6 +1,8 @@
 #ifndef MAVLINK_TYPES_H_
 #define MAVLINK_TYPES_H_
 
+#include <inttypes.h>
+
 enum MAV_ACTION
 {
     MAV_ACTION_HOLD = 0,
@@ -66,6 +68,8 @@ typedef struct param_union {
 		float param_float;
 		int32_t param_int32;
 		uint32_t param_uint32;
+		uint8_t param_uint8;
+		uint8_t bytes[4];
 	};
 	uint8_t type;
 } mavlink_param_union_t;
@@ -107,12 +111,12 @@ typedef enum {
 #define MAVLINK_MAX_FIELDS 64
 
 typedef struct __mavlink_field_info {
-	const char *name;             // name of this field
-	const char *print_format;     // printing format hint, or NULL
-	mavlink_message_type_t type;  // type of this field
-	unsigned array_length;        // if non-zero, field is an array
-	unsigned wire_offset;         // offset of each field in the payload
-	unsigned structure_offset;    // offset in a C structure
+        const char *name;                 // name of this field
+        const char *print_format;         // printing format hint, or NULL
+        mavlink_message_type_t type;      // type of this field
+        unsigned int array_length;        // if non-zero, field is an array
+        unsigned int wire_offset;         // offset of each field in the payload
+        unsigned int structure_offset;    // offset in a C structure
 } mavlink_field_info_t;
 
 // note that in this structure the order of fields is the order
@@ -123,11 +127,12 @@ typedef struct __mavlink_message_info {
 	mavlink_field_info_t fields[MAVLINK_MAX_FIELDS];       // field information
 } mavlink_message_info_t;
 
-#define _MAV_PAYLOAD(msg) ((char *)(&(msg)->payload64[0]))
+#define _MAV_PAYLOAD(msg) ((const char *)(&(msg)->payload64[0]))
+#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)((char *)(&(msg)->payload64[0])))
 
 // checksum is immediately after the payload bytes
-#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD(msg))
-#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD(msg))
+#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
+#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
 
 typedef enum {
     MAVLINK_COMM_0,
diff --git a/files/mavlink_generator/C/include_v1.0/protocol.h b/files/mavlink_generator/C/include_v1.0/protocol.h
index 27c2396836e839638ef2d0a186442943346b28e3..7b3e3c0bd2112c9516e694b729cbc990d2e766bc 100644
--- a/files/mavlink_generator/C/include_v1.0/protocol.h
+++ b/files/mavlink_generator/C/include_v1.0/protocol.h
@@ -153,17 +153,25 @@ static inline void byte_copy_8(char *dst, const char *src)
 #define _mav_put_double(buf, wire_offset, b)   *(double *)&buf[wire_offset] = b
 #endif
 
+/*
+  like memcpy(), but if src is NULL, do a memset to zero
+*/
+static void mav_array_memcpy(void *dest, const void *src, size_t n)
+{
+	if (src == NULL) {
+		memset(dest, 0, n);
+	} else {
+		memcpy(dest, src, n);
+	}
+}
 
 /*
  * Place a char array into a buffer
  */
 static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
 {
-	if (b == NULL) {
-		memset(&buf[wire_offset], 0, array_length);
-	} else {
-		memcpy(&buf[wire_offset], b, array_length);
-	}
+	mav_array_memcpy(&buf[wire_offset], b, array_length);
+
 }
 
 /*
@@ -171,11 +179,8 @@ static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const cha
  */
 static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
 {
-	if (b == NULL) {
-		memset(&buf[wire_offset], 0, array_length);
-	} else {
-		memcpy(&buf[wire_offset], b, array_length);
-	}
+	mav_array_memcpy(&buf[wire_offset], b, array_length);
+
 }
 
 /*
@@ -183,11 +188,8 @@ static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const
  */
 static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
 {
-	if (b == NULL) {
-		memset(&buf[wire_offset], 0, array_length);
-	} else {
-		memcpy(&buf[wire_offset], b, array_length);
-	}
+	mav_array_memcpy(&buf[wire_offset], b, array_length);
+
 }
 
 #if MAVLINK_NEED_BYTE_SWAP
@@ -207,11 +209,7 @@ static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, co
 #define _MAV_PUT_ARRAY(TYPE, V)					\
 static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
 { \
-	if (b == NULL) { \
-		memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
-	} else { \
-		memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
-	} \
+	mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
 }
 #endif
 
@@ -224,9 +222,9 @@ _MAV_PUT_ARRAY(int64_t,  i64)
 _MAV_PUT_ARRAY(float,    f)
 _MAV_PUT_ARRAY(double,   d)
 
-#define _MAV_RETURN_char(msg, wire_offset)             _MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_int8_t(msg, wire_offset)   (int8_t)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_int8_t(msg, wire_offset)   (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
 
 #if MAVLINK_NEED_BYTE_SWAP
 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
diff --git a/files/mavlink_generator/generator/_ctypes.pyd b/files/mavlink_generator/generator/_ctypes.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..ae456be8063dc5776e31bda4300a9c04304a2511
Binary files /dev/null and b/files/mavlink_generator/generator/_ctypes.pyd differ
diff --git a/files/mavlink_generator/generator/_hashlib.pyd b/files/mavlink_generator/generator/_hashlib.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..5efea4ac7366e80f01d857462a066a1bbb74adfa
Binary files /dev/null and b/files/mavlink_generator/generator/_hashlib.pyd differ
diff --git a/files/mavlink_generator/generator/_socket.pyd b/files/mavlink_generator/generator/_socket.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..b4b3b1fe60899deb850110a1aacd46e956722ab0
Binary files /dev/null and b/files/mavlink_generator/generator/_socket.pyd differ
diff --git a/files/mavlink_generator/generator/_ssl.pyd b/files/mavlink_generator/generator/_ssl.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..c054f240d67ff88e4fb9f6745858b0365099829e
Binary files /dev/null and b/files/mavlink_generator/generator/_ssl.pyd differ
diff --git a/files/mavlink_generator/generator/bz2.pyd b/files/mavlink_generator/generator/bz2.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..df1a20e4c98d752349fefb7af3952102aa7cd2f4
Binary files /dev/null and b/files/mavlink_generator/generator/bz2.pyd differ
diff --git a/files/mavlink_generator/generator/library.zip b/files/mavlink_generator/generator/library.zip
new file mode 100644
index 0000000000000000000000000000000000000000..fb48826192e9366422e77132c1dcfa05e616dd1f
Binary files /dev/null and b/files/mavlink_generator/generator/library.zip differ
diff --git a/files/mavlink_generator/generator/mavgen.exe b/files/mavlink_generator/generator/mavgen.exe
new file mode 100644
index 0000000000000000000000000000000000000000..b59dd82c45c0f4288c3ece8bd02f4ea3f1f72379
Binary files /dev/null and b/files/mavlink_generator/generator/mavgen.exe differ
diff --git a/files/mavlink_generator/generator/pyexpat.pyd b/files/mavlink_generator/generator/pyexpat.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..03c607001ccfa956e83f38fcab4edc5eab33899b
Binary files /dev/null and b/files/mavlink_generator/generator/pyexpat.pyd differ
diff --git a/files/mavlink_generator/generator/python27.dll b/files/mavlink_generator/generator/python27.dll
new file mode 100644
index 0000000000000000000000000000000000000000..9b03b95e80bf87624389f3c348fcfbfd43d8db8e
Binary files /dev/null and b/files/mavlink_generator/generator/python27.dll differ
diff --git a/files/mavlink_generator/generator/select.pyd b/files/mavlink_generator/generator/select.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..e1b0ec2c49f1108286fcd941a922070976c0e9ab
Binary files /dev/null and b/files/mavlink_generator/generator/select.pyd differ
diff --git a/files/mavlink_generator/generator/unicodedata.pyd b/files/mavlink_generator/generator/unicodedata.pyd
new file mode 100644
index 0000000000000000000000000000000000000000..a5c460d6fb27d2c938ea4b3c7401ed2358e14e3a
Binary files /dev/null and b/files/mavlink_generator/generator/unicodedata.pyd differ
diff --git a/files/mavlink_generator/generator/w9xpopen.exe b/files/mavlink_generator/generator/w9xpopen.exe
new file mode 100644
index 0000000000000000000000000000000000000000..f6033fdeaaaf14dc80579adeb1a8827f08d1de4f
Binary files /dev/null and b/files/mavlink_generator/generator/w9xpopen.exe differ
diff --git a/images/earth-singlesystem.html b/images/earth-singlesystem.html
new file mode 100644
index 0000000000000000000000000000000000000000..5091ce347d391c2ac7a7d1bf0ada9520905db346
--- /dev/null
+++ b/images/earth-singlesystem.html
@@ -0,0 +1,759 @@
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+
+<html>
+  <head>
+  <meta http-equiv="content-type" content="text/html; charset=utf-8" />
+  <!-- <head> -->
+<!-- QGroundControl -->
+    <title>QGroundControl Google Earth View</title>
+    <!-- *** Replace the key below below with your own API key, available at http://code.google.com/apis/maps/signup.html *** -->
+    <script type="text/javascript" src="https://getfirebug.com/firebug-lite-beta.js"></script>
+    <script type="text/javascript" src="https://www.google.com/jsapi?key=ABQIAAAA5Q6wxQ6lxKS8haLVdUJaqhSjosg_0jiTTs2iXtkDVG0n0If1mBRHzhWw5VqBZX-j4NuzoVpU-UaHVg"></script>
+    <script type="text/javascript">
+google.load("earth", "1", { 'language': 'en'});
+
+var ge = null;
+var initialized = false;
+var currAircraft = 0;
+var followEnabled = false;
+var lineAltitudeOffset = 0.5; ///< 0.5 m higher than waypoint, prevents the line from entering the ground
+
+var lastLat = 0;
+var lastLon = 0;
+var lastAlt = 0;
+var currLat = 47.3769;
+var currLon = 8.549444;
+var currAlt = 470;
+
+var currentCameraLatitude = 0;
+var currentCameraLongitude = 0;
+var currentCameraAltitude = 0;
+
+var currFollowHeading = 0.0;
+
+var homeLat = 0;
+var homeLon = 0;
+var homeAlt = 0;
+var homeViewRange = 800;
+var homeLocation = null;
+var homeGroundLevel = 0;
+
+var currViewRange = 50.0;	///<< The current viewing range from this position (in meters)
+var currTilt = 40.0;		///<< The tilt angle (in degrees)
+var currFollowTilt = 40.0;
+var currView = null;
+var distanceMode = 0;
+
+var viewMode = 0;
+var lastTilt = currTilt;
+var lastRoll = 0;
+var lastHeading = 0;
+
+var M_PI = 3.14159265;
+
+
+
+var planeOrient;
+var planeLoc;
+
+var aircraft = [];
+var aircraftLocations = [];
+var aircraftLastLocations = [];
+var attitudes = [];
+var locations = [];
+var trails = [];
+var trailPlacemarks = [];
+var trailsVisible = [];
+var trailColors = [];
+var waypoints = [];
+
+var waypointLines = [];
+var waypointLinePlacemarks = [];
+var waypointLineColors = [];
+//var waypointLines = [];
+//var trailPlacemarks[id];
+var lineStyle;
+
+// Aircraft class
+var planeColor = '6600ffff';
+
+// Enable / disable dragging
+var draggingAllowed = true;
+
+// Waypoint interaction flags
+var dragInfo = null;
+
+var dragWaypointIndex = "";
+var dragWaypointLatitude = 0;
+var dragWaypointLongitude = 0;
+var dragWaypointAltitude = 0;
+var dragWaypointPending = false;
+
+// Waypoint creation flags
+var newWaypointLatitude = 0;
+var newWaypointLongitude = 0;
+var newWaypointAltitude = 0;
+var newWaypointPending = false;
+
+var clickMode = 0;
+
+var homePlacemark = null;
+
+
+// Data / heightmap
+var heightMapPlacemark = null;
+var heightMapModel = null;
+
+
+function getGlobal(variable)
+{
+	return variable;
+}
+
+function getDraggingAllowed()
+{
+	return draggingAllowed;
+}
+
+function setDistanceMode(mode)
+{
+	distanceMode = mode;
+}
+
+function setDraggingAllowed(allowed)
+{
+	draggingAllowed = allowed;
+}
+
+function sampleCurrentPosition()
+{
+    var thisView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
+	currentCameraLatitude = thisView.getLatitude();
+	currentCameraLongitude = thisView.getLongitude();
+	currentCameraGroundAltitude = ge.getGlobe().getGroundAltitude(currentCameraLatitude, currentCameraLongitude);
+}
+
+function enableSetHomeMode()
+{
+	clickMode = 1;
+}
+
+function setLookAtLatLon(lat, lon)
+{
+	// Keep the current altitude above ground, just move the position
+	currView = ge.getView().copyAsLookAt(ge.ALTITUDE_RELATIVE_TO_GROUND);
+	currView.setLatitude(lat);
+	currView.setLongitude(lon);
+	ge.getView().setAbstractView(currView);
+}
+
+function setNewWaypointPending(pending)
+{
+	newWaypointPending = pending;
+	document.getElementById('JScript_newWaypointPending').setAttribute('value',pending);
+}
+
+function setDragWaypointPending(pending)
+{
+	dragWaypointPending = pending;
+	document.getElementById('JScript_dragWaypointPending').setAttribute('value',pending);
+}
+
+function isInitialized()
+{
+	return initialized;
+}
+
+
+function init()
+{
+	google.earth.createInstance("map3d", initCallback, failureCallback);
+}
+
+function setFollowEnabled(enable)
+{
+	followEnabled = enable;
+}
+
+function enableEventListener()
+{
+// listen for mousedown on the window (look specifically for point placemarks)
+google.earth.addEventListener(ge.getWindow(), 'mousedown', function(event)
+{
+if (clickMode == 1)
+    {
+    	// Set home mode
+    	dragWaypointIndex = 'HOME';
+    	document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex);
+		dragWaypointLatitude = event.getLatitude();
+		dragWaypointLongitude = event.getLongitude();
+		dragWaypointAltitude = ge.getGlobe().getGroundAltitude(dragWaypointLatitude, dragWaypointLongitude);
+		dragWaypointPending = true;
+		document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude);
+	   document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude);
+	   document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude);
+	   document.getElementById('JScript_dragWaypointPending').setAttribute('value',true);
+	   setGCSHome(dragWaypointLatitude, dragWaypointLongitude, dragWaypointAltitude);
+}
+    
+  if (event.getTarget().getType() == 'KmlPlacemark' &&
+      event.getTarget().getGeometry().getType() == 'KmlPoint') {
+    var placemark = event.getTarget();
+    event.preventDefault();
+      if (draggingAllowed)
+  {
+  	if (clickMode == 0)
+  	{
+    dragInfo = {
+      placemark: event.getTarget(),
+      dragged: false
+    };
+    }
+  }
+  }
+});
+
+// listen for mousemove on the globe
+google.earth.addEventListener(ge.getGlobe(), 'mousemove', function(event)
+{
+  if (draggingAllowed && (clickMode == 0))
+  {
+  	if (dragInfo)
+  	{
+    	event.preventDefault();
+    	var point = dragInfo.placemark.getGeometry();
+    	point.setLatitude(event.getLatitude());
+    	point.setLongitude(event.getLongitude());
+    	dragInfo.dragged = true;
+    	dragWaypointIndex = dragInfo.placemark.getDescription();
+		document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex);
+		dragWaypointLatitude = event.getLatitude();
+		dragWaypointLongitude = event.getLongitude();
+		dragWaypointAltitude = point.getAltitude();
+		dragWaypointPending = true;
+		document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude);
+	    document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude);
+	    document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude);
+	    document.getElementById('JScript_dragWaypointPending').setAttribute('value',true);
+  	}
+  }
+});
+
+// listen for mouseup on the window
+google.earth.addEventListener(ge.getWindow(), 'mouseup', function(event)
+{
+  if (draggingAllowed && (clickMode == 0))
+  {
+  if (dragInfo) {
+    if (dragInfo.dragged)
+    {
+      	// if the placemark was dragged, prevent balloons from popping up
+      	event.preventDefault();
+      	// Get drag end location
+      	dragWaypointIndex = dragInfo.placemark.getDescription();
+		document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex);
+      	var point = dragInfo.placemark.getGeometry();
+      	dragWaypointLatitude = event.getLatitude();
+		dragWaypointLongitude = event.getLongitude();
+		dragWaypointAltitude = point.getAltitude();
+    	dragWaypointPending = true;
+		document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude);
+	   document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude);
+	   document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude);
+	   document.getElementById('JScript_dragWaypointPending').setAttribute('value',true);
+    }
+    
+    dragInfo = null;
+  }
+  }
+      clickMode = 0;
+});
+
+// Listen for wp creation request on the globe
+
+google.earth.addEventListener(ge.getGlobe(), 'dblclick', function(event){
+	if (draggingAllowed)
+	{
+       event.preventDefault();
+       newWaypointLatitude = event.getLatitude();
+       newWaypointLongitude = event.getLongitude();
+       newWaypointAltitude = ge.getGlobe().getGroundAltitude(newWaypointLatitude, newWaypointLongitude);
+       newWaypointPending = true;
+	   document.getElementById('JScript_newWaypointLatitude').setAttribute('value',newWaypointLatitude);
+	   document.getElementById('JScript_newWaypointLongitude').setAttribute('value',newWaypointLongitude);
+	   document.getElementById('JScript_newWaypointAltitude').setAttribute('value',newWaypointAltitude);
+	   document.getElementById('JScript_newWaypointPending').setAttribute('value',true);
+    }
+}); 
+}
+
+
+
+function setCurrAircraft(id)
+{
+	currAircraft = id;
+}
+
+function setGCSHome(lat, lon, alt)
+{
+	// Only update if position actually changed
+	if (lat != homeLat || lon != homeLon || alt != homeAlt)
+	{
+	homeLat = lat;
+	homeLon = lon;
+	homeAlt = alt;
+	
+	
+	if (homePlacemark == null)
+	{
+		var placemark = ge.createPlacemark(''); 
+	    var icon = ge.createIcon(''); 
+	    icon.setHref('http://google-maps-icons.googlecode.com/files/blackH.png'); 
+	    var style = ge.createStyle(''); 
+	    style.getIconStyle().setIcon(icon); 
+	    //style.getIconStyle().setScale(0.5); 
+	    placemark.setStyleSelector(style);
+	    placemark.setDescription('HOME');
+
+	    // Set the placemark's location.   
+	    homeLocation = ge.createPoint(''); 
+	    homeLocation.setLatitude(lat); 
+	    homeLocation.setLongitude(lon);
+	    homeLocation.setAltitude(alt); 
+	    placemark.setGeometry(homeLocation);  
+
+	    // Add the placemark to Earth. 
+	    ge.getFeatures().appendChild(placemark);
+	    
+	    homePlacemark = placemark;
+	    }
+	    else
+	    {
+	    	var location = ge.createPoint('');
+	    	if (location.getLatitude() != lat || location.getLongitude() != lon || location.getAltitude() != alt)
+	    	{
+	    		location.setLatitude(lat); 
+	    		location.setLongitude(lon);
+	    		location.setAltitude(alt); 
+	    		homePlacemark.setGeometry(location);
+	    		homePlacemark.setDescription('HOME');
+	    	}
+	    }
+
+	    homeGroundLevel = ge.getGlobe().getGroundAltitude(lat,lon);
+	    if (homeGroundLevel == 0)
+	    {
+		    homeGroundLevel = alt;
+	    }
+	}
+}
+
+function updateWaypointListLength(id, len)
+{
+	// Delete any non-needed waypoints
+	if (waypoints.length > len)
+	{
+		var initialLength = waypoints.length;
+		for (var i=len; i<initialLength; i++)
+		{
+			var placemark = waypoints.pop();
+		    ge.getFeatures().removeChild(placemark);
+		    waypointLines[id].getCoordinates().pop();
+		}
+	}
+}
+
+function updateWaypoint(id, index, lat, lon, alt, action)
+{
+	// Check if waypoint exists
+	if (waypoints.length > index)
+	{
+		// Waypoint exists
+		// Set the placemark's location.   
+	    var location = ge.createPoint(''); 
+	    location.setLatitude(lat); 
+	    location.setLongitude(lon);
+	    location.setAltitude(alt); 
+	    waypoints[index].setGeometry(location);
+	    waypoints[index].setDescription(index+"");
+	    
+	    // Set the WP line location
+	    waypointLines[id].getCoordinates().setLatLngAlt(index, lat, lon, alt);
+	}
+	else
+	{
+		// Waypoint does not exist yet
+		var placemark = ge.createPlacemark(''); 
+	    var icon = ge.createIcon('');
+	    var numberstring = index;
+	    if (index < 10) numberstring = '0' + numberstring
+	    icon.setHref('http://google-maps-icons.googlecode.com/files/red' + numberstring +'.png'); 
+	    var style = ge.createStyle('');
+	    //console.log('WP ICON created:' + 'http://google-maps-icons.googlecode.com/files/red' + numberstring +'.png');
+	    style.getIconStyle().setIcon(icon); 
+	    //style.getIconStyle().setScale(0.5); 
+	    placemark.setStyleSelector(style);
+	    placemark.setDescription(index+"");
+
+	    // Set the placemark's location.   
+	    var location = ge.createPoint(''); 
+	    location.setLatitude(lat); 
+	    location.setLongitude(lon);
+	    location.setAltitude(alt+lineAltitudeOffset); 
+	    placemark.setGeometry(location);  
+
+	    // Add the placemark to Earth. 
+	    ge.getFeatures().appendChild(placemark); 
+		waypoints[index] = placemark;
+        
+        // Add LineString points
+	    waypointLines[id].getCoordinates().pushLatLngAlt(lat, lon, alt);
+	}
+}
+
+function createAircraft(id, type, color)
+{
+	planePlacemark = ge.createPlacemark('');
+	planePlacemark.setName('aircraft');
+	planeModel = ge.createModel('');
+	ge.getFeatures().appendChild(planePlacemark);
+	planeLoc = ge.createLocation('');
+	planeModel.setLocation(planeLoc);
+	planeLink = ge.createLink('');
+	planeOrient = ge.createOrientation(''); 
+	planeModel.setOrientation(planeOrient);
+	
+	var factor = 1.0;
+
+	//planeLink.setHref('http://www.asl.ethz.ch/people/rudink/senseSoarDummy.dae'); 
+	planeLink.setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
+	factor = 1.0/1000.0;
+	//planeLink.setHref('http://qgroundcontrol.org/_media/users/models/ascent-park-glider.dae');
+	planeModel.setLink(planeLink);
+	var scale = planeModel.getScale();
+	scale.set(scale.getX()*factor, scale.getY()*factor, scale.getZ()*factor)
+	planeModel.setScale(scale);
+	planeModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE); 
+
+	planeLoc.setLatitude(currLat);
+	planeLoc.setLongitude(currLon);
+	planeLoc.setAltitude(currAlt);
+
+	planePlacemark.setGeometry(planeModel);
+          
+	// Write into global structure
+	aircraft[id] = planePlacemark;
+	attitudes[id] = planeOrient;
+	aircraftLocations[id] = planeLoc;
+	aircraftLastLocations[id] = ge.createLocation('');
+          
+	createTrail(id, color);
+	createWaypointLine(id, color);
+}
+
+function createTrail(id, color)
+{
+	trailPlacemarks[id] = ge.createPlacemark('');
+	// Create the placemark
+	// Create the LineString; set it to extend down to the ground
+	// and set the altitude mode
+	trails[id] = ge.createLineString('');
+	trailPlacemarks[id].setGeometry(trails[id]);
+	trails[id].setExtrude(false);
+	trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
+
+	// Create a style and set width and color of line
+	trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
+	lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
+	lineStyle.setWidth(5);
+	trailColors[id] = color;
+	lineStyle.getColor().set('00000000');  // aabbggrr format
+
+	trailsVisible[id] = false;
+}
+
+function createWaypointLine(id, color)
+{
+	// Create the placemark
+	waypointLinePlacemarks[id] = ge.createPlacemark('');
+	// Create the LineString; set it to extend down to the ground
+	// and set the altitude mode
+	waypointLines[id] = ge.createLineString('');
+	waypointLinePlacemarks[id].setGeometry(waypointLines[id]);
+	waypointLines[id].setExtrude(false);
+	waypointLines[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
+
+	// Add LineString points
+	//lineString.getCoordinates().pushLatLngAlt(48.754, -121.835, 700);
+
+	// Create a style and set width and color of line
+	waypointLinePlacemarks[id].setStyleSelector(ge.createStyle(''));
+	lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle();
+	lineStyle.setWidth(15);
+	waypointLineColors[id] = color;
+	lineStyle.getColor().set(color);  // aabbggrr format
+
+        
+
+        // Create a style and set width and color of line
+        //waypointLinePlacemarks[id].setStyleSelector(ge.createStyle(''));
+        //lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle();
+        //lineStyle.setWidth(15);
+        //lineStyle.getColor().set(waypointLineColors[id]);  // aabbggrr format
+        //lineStyle.getColor().set(color);  // aabbggrr format
+
+	// Add the feature to Earth
+	ge.getFeatures().appendChild(waypointLinePlacemarks[id]);
+}
+
+function clearTrail(id)
+{
+	ge.getFeatures().removeChild(trailPlacemarks[id]);
+	trailPlacemarks[id] = null;
+	var isVisible = trailsVisible[id];
+	createTrail(id, trailColors[id]);
+	if (isVisible)
+	{
+		showTrail(id);
+	}
+}
+
+function hideTrail(id)
+{
+	trailsVisible[id] = false;
+	ge.getFeatures().removeChild(trailPlacemarks[id]);
+}
+
+function showTrail(id)
+{
+	ge.getFeatures().appendChild(trailPlacemarks[id]);
+	trailsVisible[id] = true;
+}
+
+function setViewRange(dist)
+{
+	currViewRange = dist;
+}
+
+function addTrailPosition(id, lat, lon, alt)
+{
+	//trails[id].setExtrude(false);
+    //trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
+
+    // Add LineString points
+	trails[id].getCoordinates().pushLatLngAlt(lat, lon, alt);
+
+    // Create a style and set width and color of line
+    trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
+    lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
+    lineStyle.setWidth(5);
+    lineStyle.getColor().set(trailColors[id]);  // aabbggrr format
+}
+
+function initCallback(object)
+{
+    ge = object;
+    ge.getWindow().setVisibility(true);
+    ge.getOptions().setStatusBarVisibility(true); 
+    ge.getOptions().setScaleLegendVisibility(true);
+    //ge.getOptions().setFlyToSpeed(5.0); 
+    ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); 
+    ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
+        
+    ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); 
+    ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true); 
+    ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); 
+    ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
+    
+    enableEventListener();
+	document.getElementById('JScript_initialized').setAttribute('value','true');
+	
+	// Load heightmap
+	// http://www.inf.ethz.ch/personal/lomeier/data/untex-environment.dae
+	
+	heightMapPlacemark = ge.createPlacemark('');
+	heightMapPlacemark.setName('aircraft');
+	heightMapModel = ge.createModel('');
+	ge.getFeatures().appendChild(heightMapPlacemark);
+	planeLoc = ge.createLocation('');
+	heightMapModel.setLocation(planeLoc);
+	planeLink = ge.createLink('');
+	planeOrient = ge.createOrientation(''); 
+	heightMapModel.setOrientation(planeOrient); 
+
+	planeLink.setHref('http://www.inf.ethz.ch/personal/lomeier/data/untex-environment.dae'); 
+	heightMapModel.setLink(planeLink);
+	var scale = heightMapModel.getScale();
+	var factor = 1.0;//1.0/1000.0;
+	scale.set(scale.getX()*factor, scale.getY()*factor, scale.getZ()*factor)
+	heightMapModel.setScale(scale);
+	heightMapModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE); 
+
+	planeLoc.setLatitude(currLat);
+	planeLoc.setLongitude(currLon);
+	planeLoc.setAltitude(currAlt);
+
+	heightMapPlacemark.setGeometry(heightMapModel);
+	
+    initialized = true;
+}
+
+function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
+{
+	if (id == currAircraft)
+	{
+		if (lastLat == 0)
+		{
+			lastLat = currLat;
+			lastLon = currLon;
+		}
+		
+		currLat = lat;
+		currLon = lon;
+		var trueGroundAlt = ge.getGlobe().getGroundAltitude(lat, lon);
+		if (trueGroundAlt < alt)
+		{
+		  currAlt = alt;
+	    }
+	    else
+	    {
+	      currAlt = trueGroundAlt+0.1;
+	    }
+		// Interpolate between t-1 and t and set new states
+		lastLat = lastLat*0.5+currLat*0.5;
+		lastLon = lastLon*0.5+currLon*0.5;
+		lastAlt = lastAlt*0.5+currAlt*0.5;
+	
+		planeOrient.setRoll(+((roll/M_PI))*180.0);
+		planeOrient.setTilt(-((pitch/M_PI))*180.0);
+		planeOrient.setHeading(((yaw/M_PI))*180.0-90.0);
+		planeModel.setOrientation(planeOrient);
+	
+		currFollowHeading = currFollowHeading*0.95+0.05*(((yaw/M_PI))*180.0);
+    
+    	planeLoc.setLatitude(lastLat);
+    	planeLoc.setLongitude(lastLon);
+    	planeLoc.setAltitude(lastAlt);
+    	planeModel.setLocation(planeLoc);
+    
+    	if (followEnabled) updateFollowAircraft();
+    }
+}
+
+function enableDaylight(enabled)
+{
+	if(enabled)
+	{
+		ge.getSun().setVisibility(true);
+	}
+	else
+	{
+		ge.getSun().setVisibility(false);
+	}
+}
+
+function enableAtmosphere(enabled)
+{
+	ge.getOptions().setAtmosphereVisibility(enabled);
+}
+		
+function goHome()
+{
+	var currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
+	currView.setLatitude(homeLat);
+	currView.setLongitude(homeLon);
+	currView.setAltitude(homeAlt);
+	currView.setRange(homeViewRange);
+	currView.setTilt(currTilt);
+	ge.getView().setAbstractView(currView);
+}
+
+function setCurrentAircraft(id)
+{
+	currAircraft = id;
+}
+
+/** @brief Set the current view mode
+ *
+ * @param mode 0: side map view, 1: top/north map view, 2: fixed chase cam, 3: free chase cam
+ */
+function setViewMode(mode)
+{
+	var currView = ge.getView().copyAsLookAt(ge.ALTITUDE_RELATIVE_TO_GROUND);
+	
+	if (mode == 0)
+	{
+		currView.setTilt(lastTilt);
+		currView.setHeading(lastHeading);
+	}
+	
+	if (mode == 1 && viewMode != mode)
+	{
+		lastTilt = currView.getTilt();
+		lastHeading = currView.getHeading();
+		currView.setTilt(0);
+		currView.setHeading(0);
+	}
+	
+	viewMode = mode;
+	
+	ge.getView().setAbstractView(currView);
+}
+
+function updateFollowAircraft()
+{
+	currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
+	currView.setLatitude(lastLat);
+	currView.setLongitude(lastLon);
+	
+	if (distanceMode == 1)
+	{
+		var groundAltitude = ge.getGlobe().getGroundAltitude(lastLat, lastLon);
+		currView.setAltitude(groundAltitude);
+	}
+	
+	if (distanceMode == 0) currView.setAltitude(lastAlt);
+
+	currView.setRange(currViewRange);
+	
+	if (viewMode != 3) // VIEW_MODE_CHASE_FREE
+	{
+		currView.setTilt(currFollowTilt);
+		currView.setHeading(currFollowHeading);
+	}
+	ge.getView().setAbstractView(currView);
+}
+
+function failureCallback(object)
+{
+}
+</script>
+
+
+    <style type="text/css">
+    html, body {
+      margin: 0;
+      width: 100%;
+      height: 100%;
+    }
+    </style>
+  </head>
+  <body onload='init()' id='body'>
+    <center>
+      <div id='map3d' style='margin: 0; spacing: 0; height: 100%; width: 100%'></div>
+    </center>
+	<input type="hidden" id="JScript_initialized" value="false" />
+    <input type="hidden" id="JScript_dragWaypointIndex" value="0" />
+	<input type="hidden" id="JScript_dragWaypointLatitude" value="0" />
+	<input type="hidden" id="JScript_dragWaypointLongitude" value="0" />
+	<input type="hidden" id="JScript_dragWaypointAltitude" value="0" />
+	<input type="hidden" id="JScript_dragWaypointPending" value="false" />
+	<input type="hidden" id="JScript_newWaypointLatitude" value="0" />
+	<input type="hidden" id="JScript_newWaypointLongitude" value="0" />
+	<input type="hidden" id="JScript_newWaypointAltitude" value="0" />
+	<input type="hidden" id="JScript_newWaypointPending" value="false" />
+	<input type="hidden" id="JScript_currentCameraLatitude" value="0" />
+	<input type="hidden" id="JScript_currentCameraLongitude" value="0" />
+	<input type="hidden" id="JScript_currentCameraGroundAltitude" value="0" />
+  </body>
+</html>
diff --git a/images/earth.html b/images/earth.html
index 5091ce347d391c2ac7a7d1bf0ada9520905db346..8fa28ef5f27481c8d65defc15264bf17b4d64781 100644
--- a/images/earth.html
+++ b/images/earth.html
@@ -11,16 +11,15 @@
     <script type="text/javascript" src="https://www.google.com/jsapi?key=ABQIAAAA5Q6wxQ6lxKS8haLVdUJaqhSjosg_0jiTTs2iXtkDVG0n0If1mBRHzhWw5VqBZX-j4NuzoVpU-UaHVg"></script>
     <script type="text/javascript">
 google.load("earth", "1", { 'language': 'en'});
-
 var ge = null;
 var initialized = false;
 var currAircraft = 0;
 var followEnabled = false;
 var lineAltitudeOffset = 0.5; ///< 0.5 m higher than waypoint, prevents the line from entering the ground
 
-var lastLat = 0;
-var lastLon = 0;
-var lastAlt = 0;
+var lastLat = [];
+var lastLon = [];
+var lastAlt = [];
 var currLat = 47.3769;
 var currLon = 8.549444;
 var currAlt = 470;
@@ -59,6 +58,9 @@ var planeLoc;
 var aircraft = [];
 var aircraftLocations = [];
 var aircraftLastLocations = [];
+var aircraftScaleFactors = [];
+var aircraftLinks = [];
+var aircraftModels = [];
 var attitudes = [];
 var locations = [];
 var trails = [];
@@ -104,7 +106,6 @@ var homePlacemark = null;
 var heightMapPlacemark = null;
 var heightMapModel = null;
 
-
 function getGlobal(variable)
 {
 	return variable;
@@ -303,7 +304,6 @@ function setGCSHome(lat, lon, alt)
 	homeLon = lon;
 	homeAlt = alt;
 	
-	
 	if (homePlacemark == null)
 	{
 		var placemark = ge.createPlacemark(''); 
@@ -413,39 +413,39 @@ function updateWaypoint(id, index, lat, lon, alt, action)
 
 function createAircraft(id, type, color)
 {
-	planePlacemark = ge.createPlacemark('');
-	planePlacemark.setName('aircraft');
-	planeModel = ge.createModel('');
-	ge.getFeatures().appendChild(planePlacemark);
-	planeLoc = ge.createLocation('');
-	planeModel.setLocation(planeLoc);
-	planeLink = ge.createLink('');
-	planeOrient = ge.createOrientation(''); 
-	planeModel.setOrientation(planeOrient);
+	aircraft[id] = ge.createPlacemark('');
+	aircraft[id].setName('aircraft');
+	aircraftModels[id] = ge.createModel('');
+	ge.getFeatures().appendChild(aircraft[id]);
+	aircraftLocations[id] = ge.createLocation('');
+	aircraftModels[id].setLocation(aircraftLocations[id]);
+	aircraftLinks[id] = ge.createLink('');
+	attitudes[id] = ge.createOrientation(''); 
+	aircraftModels[id].setOrientation(attitudes[id]);
 	
-	var factor = 1.0;
+	aircraftScaleFactors[id] = 1.0;
 
 	//planeLink.setHref('http://www.asl.ethz.ch/people/rudink/senseSoarDummy.dae'); 
-	planeLink.setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
-	factor = 1.0/1000.0;
+	aircraftLinks[id].setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
+	aircraftScaleFactors[id] = 1.0/1000.0;
 	//planeLink.setHref('http://qgroundcontrol.org/_media/users/models/ascent-park-glider.dae');
-	planeModel.setLink(planeLink);
-	var scale = planeModel.getScale();
-	scale.set(scale.getX()*factor, scale.getY()*factor, scale.getZ()*factor)
-	planeModel.setScale(scale);
-	planeModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE); 
+	aircraftModels[id].setLink(aircraftLinks[id]);
+	var scale = aircraftModels[id].getScale();
+	scale.set(scale.getX()*aircraftScaleFactors[id], scale.getY()*aircraftScaleFactors[id], scale.getZ()*aircraftScaleFactors[id])
+	aircraftModels[id].setScale(scale);
+	aircraftModels[id].setAltitudeMode (ge.ALTITUDE_ABSOLUTE); 
 
-	planeLoc.setLatitude(currLat);
-	planeLoc.setLongitude(currLon);
-	planeLoc.setAltitude(currAlt);
+	aircraftLocations[id].setLatitude(currLat);
+	aircraftLocations[id].setLongitude(currLon);
+	aircraftLocations[id].setAltitude(currAlt);
 
-	planePlacemark.setGeometry(planeModel);
+	aircraft[id].setGeometry(aircraftModels[id]);
           
 	// Write into global structure
-	aircraft[id] = planePlacemark;
-	attitudes[id] = planeOrient;
-	aircraftLocations[id] = planeLoc;
-	aircraftLastLocations[id] = ge.createLocation('');
+	aircraftLastLocations[id] = aircraftLocations[id];
+	lastLat[id] = 0;
+	lastLon[id] = 0;
+	lastAlt[id] = 0;
           
 	createTrail(id, color);
 	createWaypointLine(id, color);
@@ -565,13 +565,12 @@ function initCallback(object)
     ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); 
     ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
     
-    enableEventListener();
-	document.getElementById('JScript_initialized').setAttribute('value','true');
+    
 	
 	// Load heightmap
 	// http://www.inf.ethz.ch/personal/lomeier/data/untex-environment.dae
 	
-	heightMapPlacemark = ge.createPlacemark('');
+	/*heightMapPlacemark = ge.createPlacemark('');
 	heightMapPlacemark.setName('aircraft');
 	heightMapModel = ge.createModel('');
 	ge.getFeatures().appendChild(heightMapPlacemark);
@@ -593,51 +592,56 @@ function initCallback(object)
 	planeLoc.setLongitude(currLon);
 	planeLoc.setAltitude(currAlt);
 
-	heightMapPlacemark.setGeometry(heightMapModel);
-	
+	heightMapPlacemark.setGeometry(heightMapModel);*/
+	enableEventListener();
+    document.getElementById('JScript_initialized').setAttribute('value','true');
     initialized = true;
 }
 
 function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 {
+	if (lastLat[id] == 0)
+	{
+		lastLat[id] = lat;
+		lastLon[id] = lon;
+	}
+	
 	if (id == currAircraft)
 	{
-		if (lastLat == 0)
-		{
-			lastLat = currLat;
-			lastLon = currLon;
-		}
-		
+		currFollowHeading = currFollowHeading*0.95+0.05*(((yaw/M_PI))*180.0);
 		currLat = lat;
 		currLon = lon;
 		var trueGroundAlt = ge.getGlobe().getGroundAltitude(lat, lon);
 		if (trueGroundAlt < alt)
 		{
-		  currAlt = alt;
+		  	currAlt = alt;
 	    }
 	    else
 	    {
-	      currAlt = trueGroundAlt+0.1;
+	     	currAlt = trueGroundAlt+0.1;
 	    }
-		// Interpolate between t-1 and t and set new states
-		lastLat = lastLat*0.5+currLat*0.5;
-		lastLon = lastLon*0.5+currLon*0.5;
-		lastAlt = lastAlt*0.5+currAlt*0.5;
+	}
+	    
+	// Interpolate between t-1 and t and set new states
+	//lastLat[id] = lastLat[id]*0.5+lat*0.5;
+	//lastLon[id] = lastLon[id]*0.5+lon*0.5;
+	//lastAlt[id] = lastAlt[id]*0.5+alt*0.5;
 	
-		planeOrient.setRoll(+((roll/M_PI))*180.0);
-		planeOrient.setTilt(-((pitch/M_PI))*180.0);
-		planeOrient.setHeading(((yaw/M_PI))*180.0-90.0);
-		planeModel.setOrientation(planeOrient);
+	lastLat[id] = lastLat[id]*0.5+lat*0.5;
+	lastLon[id] = lastLon[id]*0.5+lon*0.5;
+	lastAlt[id] = lastAlt[id]*0.5+alt*0.5;
 	
-		currFollowHeading = currFollowHeading*0.95+0.05*(((yaw/M_PI))*180.0);
+	attitudes[id].setRoll(+((roll/M_PI))*180.0);
+	attitudes[id].setTilt(-((pitch/M_PI))*180.0);
+	attitudes[id].setHeading(((yaw/M_PI))*180.0-90.0);
+	aircraftModels[id].setOrientation(attitudes[id]);
     
-    	planeLoc.setLatitude(lastLat);
-    	planeLoc.setLongitude(lastLon);
-    	planeLoc.setAltitude(lastAlt);
-    	planeModel.setLocation(planeLoc);
+    aircraftLocations[id].setLatitude(lastLat[id]);
+    aircraftLocations[id].setLongitude(lastLon[id]);
+    aircraftLocations[id].setAltitude(lastAlt[id]);
+    aircraftModels[id].setLocation(aircraftLocations[id]);
     
-    	if (followEnabled) updateFollowAircraft();
-    }
+    if (followEnabled && id == currAircraft) updateFollowAircraft();
 }
 
 function enableDaylight(enabled)
@@ -703,16 +707,16 @@ function setViewMode(mode)
 function updateFollowAircraft()
 {
 	currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
-	currView.setLatitude(lastLat);
-	currView.setLongitude(lastLon);
+	currView.setLatitude(lastLat[currAircraft]);
+	currView.setLongitude(lastLon[currAircraft]);
 	
 	if (distanceMode == 1)
 	{
-		var groundAltitude = ge.getGlobe().getGroundAltitude(lastLat, lastLon);
+		var groundAltitude = ge.getGlobe().getGroundAltitude(lastLat[currAircraft], lastLon[currAircraft]);
 		currView.setAltitude(groundAltitude);
 	}
 	
-	if (distanceMode == 0) currView.setAltitude(lastAlt);
+	if (distanceMode == 0) currView.setAltitude(lastAlt[currAircraft]);
 
 	currView.setRange(currViewRange);
 	
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 1e181f1df20fb83f5e713ca4f5a735a0434540e9..8b79dbc089c3682ce6fd4f1485a06a6c3609bba4 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -545,19 +545,19 @@ TRANSLATIONS += es-MX.ts \
 
 # xbee support
 # libxbee only supported by linux and windows systems
-#win32-msvc2008|win32-msvc2010|linux{
-#    HEADERS += src/comm/XbeeLinkInterface.h \
-#	src/comm/XbeeLink.h \
-#	src/comm/HexSpinBox.h \
-#	src/ui/XbeeConfigurationWindow.h \
-#	src/comm/CallConv.h
-#    SOURCES += src/comm/XbeeLink.cpp \
-#	src/comm/HexSpinBox.cpp \
-#	src/ui/XbeeConfigurationWindow.cpp
-#    DEFINES += XBEELINK
-#    INCLUDEPATH += thirdParty/libxbee
+win32-msvc2008|win32-msvc2010|linux{
+    HEADERS += src/comm/XbeeLinkInterface.h \
+	src/comm/XbeeLink.h \
+	src/comm/HexSpinBox.h \
+	src/ui/XbeeConfigurationWindow.h \
+	src/comm/CallConv.h
+    SOURCES += src/comm/XbeeLink.cpp \
+	src/comm/HexSpinBox.cpp \
+	src/ui/XbeeConfigurationWindow.cpp
+    DEFINES += XBEELINK
+    INCLUDEPATH += thirdParty/libxbee
 # TO DO: build library when it does not exists already
-#    LIBS += -LthirdParty/libxbee/lib \
-#	-llibxbee
-#
-#}
+    LIBS += -LthirdParty/libxbee/lib \
+	-llibxbee
+
+}
diff --git a/src/QGCCore.cc b/src/QGCCore.cc
index 1d22ae53ff47be672fd5391a90fab03e891e1f0e..1b08a062e1e34bffec3bf05cfd15ad4819363467 100644
--- a/src/QGCCore.cc
+++ b/src/QGCCore.cc
@@ -204,8 +204,9 @@ QGCCore::~QGCCore()
 {
     //mainWindow->storeSettings();
     mainWindow->close();
-    mainWindow->deleteLater();
+    //mainWindow->deleteLater();
     // Delete singletons
+	delete MainWindow::instance();
     delete LinkManager::instance();
     delete UASManager::instance();
 }
diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc
index 5bbb3ec45fe4fa566a4700635d37c77f596b2f0d..b843daea5f7655a5ba96deb069e24f658303b955 100644
--- a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc
+++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc
@@ -96,7 +96,7 @@ void MAVLinkXMLParserV10::processError(QProcess::ProcessError err)
 bool MAVLinkXMLParserV10::generate()
 {
 #ifdef Q_OS_WIN
-    QString generatorCall("%1/files/mavlink_generator/generator/mavgen.exe");
+    QString generatorCall("files/mavlink_generator/generator/mavgen.exe");
 #endif
 #if (defined Q_OS_MAC) || (defined Q_OS_LINUX)
     QString generatorCall("python");
diff --git a/src/comm/HexSpinBox.cpp b/src/comm/HexSpinBox.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..878e9409e6ca56e11b27fcc3bb6e4bdc2350ff9e
--- /dev/null
+++ b/src/comm/HexSpinBox.cpp
@@ -0,0 +1,35 @@
+#include "HexSpinBox.h"
+
+#include <qregexp.h>
+
+HexSpinBox::HexSpinBox(QWidget *parent)
+    : QSpinBox(parent), validator(NULL)
+{
+    setRange(0, 0x7fffffff);
+    validator = new QRegExpValidator(QRegExp("[0-9A-Fa-f]{1,8}"), this);
+}
+
+HexSpinBox::~HexSpinBox(void)
+{
+	if(this->validator)
+	{
+		delete this->validator;
+		this->validator = NULL;
+	}
+}
+
+QValidator::State HexSpinBox::validate(QString &text, int &pos) const
+{
+    return validator->validate(text, pos);
+}
+
+QString HexSpinBox::textFromValue(int value) const
+{
+    return QString::number(value, 16).toUpper();
+}
+
+int HexSpinBox::valueFromText(const QString &text) const
+{
+    bool ok;
+    return text.toInt(&ok, 16);
+}
\ No newline at end of file
diff --git a/src/comm/HexSpinBox.h b/src/comm/HexSpinBox.h
new file mode 100644
index 0000000000000000000000000000000000000000..84703c95c79e94c931a850a3e64c2bd1cc7a0060
--- /dev/null
+++ b/src/comm/HexSpinBox.h
@@ -0,0 +1,25 @@
+#ifndef HEXSPINBOX_H_
+#define HEXSPINBOX_H_
+
+#include <qspinbox.h>
+
+class QRegExpValidator;
+
+class HexSpinBox : public QSpinBox
+{
+    Q_OBJECT
+public:
+    HexSpinBox(QWidget *parent = 0);
+	~HexSpinBox(void);
+
+protected:
+    QValidator::State validate(QString &text, int &pos) const;
+    int valueFromText(const QString &text) const;
+    QString textFromValue(int value) const;
+
+private:
+    QRegExpValidator *validator;
+};
+
+#endif // HEXSPINBOX_H_
+
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 484404ae8e370b5ba355c082c56e160b55a41c8b..27056fbf060447bc711fe80aa0aaabecc910942c 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -358,19 +358,19 @@ void MAVLinkSimulationLink::mainloop()
     if (rate10hzCounter == 1000 / rate / 9) {
         rate10hzCounter = 1;
 
-        float lastX = x;
-        float lastY = y;
-        float lastZ = z;
-        float hackDt = 0.1f; // 100 ms
+        double lastX = x;
+        double lastY = y;
+        double lastZ = z;
+        double hackDt = 0.1f; // 100 ms
 
         // Move X Position
         x = 12.0*sin(((double)circleCounter)/200.0);
         y = 5.0*cos(((double)circleCounter)/200.0);
         z = 1.8 + 1.2*sin(((double)circleCounter)/200.0);
 
-        float xSpeed = (x - lastX)/hackDt;
-        float ySpeed = (y - lastY)/hackDt;
-        float zSpeed = (z - lastZ)/hackDt;
+        double xSpeed = (x - lastX)/hackDt;
+        double ySpeed = (y - lastY)/hackDt;
+        double zSpeed = (z - lastZ)/hackDt;
 
 
 
@@ -414,12 +414,12 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
-//        // GLOBAL POSITION VEHICLE 2
-//        mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed);
-//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
-//        //add data into datastream
-//        memcpy(stream+streampointer,buffer, bufferlength);
-//        streampointer += bufferlength;
+        // GLOBAL POSITION VEHICLE 2
+        mavlink_msg_global_position_int_pack(systemId+1, componentId+1, &ret, 0, (473780.28137103+(x+0.00001))*1E3, (85489.9892510421+((y/2)+0.00001))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw);
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
+        //add data into datastream
+        memcpy(stream+streampointer,buffer, bufferlength);
+        streampointer += bufferlength;
 
 //        // ATTITUDE VEHICLE 2
 //        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
@@ -427,7 +427,7 @@ void MAVLinkSimulationLink::mainloop()
 
 
 //        // GLOBAL POSITION VEHICLE 3
-//        mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
+//        mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
 //        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
 //        //add data into datastream
 //        memcpy(stream+streampointer,buffer, bufferlength);
@@ -565,6 +565,15 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
+        // Pack message and get size of encoded byte string
+        messageSize = mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status);
+        // Allocate buffer with packet data
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
+        //qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
+        //add data into datastream
+        memcpy(stream+streampointer,buffer, bufferlength);
+        streampointer += bufferlength;
+
 
         // Send controller states
 
diff --git a/src/libs/opmapcontrol/src/core/pureimagecache.cpp b/src/libs/opmapcontrol/src/core/pureimagecache.cpp
index de648fb276cf0f97acca787819ec63452daaa717..efda4187547e4e685477fcb7e00203dd2e6409f5 100644
--- a/src/libs/opmapcontrol/src/core/pureimagecache.cpp
+++ b/src/libs/opmapcontrol/src/core/pureimagecache.cpp
@@ -227,23 +227,25 @@ namespace core {
 #endif //DEBUG_PUREIMAGECACHE
 
             QString db=dir+"Data.qmdb";
-            QSqlDatabase cn;
-
-            cn = QSqlDatabase::addDatabase("QSQLITE",QString::number(id));
+			{
+				QSqlDatabase cn;
+			
+				cn = QSqlDatabase::addDatabase("QSQLITE",QString::number(id));
 
-            cn.setDatabaseName(db);
-            cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE");
-            if(cn.open())
-            {
-				QSqlQuery query(cn);
-				query.exec(QString("SELECT Tile FROM TilesData WHERE id = (SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4)").arg(pos.X()).arg(pos.Y()).arg(zoom).arg((int) type));
-				query.next();
-				if(query.isValid())
+	            cn.setDatabaseName(db);
+		        cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE");
+			    if(cn.open())
 				{
-					ar=query.value(0).toByteArray();
-                }
-                cn.close();
-             }
+					QSqlQuery query(cn);
+					query.exec(QString("SELECT Tile FROM TilesData WHERE id = (SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4)").arg(pos.X()).arg(pos.Y()).arg(zoom).arg((int) type));
+					query.next();
+					if(query.isValid())
+					{
+						ar=query.value(0).toByteArray();
+					}
+					cn.close();
+				}
+			}
 			QSqlDatabase::removeDatabase(QString::number(id));
         lock.unlock();
         return ar;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 6bd4065300a0bab00bc120f084514c0d88904b65..89c403e0b58656bd4f5567124e26185f0b143f0b 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -82,6 +82,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     isGlobalPositionKnown(false),
     systemIsArmed(false)
 {
+    for (unsigned int i = 0; i<255;++i)
+    {
+        componentID[i] = -1;
+        componentMulti[i] = false;
+    }
+
     color = UASInterface::getNextColor();
     setBatterySpecs(QString("9V,9.5V,12.6V"));
     connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
@@ -210,6 +216,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         QString uasState;
         QString stateDescription;
 
+        bool multiComponentSourceDetected = false;
+        bool wrongComponent = false;
+
+        // Store component ID
+        if (componentID[message.msgid] == -1)
+        {
+            componentID[message.msgid] = message.compid;
+        }
+        else
+        {
+            // Got this message already
+            if (componentID[message.msgid] != message.compid)
+            {
+                componentMulti[message.msgid] = true;
+                wrongComponent = true;
+            }
+        }
+
+        if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true;
+
+
         switch (message.msgid)
         {
         case MAVLINK_MSG_ID_HEARTBEAT:
@@ -336,6 +363,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             break;
         case MAVLINK_MSG_ID_SYS_STATUS:
         {
+                if (multiComponentSourceDetected && message.compid != MAV_COMP_ID_IMU_2)
+                {
+                    break;
+                }
                 mavlink_sys_status_t state;
                 mavlink_msg_sys_status_decode(&message, &state);
 
@@ -371,6 +402,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             break;
         case MAVLINK_MSG_ID_ATTITUDE:
             {
+                if (wrongComponent) break;
+
                 mavlink_attitude_t attitude;
                 mavlink_msg_attitude_decode(&message, &attitude);
                 quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
@@ -499,7 +532,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     if (!isnan(alt) && !isinf(alt))
                     {
                         alt = 0;
-                        emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
+                        //emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
                     }
                     // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
                     // Smaller than threshold and not NaN
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 0493dc4bd669d3cf1c9307c3c704644ec610f7cb..63e5c5a924876792fa0a1d0b99f741c44b8e8de2 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -544,6 +544,8 @@ protected:
     quint64 getUnixTimeFromMs(quint64 time);
     /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
     quint64 getUnixReferenceTime(quint64 time);
+    int componentID[256];
+    bool componentMulti[256];
 
 protected slots:
     /** @brief Write settings to disk */
diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc
index bedc2f83f984bd5a91c97152455c736b148d4798..53a5d637de062dc7e452c2bca3e373048401fc32 100644
--- a/src/ui/HDDisplay.cc
+++ b/src/ui/HDDisplay.cc
@@ -50,8 +50,8 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
     acceptUnitList(new QStringList()),
     lastPaintTime(0),
     columns(3),
-    valuesChanged(true)/*,
-    m_ui(new Ui::HDDisplay)*/
+    valuesChanged(true),
+    m_ui(NULL)
 {
     setWindowTitle(title);
     //m_ui->setupUi(this);
@@ -135,7 +135,22 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
 HDDisplay::~HDDisplay()
 {
     saveState();
-    delete m_ui;
+	if(this->refreshTimer)
+	{
+		delete this->refreshTimer;
+	}
+	if(this->acceptList)
+	{
+		delete this->acceptList;
+	}
+	if(this->acceptUnitList)
+	{
+		delete this->acceptUnitList;
+	}
+	if(this->m_ui)
+	{
+		delete m_ui;
+	}
 }
 
 QSize HDDisplay::sizeHint() const
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index ec0a309c23e26d4c76c7a627c8f52f35634d0cd6..296e25d3e08e16c7335a5fc65bf7f1bfd0d1f3f2 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -136,6 +136,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
     setFocusPolicy(Qt::StrongFocus);
 }
 
+HSIDisplay::~HSIDisplay()
+{
+
+}
+
 void HSIDisplay::resetMAVState()
 {
     mavInitialized = false;
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index df3608dfb95d68970a5c6c0077acd0c7aa6be87e..cdce6e7d911a7fae2324a4797f9e0fef4d18f9d4 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -48,7 +48,7 @@ class HSIDisplay : public HDDisplay
     Q_OBJECT
 public:
     HSIDisplay(QWidget *parent = 0);
-    // ~HSIDisplay();
+    ~HSIDisplay();
 
 public slots:
     void setActiveUAS(UASInterface* uas);
diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc
index 703e09f6382c1210e208bb29a78b65262e02d11c..082b71f93d5d9cd50545437a6dd54181453fd68e 100644
--- a/src/ui/MAVLinkDecoder.cc
+++ b/src/ui/MAVLinkDecoder.cc
@@ -7,6 +7,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
     mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO;
     memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256);
     memset(receivedMessages, 0, sizeof(mavlink_message_t)*256);
+    for (unsigned int i = 0; i<255;++i)
+    {
+        componentID[i] = -1;
+        componentMulti[i] = false;
+    }
 
     // Fill filter
     messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false);
@@ -69,6 +74,26 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
 
 void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time)
 {
+    bool multiComponentSourceDetected = false;
+    bool wrongComponent = false;
+
+    // Store component ID
+    if (componentID[msg->msgid] == -1)
+    {
+        componentID[msg->msgid] = msg->compid;
+    }
+    else
+    {
+        // Got this message already
+        if (componentID[msg->msgid] != msg->compid)
+        {
+            componentMulti[msg->msgid] = true;
+            wrongComponent = true;
+        }
+    }
+
+    if (componentMulti[msg->msgid] == true) multiComponentSourceDetected = true;
+
     // Add field tree widget item
     uint8_t msgid = msg->msgid;
     if (messageFilter.contains(msgid)) return;
@@ -78,6 +103,8 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
     QString name("%1.%2");
     QString unit("");
     name = name.arg(messageInfo[msgid].name, fieldName);
+    if (multiComponentSourceDetected) name.prepend(QString("C%1:").arg(msg->compid));
+    name.prepend(QString("M%1:").arg(msg->sysid));
     switch (messageInfo[msgid].fields[fieldid].type)
     {
     case MAVLINK_TYPE_CHAR:
diff --git a/src/ui/MAVLinkDecoder.h b/src/ui/MAVLinkDecoder.h
index 54f989430c937cb6a4f1d4c60bb7b2b2b48ee3f6..904b7d55424fff676af6efbaac415dbaa465b2eb 100644
--- a/src/ui/MAVLinkDecoder.h
+++ b/src/ui/MAVLinkDecoder.h
@@ -30,6 +30,8 @@ protected:
     mavlink_message_info_t messageInfo[256]; ///< Message information
     QMap<uint16_t, bool> messageFilter;               ///< Message/field names not to emit
     QMap<uint16_t, bool> textMessageFilter;           ///< Message/field names not to emit in text mode
+    int componentID[256];                             ///< Multi component detection
+    bool componentMulti[256];                         ///< Multi components detected
 
 };
 
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 51858292dbe5986adc7734e6a6b2777d6fec3d0c..bfda0a0d325b37a8e01b5b98645df6b1f5be4bcf 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -91,7 +91,7 @@ MainWindow::MainWindow(QWidget *parent):
     currentStyle(QGC_MAINWINDOW_STYLE_INDOOR),
     aboutToCloseFlag(false),
     changingViewsFlag(false),
-    centerStackActionGroup(this),
+    centerStackActionGroup(new QActionGroup(this)),
     styleFileName(QCoreApplication::applicationDirPath() + "/style-indoor.css"),
     autoReconnect(false),
     lowPowerMode(false)
@@ -138,7 +138,7 @@ MainWindow::MainWindow(QWidget *parent):
     setCorner(Qt::BottomRightCorner, Qt::RightDockWidgetArea);
 
     // Setup UI state machines
-    centerStackActionGroup.setExclusive(true);
+	centerStackActionGroup->setExclusive(true);
 
     centerStack = new QStackedWidget(this);
     setCentralWidget(centerStack);
@@ -246,8 +246,8 @@ MainWindow::~MainWindow()
         if (dockWidget)
         {
             // Remove dock widget from main window
-            removeDockWidget(dockWidget);
-            delete dockWidget->widget();
+            // removeDockWidget(dockWidget);
+            // delete dockWidget->widget();
             delete dockWidget;
         }
         else
@@ -417,14 +417,14 @@ void MainWindow::buildCommonWidgets()
         parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
         addTool(parametersDockWidget, tr("Calibration and Parameters"), Qt::RightDockWidgetArea);
     }
-
+	
     if (!hsiDockWidget) {
         hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this);
         hsiDockWidget->setWidget( new HSIDisplay(this) );
         hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET");
         addTool(hsiDockWidget, tr("Horizontal Situation"), Qt::BottomDockWidgetArea);
     }
-
+	
     if (!headDown1DockWidget) {
         headDown1DockWidget = new QDockWidget(tr("Flight Display"), this);
         HDDisplay* hdDisplay = new HDDisplay(acceptList, "Flight Display", this);
@@ -442,7 +442,7 @@ void MainWindow::buildCommonWidgets()
         headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET");
         addTool(headDown2DockWidget, tr("Actuator Status"), Qt::RightDockWidgetArea);
     }
-
+	
     if (!rcViewDockWidget) {
         rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
         rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
@@ -555,7 +555,7 @@ void MainWindow::addCentralWidget(QWidget* widget, const QString& title)
         QVariant var;
         var.setValue((QWidget*)widget);
         tempAction->setData(var);
-        centerStackActionGroup.addAction(tempAction);
+        centerStackActionGroup->addAction(tempAction);
         connect(tempAction,SIGNAL(triggered()),this, SLOT(showCentralWidget()));
         connect(widget, SIGNAL(visibilityChanged(bool)), tempAction, SLOT(setChecked(bool)));
         tempAction->setChecked(widget->isVisible());
@@ -1503,108 +1503,3 @@ QList<QAction*> MainWindow::listLinkMenuActions(void)
 {
     return ui.menuNetwork->actions();
 }
-/*
-void MainWindow::buildSenseSoarWidgets()
-{
-	if (!linechartWidget) 
-	{
-        // Center widgets
-        linechartWidget   = new Linecharts(this);
-        addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART);
-    }
-
-    if (!hudWidget) 
-	{
-        hudWidget         = new HUD(320, 240, this);
-        addToCentralWidgetsMenu(hudWidget, tr("Head Up Display"), SLOT(showCentralWidget()), CENTRAL_HUD);
-    }
-
-    if (!dataplotWidget) {
-        dataplotWidget    = new QGCDataPlot2D(this);
-        addToCentralWidgetsMenu(dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT);
-    }
-
-#ifdef QGC_OSG_ENABLED
-    if (!_3DWidget) {
-        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
-        addToCentralWidgetsMenu(_3DWidget, tr("Local 3D"), SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
-    }
-#endif
-
-#ifdef QGC_OSGEARTH_ENABLED
-    if (!_3DMapWidget) {
-        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
-        addToCentralWidgetsMenu(_3DMapWidget, tr("OSG Earth 3D"), SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
-    }
-#endif
-
-#if (defined _MSC_VER) | (defined Q_OS_MAC)
-    if (!gEarthWidget) {
-        gEarthWidget = new QGCGoogleEarthView(this);
-        addToCentralWidgetsMenu(gEarthWidget, tr("Google Earth"), SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH);
-    }
-
-#endif
-
-    // Dock widgets
-
-    if (!parametersDockWidget) {
-        parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this);
-        parametersDockWidget->setWidget( new ParameterInterface(this) );
-        parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
-        addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea);
-    }
-
-    if (!hsiDockWidget) {
-        hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this);
-        hsiDockWidget->setWidget( new HSIDisplay(this) );
-        hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET");
-        addToToolsMenu (hsiDockWidget, tr("Horizontal Situation"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea);
-    }
-	
-    if (!rcViewDockWidget) {
-        rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
-        rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
-        rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET");
-        addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
-    }
-
-    if (!headUpDockWidget) {
-        headUpDockWidget = new QDockWidget(tr("HUD"), this);
-        headUpDockWidget->setWidget( new HUD(320, 240, this));
-        headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET");
-        addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea);
-    }
-}
-
-void MainWindow::connectSenseSoarWidgets()
-{
-}
-
-void MainWindow::arrangeSenseSoarCenterStack()
-{
-	if (!centerStack) {
-        qDebug() << "Center Stack not Created!";
-        return;
-    }
-
-
-    if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget);
-
-#ifdef QGC_OSG_ENABLED
-    if (_3DWidget && (centerStack->indexOf(_3DWidget) == -1)) centerStack->addWidget(_3DWidget);
-#endif
-#ifdef QGC_OSGEARTH_ENABLED
-    if (_3DMapWidget && (centerStack->indexOf(_3DMapWidget) == -1)) centerStack->addWidget(_3DMapWidget);
-#endif
-#if (defined _MSC_VER) | (defined Q_OS_MAC)
-    if (gEarthWidget && (centerStack->indexOf(gEarthWidget) == -1)) centerStack->addWidget(gEarthWidget);
-#endif
-    if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget);
-    if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget);
-}
-
-void MainWindow::connectSenseSoarActions()
-{
-}
-*/
diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h
index 9935a8f87f4b9fa5bfc22cea27c5afd8ce7137e8..1f6195cdffb3bff868dcfedb83fc6b21931cc2dd 100644
--- a/src/ui/MainWindow.h
+++ b/src/ui/MainWindow.h
@@ -303,7 +303,7 @@ protected:
 
     QSettings settings;
     QStackedWidget *centerStack;
-    QActionGroup centerStackActionGroup;
+    QActionGroup* centerStackActionGroup;
 
     // Center widgets
     QPointer<Linecharts> linechartWidget;
diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc
index a65ebb4d01c84efa6a92e53670b2ddb83243ac48..809274f7ad43e5a5637015624e43b9fc94913772 100644
--- a/src/ui/QGCMAVLinkInspector.cc
+++ b/src/ui/QGCMAVLinkInspector.cc
@@ -82,7 +82,7 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
         {
             msgHz = 1;
         }
-        float newHz = 0.001f*msgHz+0.999f*messagesHz.value(message.msgid, 1);
+        float newHz = 0.05f*msgHz+0.95f*messagesHz.value(message.msgid, 1);
         messagesHz.insert(message.msgid, newHz);
     }
 
diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc
index f694ec5f0b9b5fa246754ca48879fdd0cac57705..7efc3150e7017a19d6affda4752a88fc0e37c2ed 100644
--- a/src/ui/QGCRemoteControlView.cc
+++ b/src/ui/QGCRemoteControlView.cc
@@ -43,8 +43,8 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) :
     uasId(-1),
     rssi(0.0f),
     updated(false),
-    channelLayout(new QVBoxLayout())//,
-    //ui(new Ui::QGCRemoteControlView)
+    channelLayout(new QVBoxLayout()),
+    ui(NULL)
 {
     ui->setupUi(this);
     QGridLayout* layout = new QGridLayout(this);
@@ -71,8 +71,14 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) :
 
 QGCRemoteControlView::~QGCRemoteControlView()
 {
-    delete ui;
-    delete channelLayout;
+	if(this->ui)
+	{
+		delete ui;
+	}
+	if(this->channelLayout)
+	{
+		delete channelLayout;
+	}
 }
 
 void QGCRemoteControlView::setUASId(int id)
@@ -172,7 +178,7 @@ void QGCRemoteControlView::setRemoteRSSI(float rssiNormalized)
 void QGCRemoteControlView::appendChannelWidget(int channelId)
 {
     // Create new layout
-    QHBoxLayout* layout = new QHBoxLayout();
+    QHBoxLayout* layout = new QHBoxLayout(this);
     // Add content
     layout->addWidget(new QLabel(QString("Channel %1").arg(channelId + 1), this));
     QLabel* raw = new QLabel(this);