diff --git a/images/earth.html b/images/earth.html
index ba88b2323724c9a76d763b43452ce2461b542dcf..721003b0f4b9476eeee8172285e57f976fe7b14d 100644
--- a/images/earth.html
+++ b/images/earth.html
@@ -61,6 +61,48 @@ 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;
+
+function getGlobal(variable)
+{
+	return variable;
+}
+
+function getDraggingAllowed()
+{
+	return draggingAllowed;
+}
+
+function setDraggingAllowed(allowed)
+{
+	draggingAllowed = allowed;
+}
+
+function setNewWaypointPending(pending)
+{
+	newWaypointPending = pending;
+}
+
+function setDragWaypointPending(pending)
+{
+	dragWaypointPending = pending;
+}
 
 function isInitialized()
 {
@@ -78,6 +120,84 @@ 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 (draggingAllowed)
+  {
+  if (event.getTarget().getType() == 'KmlPlacemark' &&
+      event.getTarget().getGeometry().getType() == 'KmlPoint') {
+    //event.preventDefault();
+    var placemark = event.getTarget();
+    
+    dragInfo = {
+      placemark: event.getTarget(),
+      dragged: false
+    };
+  }
+  }
+});
+
+// listen for mousemove on the globe
+google.earth.addEventListener(ge.getGlobe(), 'mousemove', function(event)
+{
+  if (draggingAllowed)
+  {
+  if (dragInfo) {
+    event.preventDefault();
+    var point = dragInfo.placemark.getGeometry();
+    point.setLatitude(event.getLatitude());
+    point.setLongitude(event.getLongitude());
+    dragInfo.dragged = true;
+    dragWaypointIndex = dragInfo.placemark.getDescription();
+	dragWaypointLatitude = event.getLatitude();
+	dragWaypointLongitude = event.getLongitude();
+	dragWaypointAltitude = point.getAltitude();
+	dragWaypointPending = true;
+  }
+  }
+});
+
+// listen for mouseup on the window
+google.earth.addEventListener(ge.getWindow(), 'mouseup', function(event)
+{
+  if (draggingAllowed)
+  {
+  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();
+      	var point = dragInfo.placemark.getGeometry();
+      	dragWaypointLatitude = event.getLatitude();
+		dragWaypointLongitude = event.getLongitude();
+		dragWaypointAltitude = point.getAltitude();
+    	dragWaypointPending = true;
+    }
+    
+    dragInfo = null;
+  }
+  }
+});
+
+// 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;
+    }
+}); 
+}
+
 
 
 function setCurrAircraft(id)
@@ -143,6 +263,7 @@ function updateWaypoint(id, index, lat, lon, alt, action)
 	    location.setLongitude(lon);
 	    location.setAltitude(alt); 
 	    waypoints[index].setGeometry(location);
+	    waypoints[index].setDescription(index+"");
 	    console.log('WP LOC:' + lat + lon + alt);
 	}
 	else
@@ -157,7 +278,8 @@ function updateWaypoint(id, index, lat, lon, alt, action)
 	    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.setStyleSelector(style);
+	    placemark.setDescription(index+"");
 
 	    // Set the placemark's location.   
 	    var location = ge.createPoint(''); 
@@ -285,6 +407,8 @@ function initCallback(object)
     ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true); 
     ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); 
     ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
+    
+    enableEventListener();
 
     initialized = true;
 }
@@ -300,7 +424,15 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 		}
 		currLat = lat;
 		currLon = lon;
-		currAlt = alt;
+		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.8+currLat*0.2;
 		lastLon = lastLon*0.8+currLon*0.2;
@@ -352,6 +484,15 @@ function setCurrentAircraft(id)
 	currAircraft = id;
 }
 
+/** @brief Set the current view mode
+ *
+ * @param mode 0: map view, 1: chase cam, fixed, 2: chase cam, free
+ */
+function setViewMode(mode)
+{
+
+}
+
 function updateFollowAircraft()
 {
 	currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
diff --git a/src/Waypoint.cc b/src/Waypoint.cc
index 16b2ac63ad7f3c9ecc067165bca7e6e7148315e9..3f3f484fef8bd1437212d299d128f51b4cf0dccf 100644
--- a/src/Waypoint.cc
+++ b/src/Waypoint.cc
@@ -129,6 +129,7 @@ void Waypoint::setLatitude(double lat)
     if (this->x != lat)
     {
         this->x = lat;
+        this->frame = MAV_FRAME_GLOBAL;
         emit changed(this);
     }
 }
@@ -138,6 +139,7 @@ void Waypoint::setLongitude(double lon)
     if (this->y != lon)
     {
         this->y = lon;
+        this->frame = MAV_FRAME_GLOBAL;
         emit changed(this);
     }
 }
@@ -147,6 +149,7 @@ void Waypoint::setAltitude(double altitude)
     if (this->z != altitude)
     {
         this->z = altitude;
+        this->frame = MAV_FRAME_GLOBAL;
         emit changed(this);
     }
 }
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index c9fd0840e5835d479e4e7a2412aa32d01595e520..3ee6738d5d33075cc8a3a26d39690401eb4c6d07 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -1213,10 +1213,17 @@ void UAS::setMode(int mode)
 void UAS::sendMessage(mavlink_message_t message)
 {
     // Emit message on all links that are currently connected
-    QList<LinkInterface*>::iterator i;
-    for (i = links->begin(); i != links->end(); ++i)
+    foreach (LinkInterface* link, *links)
     {
-        sendMessage(*i, message);
+        if (link)
+        {
+            sendMessage(link, message);
+        }
+        else
+        {
+            // Remove from list
+            links->removeAt(links->indexOf(link));
+        }
     }
 }
 
@@ -2039,8 +2046,6 @@ void UAS::addLink(LinkInterface* link)
         links->append(link);
         connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
     }
-    //links->append(link);
-    //qDebug() << link<<" ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK";
 }
 
 void UAS::removeLink(QObject* object)
diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc
index 85a993af6e56cadaff80929e0595aec77e75946f..ae26b3cc5e35e6bf6eb0e1c2a68def6a12202328 100644
--- a/src/ui/map3D/QGCGoogleEarthView.cc
+++ b/src/ui/map3D/QGCGoogleEarthView.cc
@@ -11,6 +11,7 @@
 #ifdef Q_OS_MAC
 #include <QWebFrame>
 #include <QWebPage>
+#include <QWebElement>
 #include "QGCWebPage.h"
 #endif
 
@@ -337,6 +338,30 @@ QVariant QGCGoogleEarthView::javaScript(QString javaScript)
 #endif
 }
 
+QVariant QGCGoogleEarthView::documentElement(QString name)
+{
+#ifdef Q_OS_MAC
+    QString javaScript("getGlobal(%1)");
+    QVariant result = webViewMac->page()->currentFrame()->evaluateJavaScript(javaScript.arg(name));
+    //qDebug() << "DOC ELEM:" << name << ":" << result;
+    return result;
+#endif
+#ifdef _MSC_VER
+    if(!jScriptInitialized)
+    {
+        qDebug() << "TOO EARLY JAVASCRIPT CALL, ABORTING";
+        return QVariant(false);
+    }
+    else
+    {
+        //QVariantList params;
+        //params.append(javaScript);
+        //params.append("JScript");
+        //return jScriptWin->dynamicCall("execScript(QString, QString)", params);
+    }
+#endif
+}
+
 void QGCGoogleEarthView::initializeGoogleEarth()
 {
     if (!jScriptInitialized)
@@ -422,6 +447,9 @@ void QGCGoogleEarthView::initializeGoogleEarth()
             connect(ui->camDistanceSlider, SIGNAL(valueChanged(int)), this, SLOT(setViewRangeScaledInt(int)));
             setViewRangeScaledInt(ui->camDistanceSlider->value());
 
+            // Update waypoint list
+            if (mav) updateWaypointList(mav->getUASID());
+
             // Start update timer
             updateTimer->start(refreshRateMs);
 
@@ -471,6 +499,91 @@ void QGCGoogleEarthView::updateState()
                        .arg(pitch, 0, 'f', 9)
                        .arg(yaw, 0, 'f', 9));
         }
+
+
+        // Read out new waypoint positions and waypoint create events
+        // this is polling (bad) but forced because of the crappy
+        // Microsoft API available in Qt - improvements wanted
+
+        // First check if a new WP should be created
+//        bool newWaypointPending = .to
+        bool newWaypointPending = documentElement("newWaypointPending").toBool();
+        if (newWaypointPending)
+        {
+            bool coordsOk = true;
+            bool ok;
+            double latitude = documentElement("newWaypointLatitude").toDouble(&ok);
+            coordsOk &= ok;
+            double longitude = documentElement("newWaypointLongitude").toDouble(&ok);
+            coordsOk &= ok;
+            double altitude = documentElement("newWaypointAltitude").toDouble(&ok);
+            coordsOk &= ok;
+            if (coordsOk)
+            {
+                // Add new waypoint
+                if (mav)
+                {
+                    int nextIndex = mav->getWaypointManager()->getWaypointList().count();
+                    Waypoint* wp = new Waypoint(nextIndex, latitude, longitude, altitude, true);
+                    wp->setFrame(MAV_FRAME_GLOBAL);
+//                    wp.setLatitude(latitude);
+//                    wp.setLongitude(longitude);
+//                    wp.setAltitude(altitude);
+                    mav->getWaypointManager()->addWaypoint(wp);
+                }
+            }
+            javaScript("setNewWaypointPending(false);");
+        }
+
+        // Check if a waypoint should be moved
+        bool dragWaypointPending = documentElement("dragWaypointPending").toBool();
+
+        if (dragWaypointPending)
+        {
+            bool coordsOk = true;
+            bool ok;
+            double latitude = documentElement("dragWaypointLatitude").toDouble(&ok);
+            coordsOk &= ok;
+            double longitude = documentElement("dragWaypointLongitude").toDouble(&ok);
+            coordsOk &= ok;
+            double altitude = documentElement("dragWaypointAltitude").toDouble(&ok);
+            coordsOk &= ok;
+            if (coordsOk)
+            {
+                // Add new waypoint
+                if (mav)
+                {
+                    QVector<Waypoint*> wps = mav->getWaypointManager()->getGlobalFrameWaypointList();
+
+                    QString idText = documentElement("dragWaypointIndex").toString();
+
+                    bool ok;
+                    int index = idText.toInt(&ok);
+
+                    if (ok && index >= 0 && index < wps.count())
+                    {
+                        Waypoint* wp = wps.at(index);
+                        wp->setLatitude(latitude);
+                        wp->setLongitude(longitude);
+                        wp->setAltitude(altitude);
+                        //                    Waypoint wp;
+                        //                    wp.setFrame(MAV_FRAME_GLOBAL);
+                        //                    wp.setLatitude(latitude);
+                        //                    wp.setLongitude(longitude);
+                        //                    wp.setAltitude(altitude);
+                        //                    mav->getWaypointManager()->addWaypoint(wp);
+                        mav->getWaypointManager()->notifyOfChange(wp);
+                    }
+                }
+            }
+            else
+            {
+                // If coords were not ok, move the view in google earth back
+                // to last acceptable location
+                updateWaypointList(mav->getUASID());
+            }
+            javaScript("setDragWaypointPending(false);");
+        }
     }
 }
 
diff --git a/src/ui/map3D/QGCGoogleEarthView.h b/src/ui/map3D/QGCGoogleEarthView.h
index c4a7b4704939211086397ae96ba9c466ac150439..f4e29a3f8b0f21e2c2b26cf93cc4f3262a9c1367 100644
--- a/src/ui/map3D/QGCGoogleEarthView.h
+++ b/src/ui/map3D/QGCGoogleEarthView.h
@@ -109,6 +109,8 @@ public slots:
 public:
     /** @brief Execute java script inside the Google Earth window */
     QVariant javaScript(QString javascript);
+    /** @brief Get a document element */
+    QVariant documentElement(QString name);
 
 protected:
     void changeEvent(QEvent *e);