Improving Google Earth support

images/earth.html

@@ -13,12 +13,12 @@ google.load("earth", "1", { 'language': 'en'});
 
 var ge = null;
 var initialized = false;
-var aircraft = new Array();
 var currAircraft = 220;
-var followAircraft = false;
+var followEnabled = false;
 var currLat = 47.3769;
 var currLon = 8.549444;
 var currAlt = 470;
+var currFollowHeading = 0.0;
 
 var homeLat = 0;
 var homeLon = 0;
@@ -32,11 +32,17 @@ var currTilt = 40.0;		///<< The tilt angle (in degrees)
 var currFollowTilt = 40.0;
 var currView = null;
 
+var M_PI = 3.14159265;
+
 
 
 var planeOrient;
 var planeLoc;
 
+var aircraft = [];
+var attitudes = [];
+var locations = [];
+var trails = [];
 
 // Aircraft class
 
@@ -53,7 +59,10 @@ function init() {
 
 
 
-
+function setCurrAircraft(id)
+{
+	currAircraft = id;
+}
 
 function setGCSHome(lat, lon, alt)
 {
@@ -86,13 +95,70 @@ function setGCSHome(lat, lon, alt)
 	    {
 		    homeGroundLevel = alt;
 	    }
-	
-	
-	
-	
-	
-	
-	goHome();
+}
+
+function createAircraft(id, type, color)
+{
+console.debug("Aircraft created");
+	      var planePlacemark = ge.createPlacemark('');
+          planePlacemark.setName('aircraft');
+          var planeModel = ge.createModel('');
+          ge.getFeatures().appendChild(planePlacemark);
+          var planeLoc = ge.createLocation('');
+          planeModel.setLocation(planeLoc);
+          var planeLink = ge.createLink('');
+          var planeOrient = ge.createOrientation(''); 
+          planeModel.setOrientation(planeOrient); 
+
+          planeLink.setHref('http://qgroundcontrol.org/_media/users/models/multiplex-twinstar.dae');
+          planeModel.setLink(planeLink);
+          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;
+          locations[id] = planeLoc;
+          
+          createTrail(id, color);
+          
+          console.debug("Aircraft created");
+}
+
+function createTrail(id, color)
+{
+// Create the placemark
+var lineStringPlacemark = ge.createPlacemark('');
+
+// Create the LineString; set it to extend down to the ground
+// and set the altitude mode
+trails[id] = ge.createLineString('');
+lineStringPlacemark.setGeometry(trails[id]);
+trails[id].setExtrude(true);
+lineString.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
+lineStringPlacemark.setStyleSelector(ge.createStyle(''));
+var lineStyle = lineStringPlacemark.getStyleSelector().getLineStyle();
+lineStyle.setWidth(5);
+lineStyle.getColor().set('99bbaaff');
+//lineStyle.getColor().set(color);  // aabbggrr format
+
+// Add the feature to Earth
+ge.getFeatures().appendChild(lineStringPlacemark);
+}
+
+function addTrailPosition(id, lat, lon, alt)
+{
+	//trails[id].getCoordinates().pushLatLngAlt(lat, lon, alt);
 }
 
 function initCallback(object)
@@ -110,7 +176,7 @@ function initCallback(object)
         ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
         
         // Now after the Google Earth initialization, initialize the GCS view
-        setGCSHome(currLat, currLon, currAlt);
+        //setGCSHome(currLat, currLon, currAlt);
         
         
         // Create the first aircraft model
@@ -135,8 +201,8 @@ function initCallback(object)
 
           planePlacemark.setGeometry(planeModel);
           
-          setAircraftPositionAttitude(220, 47.3772, currLon, currAlt+50, 20, 15, 50);
-          enableFollowing(true);
+          //setAircraftPositionAttitude(220, 47.3772, currLon, currAlt+50, 20, 15, 50);
+          //enableFollowing(true);
           updateFollowAircraft();
         initialized = true;
         
@@ -150,17 +216,19 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 		currLat = lat;
 		currLon = lon;
 		currAlt = alt;
+		currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
 	}
 	
-	planeOrient.setRoll(roll); 
-	planeOrient.setTilt(pitch); 
-	        planeOrient.setHeading(yaw);
+	// FIXME Currently invalid conversion from right-handed z-down to z-up frame
+	planeOrient.setRoll(((roll/M_PI)+1.0)*360.0);
+	planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0);
+	planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0);
         
         
                 
-        planeLoc.setLatitude(lat);
-        planeLoc.setLongitude(lon);
-        planeLoc.setAltitude(alt);
+    planeLoc.setLatitude(lat);
+    planeLoc.setLongitude(lon);
+    planeLoc.setAltitude(alt);
 	
 }
 
@@ -180,24 +248,16 @@ function setCurrentAircraft(id)
 	currAircraft = id;
 }
 
-function enableFollowing(follow)
-{
-	followEnabled = follow;
-}
-
-
 function updateFollowAircraft()
 {
-	if (followEnabled)
-	{
 	currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
 	currView.setLatitude(currLat);
 	currView.setLongitude(currLon);
 	currView.setAltitude(currAlt);
 	currView.setRange(currViewRange);
 	currView.setTilt(currFollowTilt);
+	currView.setHeading(currFollowHeading-90.0);
 	ge.getView().setAbstractView(currView);
-	}
 }
 
 function failureCallback(object)

src/ui/MainWindow.cc

@@ -1119,41 +1119,9 @@ void MainWindow::loadMAVLinkView()
 
 void MainWindow::presentView()
 {
-
-    showTheCentralWidget(CENTRAL_3D_LOCAL, currentView);
-    showTheCentralWidget(CENTRAL_3D_MAP, currentView);
-//#ifdef QGC_OSG_ENABLED
-//    // 3D map
-//    if (_3DWidget)
-//    {
-//        if (centerStack)
-//        {
-//            //map3DWidget->setActive(true);
-//            centerStack->setCurrentWidget(_3DWidget);
-//        }
-//    }
-//#endif
-
     qDebug() << "LC";
     showTheCentralWidget(CENTRAL_LINECHART, currentView);
 
-
-    // FIXME
-
-//    if (linechartWidget)
-//    {
-//        qDebug () << buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView) <<
-//                settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView)).toBool() ;
-//        if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView)).toBool()){
-//            if (centerStack)
-//            {
-//                centerStack->setCurrentWidget(linechartWidget);
-//            }
-//        }
-//    }
-
-
-
     // MAP
     qDebug() << "MAP";
     showTheCentralWidget(CENTRAL_MAP, currentView);
@@ -1164,16 +1132,17 @@ void MainWindow::presentView()
 
     // HEAD UP DISPLAY
     showTheCentralWidget(CENTRAL_HUD, currentView);
-//    qDebug() << "HUD";
-//    if (hudWidget){
-//        qDebug() << buildMenuKey(SUB_SECTION_CHECKED,CENTRAL_HUD,currentView) <<
-//                settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_HUD,currentView)).toBool();
-//        if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_HUD,currentView)).toBool()){
-//            if (centerStack) {
-//                centerStack->setCurrentWidget(hudWidget);
-//            }
-//        }
-//    }
+
+    // GOOGLE EARTH
+    showTheCentralWidget(CENTRAL_GOOGLE_EARTH, currentView);
+
+    // LOCAL 3D VIEW
+    showTheCentralWidget(CENTRAL_3D_LOCAL, currentView);
+
+    // GLOBAL 3D VIEW
+    showTheCentralWidget(CENTRAL_3D_MAP, currentView);
+
+
 
     // Show docked widgets based on current view and autopilot type
 

src/ui/map3D/Q3DWidget.cc

@@ -48,6 +48,7 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     , robotAttitude(new osg::PositionAttitudeTransform())
     , hudGroup(new osg::Switch())
     , hudProjectionMatrix(new osg::Projection)
+    , fps(30.0f)
 {
     // set initial camera parameters
     cameraParams.minZoomRange = 2.0f;
@@ -70,6 +71,8 @@ Q3DWidget::~Q3DWidget()
 void
 Q3DWidget::init(float fps)
 {
+    this->fps = fps;
+
     getCamera()->setGraphicsContext(osgGW);
 
     // manually specify near and far clip planes
@@ -105,6 +108,23 @@ Q3DWidget::init(float fps)
     timer.start(static_cast<int>(floorf(1000.0f / fps)));
 }
 
+void Q3DWidget::showEvent(QShowEvent* event)
+{
+    // React only to internal (pre/post-display)
+    // events
+    if (!event->spontaneous())
+    {
+        if (event->type() == QEvent::Hide)
+        {
+            timer.stop();
+        }
+        else if (event->type() == QEvent::Show)
+        {
+            timer.start(static_cast<int>(floorf(1000.0f / fps)));
+        }
+    }
+}
+
 osg::ref_ptr<osg::Geode>
 Q3DWidget::createRobot(void)
 {

src/ui/map3D/Q3DWidget.h

@@ -257,6 +257,7 @@ protected:
     };
 
     CameraParams cameraParams; /**< Struct representing camera parameters. */
+    float fps;
 };
 
 #endif // Q3DWIDGET_H

src/ui/map3D/QGCGoogleEarthView.h

@@ -54,14 +54,24 @@ public:
 public slots:
     /** @brief Update the internal state. Does not trigger a redraw */
     void updateState();
+    /** @brief Add a new MAV/UAS to the visualization */
+    void addUAS(UASInterface* uas);
     /** @brief Set the currently selected UAS */
     void setActiveUAS(UASInterface* uas);
+    /** @brief Update the global position */
+    void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);
     /** @brief Show the vehicle trail */
     void showTrail(bool state);
     /** @brief Show the waypoints */
     void showWaypoints(bool state);
     /** @brief Follow the aircraft during flight */
     void follow(bool follow);
+    /** @brief Go to the home location */
+    void goHome();
+    /** @brief Set the home location */
+    void setHome(double lat, double lon, double alt);
+    /** @brief Initialize Google Earth */
+    void initializeGoogleEarth();
 
 protected:
     void changeEvent(QEvent *e);
@@ -71,6 +81,7 @@ protected:
     bool followCamera;
     bool trailEnabled;
     bool webViewInitialized;
+    bool gEarthInitialized;
 #ifdef _MSC_VER
     QGCWebAxWidget* webViewWin;
 #endif