Fixed 2D map regarding waypoints (almost done), added option to replay image logs in HUD view

images/earth.html

@@ -192,40 +192,38 @@ function setViewRange(dist)
 function addTrailPosition(id, lat, lon, alt)
 {
 	trails[id].setExtrude(false);
-trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
+    trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
 
-// Add LineString points
+    // 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
-//lineStyle.getColor().set(color);  // aabbggrr format
+    // 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
+    //lineStyle.getColor().set(color);  // aabbggrr format
 
-// Add the feature to Earth
-if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
+    // Add the feature to Earth
+    if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
 }
 
 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 = 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);
+    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);
 
-        initialized = true;
-        
-        
+    initialized = true;
 }
 
 function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
@@ -247,13 +245,12 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 	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);
-	
+    planeModel.setLocation(planeLoc);
+    }
 }
 
 function enableDaylight(enabled)

src/uas/PxQuadMAV.cc

@@ -64,6 +64,15 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "Temperature", "raw", raw.temp, time);
             }
             break;
+        case MAVLINK_MSG_ID_IMAGE_TRIGGERED:
+            {
+                // FIXME Kind of a hack to load data from disk
+                mavlink_image_triggered_t img;
+                mavlink_msg_image_triggered_decode(&message, &img);
+                qDebug() << "IMAGE AVAILABLE:" << img.timestamp;
+                emit imageStarted(img.timestamp);
+            }
+            break;
         case MAVLINK_MSG_ID_PATTERN_DETECTED:
             {
                 mavlink_pattern_detected_t detected;

src/uas/UAS.cc

@@ -464,6 +464,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "latitude", "deg", pos.lat, time);
                 emit valueChanged(uasId, "longitude", "deg", pos.lon, time);
 
+                // FIXME REMOVE
+                longitude = pos.lon;
+                latitude = pos.lat;
+                altitude = pos.alt;
+                emit globalPositionChanged(this, longitude, latitude, altitude, time);
+
                 if (pos.fix_type > 0)
                 {
                     emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);

src/uas/UAS.h

@@ -169,7 +169,7 @@ public:
     bool isAuto();
 
 public:
-    UASWaypointManager &getWaypointManager(void) { return waypointManager; }
+    UASWaypointManager* getWaypointManager() { return &waypointManager; }
     int getSystemType();
     int getAutopilotType() {return autopilot;}
 
@@ -303,6 +303,7 @@ signals:
     void loadChanged(UASInterface* uas, double load);
     /** @brief Propagate a heartbeat received from the system */
     void heartbeat(UASInterface* uas);
+    void imageStarted(quint64 timestamp);
 
     protected:
     /** @brief Get the UNIX timestamp in microseconds */

src/uas/UASInterface.h

@@ -79,7 +79,7 @@ public:
     virtual double getYaw() const = 0;
 
     /** @brief Get reference to the waypoint manager **/
-    virtual UASWaypointManager &getWaypointManager(void) = 0;
+    virtual UASWaypointManager* getWaypointManager(void) = 0;
 
     /* COMMUNICATION FLAGS */
 

src/uas/UASWaypointManager.cc

@@ -265,6 +265,13 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
     }
 }
 
+void UASWaypointManager::notifyOfChange(Waypoint* wp)
+{
+    Q_UNUSED(wp);
+    emit waypointListChanged();
+    emit waypointListChanged(uas.getUASID());
+}
+
 int UASWaypointManager::setCurrentWaypoint(quint16 seq)
 {
     if (seq < waypoints.size())
@@ -313,6 +320,7 @@ void UASWaypointManager::addWaypoint(Waypoint *wp)
         waypoints.insert(waypoints.size(), wp);
 
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
     }
 }
 
@@ -329,6 +337,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
             waypoints[i]->setId(i);
         }
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
         return 0;
     }
     return -1;
@@ -359,6 +368,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
         //waypoints[new_seq]->setId(new_seq);
 
         emit waypointListChanged();
+        emit waypointListChanged(uas.getUASID());
     }
 }
 
@@ -425,6 +435,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
 
     emit loadWPFile();
     emit waypointListChanged();
+    emit waypointListChanged(uas.getUASID());
 }
 
 

src/uas/UASWaypointManager.h

@@ -86,11 +86,11 @@ public:
     /** @name Waypoint list operations */
     /*@{*/
     const QVector<Waypoint *> &getWaypointList(void) { return waypoints; }  ///< Returns a const reference to the waypoint list.
-    void addWaypoint(Waypoint *wp);                        ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
     int removeWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
     void moveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
     void saveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
     void loadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
+    void notifyOfChange(Waypoint* wp);                     ///< Notifies manager to changes to a waypoint
     void localAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
     int localRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
     void localMoveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
@@ -99,6 +99,8 @@ public:
 
     /*@}*/
 
+    UAS& getUAS() { return this->uas; }                         ///< Returns the owning UAS
+
     /** @name Global waypoint list operations */
     /*@{*/
     const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; }  ///< Returns a const reference to the global waypoint list.
@@ -120,14 +122,17 @@ private:
 
 public slots:
     void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
+    void addWaypoint(Waypoint *wp);                 ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
 
 signals:
-    void waypointListChanged(void);                 ///< emits signal that the local waypoint list has been changed
+    void waypointListChanged(void);                 ///< emits signal that the waypoint list has been changed
+    void waypointListChanged(int uasid);            ///< Emits signal that list has been changed
+    void waypointChanged(int uasid, Waypoint* wp);  ///< emits signal that waypoint has been changed
     void currentWaypointChanged(quint16);           ///< emits the new current waypoint sequence number
     void updateStatusString(const QString &);       ///< emits the current status string
 
-    void loadWPFile();                                ///< emits signal that a file wp has been load
-    void readGlobalWPFromUAS(bool value);                            ///< emits signal when finish to read Global WP from UAS
+    void loadWPFile();                              ///< emits signal that a file wp has been load
+    void readGlobalWPFromUAS(bool value);           ///< emits signal when finish to read Global WP from UAS
 
 private:
     UAS &uas;                                       ///< Reference to the corresponding UAS

src/ui/HUD.h

@@ -76,11 +76,19 @@ public slots:
     void updateLoad(UASInterface*, double);
     void selectWaypoint(int uasId, int id);
 
+    void startImage(quint64 timestamp);
     void startImage(int imgid, int width, int height, int depth, int channels);
     void setPixels(int imgid, const unsigned char* imageData, int length, int startIndex);
     void finishImage();
     void saveImage();
     void saveImage(QString fileName);
+    /** @brief Select directory where to load the offline files from */
+    void selectOfflineDirectory();
+    /** @brief Enable the HUD instruments */
+    void enableHUDInstruments(bool enabled);
+    /** @brief Enable Video */
+    void enableVideo(bool enabled);
+
 
 protected slots:
     void paintCenterBackground(float roll, float pitch, float yaw);
@@ -119,6 +127,8 @@ protected:
     void showEvent(QShowEvent* event);
     /** @brief Stop updating widget */
     void hideEvent(QHideEvent* event);
+    void contextMenuEvent (QContextMenuEvent* event);
+    void createActions();
 
     static const int updateInterval = 50;
 
@@ -192,6 +202,16 @@ protected:
     double lon;
     double alt;
     float load;
+    QString offlineDirectory;
+    QString nextOfflineImage;
+    bool hudInstrumentsEnabled;
+    bool videoEnabled;
+    float xImageFactor;
+    float yImageFactor;
+    QAction* enableHUDAction;
+    QAction* enableVideoAction;
+    QAction* selectOfflineDirectoryAction;
+    QAction* selectVideoChannelAction;
     void paintEvent(QPaintEvent *event);
 
 };

src/ui/MainWindow.cc

@@ -408,17 +408,17 @@ void MainWindow::buildPxWidgets()
 #ifdef QGC_OSG_ENABLED
     if (!_3DWidget)
     {
-//        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
-//        addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
+        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
+        addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
     }
 #endif
 
 #ifdef QGC_OSGEARTH_ENABLED
-//    if (!_3DMapWidget)
-//    {
-//        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
-//        addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
-//    }
+    if (!_3DMapWidget)
+    {
+        _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
+        addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
+    }
 #endif
 
 #if (defined _MSC_VER) | (defined Q_OS_MAC)
@@ -853,13 +853,7 @@ void MainWindow::connectCommonWidgets()
 
     if (mapWidget && waypointsDockWidget->widget())
     {
-        // clear path create on the map
-        connect(waypointsDockWidget->widget(), SIGNAL(clearPathclicked()), mapWidget, SLOT(clearWaypoints()));
-        // add Waypoint widget in the WaypointList widget when mouse clicked
-        connect(mapWidget, SIGNAL(captureMapCoordinateClick(QPointF)), waypointsDockWidget->widget(), SLOT(addWaypointMouse(QPointF)));
 
-        // it notifies that a waypoint global goes to do create and a map graphic too
-        connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
     }
 
     //TODO temporaly debug

src/ui/MapWidget.h

@@ -44,6 +44,7 @@ This file is part of the QGROUNDCONTROL project
 
 
 class QMenu;
+class Waypoint;
 
 namespace Ui {
     class MapWidget;
@@ -72,10 +73,18 @@ public slots:
     QPointF getPointxBearing_Range(double lat1, double lon1, double bearing, double distance);
 
     /** @brief Clear the waypoints overlay layer */
-    void clearWaypoints();
+    void clearWaypoints(int uas=0);
     /** @brief Clear the UAV tracks on the map */
-    void clearPath();
-    void changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon);
+    void clearPath(int uas=0);
+
+    /** @brief Update waypoint list */
+    void updateWaypointList(int uas);
+    /** @brief Clear all waypoints and re-add them from updated list */
+    void redoWaypoints(int uas=0);
+
+    /** @brief Update waypoint */
+    void updateWaypoint(int uas, Waypoint* wp);
+
     void drawBorderCamAtMap(bool status);
     /** @brief Bring up dialog to go to a specific location */
     void goTo();
@@ -105,12 +114,12 @@ protected:
     QMenu* mapMenu;
     QPushButton* mapButton;
 
-    qmapcontrol::MapControl* mc;                   ///< QMapControl widget
-    qmapcontrol::MapAdapter* mapadapter;           ///< Adapter to load the map data
-    qmapcontrol::Layer* l;            ///< Current map layer (background)
-    qmapcontrol::Layer* overlay;      ///< Street overlay (foreground)
-    qmapcontrol::Layer* tracks;      ///< Layer for UAV tracks
-    qmapcontrol::GeometryLayer* geomLayer; ///< Layer for waypoints
+    qmapcontrol::MapControl* mc;            ///< QMapControl widget
+    qmapcontrol::MapAdapter* mapadapter;    ///< Adapter to load the map data
+    qmapcontrol::Layer* l;                  ///< Current map layer (background)
+    qmapcontrol::Layer* overlay;            ///< Street overlay (foreground)
+    qmapcontrol::Layer* tracks;             ///< Layer for UAV tracks
+    qmapcontrol::GeometryLayer* geomLayer;  ///< Layer for waypoints
 
     //only for experiment
     qmapcontrol::GeometryLayer* camLayer; ///< Layer for camera indicator
@@ -124,6 +133,9 @@ protected:
 
     QMap<int, qmapcontrol::Point*> uasIcons;
     QMap<int, qmapcontrol::LineString*> uasTrails;
+    QMap<int, QPen*> mavPens;
+    //QMap<int, QList<qmapcontrol::Point*> > mavWps;
+    //QMap<int, qmapcontrol::LineString*> waypointPaths;
     UASInterface* mav;
     quint64 lastUpdate;
 
@@ -135,13 +147,14 @@ protected:
 
     void createPathButtonClicked(bool checked);
 
-    void createWaypointGraphAtMap (const QPointF coordinate);
+    void createWaypointGraphAtMap(const QPointF coordinate);
     void mapproviderSelected(QAction* action);
 
   signals:
     //void movePoint(QPointF newCoord);
-    void captureMapCoordinateClick(const QPointF coordinate); //ROCA
-    void createGlobalWP(bool value, QPointF centerCoordinate);
+    //void captureMapCoordinateClick(const QPointF coordinate); //ROCA
+    //void createGlobalWP(bool value, QPointF centerCoordinate);
+    void waypointCreated(Waypoint* wp);
     void sendGeometryEndDrag(const QPointF coordinate, const int index);
 
 

src/ui/QGCMAVLinkLogPlayer.cc

@@ -319,7 +319,7 @@ void QGCMAVLinkLogPlayer::logLoop()
 
         //qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime;
 
-        if (nextExecutionTime < 5)
+        if (nextExecutionTime < 2)
         {
             logLoop();
         }

src/ui/WaypointView.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1389</width>
-    <height>33</height>
+    <height>39</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -375,7 +375,7 @@ QProgressBar::chunk#thrustBar {
          <string>m</string>
         </property>
         <property name="decimals">
-         <number>7</number>
+         <number>2</number>
         </property>
         <property name="minimum">
          <double>-100000.000000000000000</double>

src/ui/linechart/LinechartWidget.cc

@@ -483,7 +483,7 @@ void LinechartWidget::startLogging()
         fileName.append(".csv");
     }
 
-    while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort)
+    while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort && fileName != "")
     {
         QMessageBox msgBox;
         msgBox.setIcon(QMessageBox::Critical);