Waypoint creation and drag-and-drop working in Google Earth

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
@@ -158,6 +279,7 @@ function updateWaypoint(id, index, lat, lon, alt, action)
 	    style.getIconStyle().setIcon(icon); 
 	    //style.getIconStyle().setScale(0.5); 
 	    placemark.setStyleSelector(style);
+	    placemark.setDescription(index+"");
 
 	    // Set the placemark's location.   
 	    var location = ge.createPoint(''); 
@@ -286,6 +408,8 @@ function initCallback(object)
     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;
+		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);

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);
     }
 }

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)

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);");
+        }
     }
 }
 

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);