Merge branch 'master' of https://github.com/mavlink/qgroundcontrol

files/pixhawk/hexrotor/widgets/mavconn.qgw → files/pixhawk/hexarotor/widgets/mavconn.qgw

 [MAVCONN%20Control]
 QGC_TOOL_WIDGET_ITEMS\1\TYPE=COMMANDBUTTON
-QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_DESCRIPTION=START Recording
+QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_DESCRIPTION=DO: Control Video
 QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_BUTTONTEXT=CAPTURE
 QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_COMMANDID=200
 QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false
@@ -12,7 +12,7 @@ QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM5=0
 QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM6=0
 QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM7=0
 QGC_TOOL_WIDGET_ITEMS\2\TYPE=COMMANDBUTTON
-QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_DESCRIPTION=STOP Recording
+QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_DESCRIPTION=DO: Control Video
 QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_BUTTONTEXT=STOP
 QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_COMMANDID=200
 QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false
@@ -35,4 +35,28 @@ QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM4=0
 QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM5=0
 QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM6=0
 QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM7=0
-QGC_TOOL_WIDGET_ITEMS\size=3
+QGC_TOOL_WIDGET_ITEMS\4\TYPE=SLIDER
+QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_DESCRIPTION=Setpoint ON<->OFF
+QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_PARAMID=
+QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_COMPONENTID=0
+QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_MIN=1
+QGC_TOOL_WIDGET_ITEMS\4\QGC_PARAM_SLIDER_MAX=0
+QGC_TOOL_WIDGET_ITEMS\5\TYPE=SLIDER
+QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_DESCRIPTION=Glob. Loc ON<->OFF
+QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_PARAMID=
+QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_COMPONENTID=0
+QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_MIN=1
+QGC_TOOL_WIDGET_ITEMS\5\QGC_PARAM_SLIDER_MAX=0
+QGC_TOOL_WIDGET_ITEMS\6\TYPE=SLIDER
+QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_DESCRIPTION=GPS ENU HL<->ASL
+QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_PARAMID=
+QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_COMPONENTID=0
+QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_MIN=0
+QGC_TOOL_WIDGET_ITEMS\6\QGC_PARAM_SLIDER_MAX=1
+QGC_TOOL_WIDGET_ITEMS\7\TYPE=SLIDER
+QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_DESCRIPTION=Yaw PX<->ASL
+QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_PARAMID=
+QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_COMPONENTID=0
+QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_MIN=0
+QGC_TOOL_WIDGET_ITEMS\7\QGC_PARAM_SLIDER_MAX=1
+QGC_TOOL_WIDGET_ITEMS\size=7

qgroundcontrol.pri

@@ -333,6 +333,10 @@ linux-g++-64 {
             -losgQt \
             -lOpenThreads
 
+    exists(/usr/local/lib64) {
+    LIBS += -L/usr/local/lib64
+    }
+
     DEFINES += QGC_OSG_ENABLED
     DEFINES += QGC_OSG_QT_ENABLED
     }
@@ -358,11 +362,28 @@ linux-g++-64 {
     }
 
     # Validated copy commands
-    QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR
-    QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR
-    QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR
-    QMAKE_POST_LINK += && mkdir -p $$TARGETDIRimages
-    QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$TARGETDIR/images/Vera.ttf
+    debug {
+        !exists($$TARGETDIR/debug){
+             QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/debug
+        }
+        DESTDIR = $$TARGETDIR/debug
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/debug
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/debug
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/debug
+        QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/debug/images
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$TARGETDIR/debug/images/Vera.ttf
+    }
+    release {
+        !exists($$TARGETDIR/release){
+             QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/release
+        }
+        DESTDIR = $$TARGETDIR/release
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/release
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/files $$TARGETDIR/release
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/release
+        QMAKE_POST_LINK += && mkdir -p $$TARGETDIR/release/images
+        QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$TARGETDIR/release/images/Vera.ttf
+    }
 
     # osg/osgEarth dynamic casts might fail without this compiler option.
     # see http://osgearth.org/wiki/FAQ for details.

qgroundcontrol.pro

@@ -361,7 +361,8 @@ HEADERS += src/MG.h \
     src/ui/mavlink/QGCMAVLinkMessageSender.h \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h \
     src/ui/QGCPluginHost.h \
-    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h
+    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h \
+    src/ui/map3D/gpl.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
@@ -494,7 +495,8 @@ SOURCES += src/main.cc \
     src/ui/mavlink/QGCMAVLinkMessageSender.cc \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc \
     src/ui/QGCPluginHost.cc \
-    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc
+    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc \
+    src/ui/map3D/gpl.cc
 
 # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
 macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

qgroundcontrol.qrc

@@ -75,6 +75,7 @@
         <file>images/status/audio-volume-medium.svg</file>
         <file>images/status/audio-volume-low.svg</file>
         <file>images/status/audio-volume-high.svg</file>
+	<file>images/status/colorbars.png</file>
         <file>images/style-mission.css</file>
         <file>images/splash.png</file>
         <file>audio/alert.wav</file>

src/QGC.cc

@@ -47,6 +47,15 @@ quint64 groundTimeMilliseconds()
     return static_cast<quint64>(seconds + (time.time().msec()));
 }
 
+qreal groundTimeSeconds()
+{
+    QDateTime time = QDateTime::currentDateTime();
+    time = time.toUTC();
+    /* Return time in seconds unit */
+    quint64 seconds = time.toTime_t();
+    return static_cast<qreal>(seconds + (time.time().msec() / 1000.0));
+}
+
 float limitAngleToPMPIf(float angle)
 {
     if (angle > -20*M_PI && angle < 20*M_PI)

src/QGC.h

@@ -77,6 +77,8 @@ const QColor colorBlack(0, 0, 0);
 quint64 groundTimeUsecs();
 /** @brief Get the current ground time in milliseconds */
 quint64 groundTimeMilliseconds();
+/** @brief Get the current ground time in seconds */
+qreal groundTimeSeconds();
 /** @brief Returns the angle limited to -pi - pi */
 float limitAngleToPMPIf(float angle);
 /** @brief Returns the angle limited to -pi - pi */

src/comm/MAVLinkProtocol.cc

@@ -31,6 +31,10 @@
 #include "QGCMAVLinkUASFactory.h"
 #include "QGC.h"
 
+#ifdef QGC_PROTOBUF_ENABLED
+#include <google/protobuf/descriptor.h>
+#endif
+
 
 /**
  * The default constructor will create a new MAVLink object sending heartbeats at
@@ -198,6 +202,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 //	    }
 //#endif
 #ifdef QGC_PROTOBUF_ENABLED
+
             if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
             {
                 mavlink_extended_message_t extended_message;
@@ -219,7 +224,42 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 
                     if (protobufManager.getMessage(protobuf_msg))
                     {
-                        emit extendedMessageReceived(link, protobuf_msg);
+                        const google::protobuf::Descriptor* descriptor = protobuf_msg->GetDescriptor();
+                        if (!descriptor)
+                        {
+                            continue;
+                        }
+
+                        const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
+                        if (!headerField)
+                        {
+                            continue;
+                        }
+
+                        const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
+                        if (!headerDescriptor)
+                        {
+                            continue;
+                        }
+
+                        const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
+                        if (!sourceSysIdField)
+                        {
+                            continue;
+                        }
+
+                        const google::protobuf::Reflection* reflection = protobuf_msg->GetReflection();
+                        const google::protobuf::Message& headerMsg = reflection->GetMessage(*protobuf_msg, headerField);
+                        const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();
+
+                        int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);
+
+                        UASInterface* uas = UASManager::instance()->getUASForId(source_sysid);
+
+                        if (uas != NULL)
+                        {
+                            emit extendedMessageReceived(link, protobuf_msg);
+                        }
                     }
                 }
 

src/uas/QGCUASParamManager.h

@@ -20,8 +20,20 @@ public:
     QList<QVariant> getParameterValues(int component) const {
         return parameters.value(component)->values();
     }
-    QVariant getParameterValue(int component, const QString& parameter) const {
-        return parameters.value(component)->value(parameter);
+    bool getParameterValue(int component, const QString& parameter, QVariant& value) const {
+        if (!parameters.contains(component))
+        {
+            return false;
+        }
+
+        if (!parameters.value(component)->contains(parameter))
+        {
+            return false;
+        }
+
+        value = parameters.value(component)->value(parameter);
+
+        return true;
     }
 
     virtual bool isParamMinKnown(const QString& param) = 0;

src/uas/UAS.cc

@@ -27,6 +27,10 @@
 #include "LinkManager.h"
 #include "SerialLink.h"
 
+#ifdef QGC_PROTOBUF_ENABLED
+#include <google/protobuf/descriptor.h>
+#endif
+
 UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     uasId(id),
     startTime(QGC::groundTimeMilliseconds()),
@@ -71,6 +75,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     pitch(0.0),
     yaw(0.0),
     statusTimeout(new QTimer(this)),
+#ifdef QGC_PROTOBUF_ENABLED
+    receivedPointCloudTimestamp(0.0),
+    receivedRGBDImageTimestamp(0.0),
+    receivedObstacleListTimestamp(0.0),
+    receivedPathTimestamp(0.0),
+#endif
     paramsOnceRequested(false),
     airframe(QGC_AIRFRAME_EASYSTAR),
     attitudeKnown(false),
@@ -230,9 +240,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         bool multiComponentSourceDetected = false;
         bool wrongComponent = false;
 
+        switch (message.compid)
+        {
+        case MAV_COMP_ID_IMU_2:
+            // Prefer IMU 2 over IMU 1 (FIXME)
+            componentID[message.msgid] = MAV_COMP_ID_IMU_2;
+            break;
+        default:
+            // Do nothing
+            break;
+        }
+
         // Store component ID
         if (componentID[message.msgid] == -1)
         {
+            // Prefer the first component
             componentID[message.msgid] = message.compid;
         }
         else
@@ -440,9 +462,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 //                    compass = 0.0f;
 //                }
 
-
                 attitudeKnown = true;
                 emit attitudeChanged(this, roll, pitch, yaw, time);
+                emit attitudeChanged(this, message.compid, roll, pitch, yaw, time);
                 emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
             }
             break;
@@ -478,19 +500,39 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_local_position_ned_t pos;
                 mavlink_msg_local_position_ned_decode(&message, &pos);
                 quint64 time = getUnixTime(pos.time_boot_ms);
-                localX = pos.x;
-                localY = pos.y;
-                localZ = pos.z;
-                emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
-                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
 
-                // Set internal state
-                if (!positionLock) {
-                    // If position was not locked before, notify positive
-                    GAudioOutput::instance()->notifyPositive();
+                // Emit position always with component ID
+                emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
+
+                if (!wrongComponent)
+                {
+
+                    localX = pos.x;
+                    localY = pos.y;
+                    localZ = pos.z;
+
+                    // Emit
+
+                    emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
+                    emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
+
+                    // Set internal state
+                    if (!positionLock) {
+                        // If position was not locked before, notify positive
+                        GAudioOutput::instance()->notifyPositive();
+                    }
+                    positionLock = true;
+                    isLocalPositionKnown = true;
                 }
-                positionLock = true;
-                isLocalPositionKnown = true;
+            }
+            break;
+        case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
+            {
+                mavlink_global_vision_position_estimate_t pos;
+                mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
+                quint64 time = getUnixTime(pos.usec);
+                emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
+                emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
             }
             break;
         case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
@@ -786,6 +828,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
             }
             break;
+        case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
+            {
+                mavlink_set_local_position_setpoint_t p;
+                mavlink_msg_set_local_position_setpoint_decode(&message, &p);
+                emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw);
+            }
+            break;
         case MAVLINK_MSG_ID_STATUSTEXT:
             {
                 QByteArray b;
@@ -955,7 +1004,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
 #endif
             // Messages to ignore
-        case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
         case MAVLINK_MSG_ID_RAW_IMU:
         case MAVLINK_MSG_ID_SCALED_IMU:
         case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
@@ -988,27 +1036,82 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 #ifdef QGC_PROTOBUF_ENABLED
 void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message)
 {
-    if (!link) return;
+    if (!link)
+    {
+        return;
+    }
     if (!links->contains(link))
     {
         addLink(link);
     }
 
+    const google::protobuf::Descriptor* descriptor = message->GetDescriptor();
+    if (!descriptor)
+    {
+        return;
+    }
+
+    const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
+    if (!headerField)
+    {
+        return;
+    }
+
+    const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
+    if (!headerDescriptor)
+    {
+        return;
+    }
+
+    const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
+    if (!sourceSysIdField)
+    {
+        return;
+    }
+
+    const google::protobuf::Reflection* reflection = message->GetReflection();
+    const google::protobuf::Message& headerMsg = reflection->GetMessage(*message, headerField);
+    const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();
+
+    int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);
+
+    if (source_sysid != uasId)
+    {
+        return;
+    }
+
     if (message->GetTypeName() == pointCloud.GetTypeName())
     {
+        receivedPointCloudTimestamp = QGC::groundTimeSeconds();
+        pointCloudMutex.lock();
         pointCloud.CopyFrom(*message);
+        pointCloudMutex.unlock();
         emit pointCloudChanged(this);
     }
     else if (message->GetTypeName() == rgbdImage.GetTypeName())
     {
+        receivedRGBDImageTimestamp = QGC::groundTimeSeconds();
+        rgbdImageMutex.lock();
         rgbdImage.CopyFrom(*message);
+        rgbdImageMutex.unlock();
         emit rgbdImageChanged(this);
     }
     else if (message->GetTypeName() == obstacleList.GetTypeName())
     {
+        receivedObstacleListTimestamp = QGC::groundTimeSeconds();
+        obstacleListMutex.lock();
         obstacleList.CopyFrom(*message);
+        obstacleListMutex.unlock();
         emit obstacleListChanged(this);
     }
+    else if (message->GetTypeName() == path.GetTypeName())
+    {
+        receivedPathTimestamp = QGC::groundTimeSeconds();
+        pathMutex.lock();
+        path.CopyFrom(*message);
+        pathMutex.unlock();
+        emit pathChanged(this);
+    }
 }
 
 #endif

src/uas/UAS.h

@@ -135,17 +135,49 @@ public:
     bool getSelected() const;
 
 #ifdef QGC_PROTOBUF_ENABLED
-    px::PointCloudXYZRGB getPointCloud() const {
+    px::PointCloudXYZRGB getPointCloud() {
+        QMutexLocker locker(&pointCloudMutex);
         return pointCloud;
     }
 
-    px::RGBDImage getRGBDImage() const {
+    px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) {
+        receivedTimestamp = receivedPointCloudTimestamp;
+        QMutexLocker locker(&pointCloudMutex);
+        return pointCloud;
+    }
+
+    px::RGBDImage getRGBDImage() {
+        QMutexLocker locker(&rgbdImageMutex);
+        return rgbdImage;
+    }
+
+    px::RGBDImage getRGBDImage(qreal& receivedTimestamp) {
+        receivedTimestamp = receivedRGBDImageTimestamp;
+        QMutexLocker locker(&rgbdImageMutex);
         return rgbdImage;
     }
 
-    px::ObstacleList getObstacleList() const {
+    px::ObstacleList getObstacleList() {
+        QMutexLocker locker(&obstacleListMutex);
+        return obstacleList;
+    }
+
+    px::ObstacleList getObstacleList(qreal& receivedTimestamp) {
+        receivedTimestamp = receivedObstacleListTimestamp;
+        QMutexLocker locker(&obstacleListMutex);
         return obstacleList;
     }
+
+    px::Path getPath() {
+        QMutexLocker locker(&pathMutex);
+        return path;
+    }
+
+    px::Path getPath(qreal& receivedTimestamp) {
+        receivedTimestamp = receivedPathTimestamp;
+        QMutexLocker locker(&pathMutex);
+        return path;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -233,8 +265,20 @@ protected: //COMMENTS FOR TEST UNIT
 
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
+    QMutex pointCloudMutex;
+    qreal receivedPointCloudTimestamp;
+
     px::RGBDImage rgbdImage;
+    QMutex rgbdImageMutex;
+    qreal receivedRGBDImageTimestamp;
+
     px::ObstacleList obstacleList;
+    QMutex obstacleListMutex;
+    qreal receivedObstacleListTimestamp;
+
+    px::Path path;
+    QMutex pathMutex;
+    qreal receivedPathTimestamp;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
@@ -573,6 +617,8 @@ signals:
     void rgbdImageChanged(UASInterface* uas);
     /** @brief Obstacle list data has been changed */
     void obstacleListChanged(UASInterface* uas);
+    /** @brief Path data has been changed */
+    void pathChanged(UASInterface* uas);
 #endif
     /** @brief HIL controls have changed */
     void hilControlsChanged(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode);

src/uas/UASInterface.h

@@ -95,9 +95,14 @@ public:
     virtual bool getSelected() const = 0;
 
 #ifdef QGC_PROTOBUF_ENABLED
-    virtual px::PointCloudXYZRGB getPointCloud() const = 0;
-    virtual px::RGBDImage getRGBDImage() const = 0;
-    virtual px::ObstacleList getObstacleList() const = 0;
+    virtual px::PointCloudXYZRGB getPointCloud() = 0;
+    virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) = 0;
+    virtual px::RGBDImage getRGBDImage() = 0;
+    virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) = 0;
+    virtual px::ObstacleList getObstacleList() = 0;
+    virtual px::ObstacleList getObstacleList(qreal& receivedTimestamp) = 0;
+    virtual px::Path getPath() = 0;
+    virtual px::Path getPath(qreal& receivedTimestamp) = 0;
 #endif
 
     virtual bool isArmed() const = 0;
@@ -442,10 +447,15 @@ signals:
     void thrustChanged(UASInterface*, double thrust);
     void heartbeat(UASInterface* uas);
     void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
+    void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec);
     void attitudeSpeedChanged(int uas, double rollspeed, double pitchspeed, double yawspeed, quint64 usec);
     void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
+    /** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
     void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
+    /** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */
+    void userPositionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
     void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec);
+    void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec);
     void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec);
     void altitudeChanged(int uasid, double altitude);
     /** @brief Update the status of one satellite used for localization */

src/uas/UASWaypointManager.cc

@@ -61,6 +61,11 @@ UASWaypointManager::UASWaypointManager(UAS* _uas)
     }
 }
 
+UASWaypointManager::~UASWaypointManager()
+{
+
+}
+
 void UASWaypointManager::timeout()
 {
     if (current_retries > 0) {

src/uas/UASWaypointManager.h

@@ -65,6 +65,7 @@ private:
 
 public:
     UASWaypointManager(UAS* uas=NULL);   ///< Standard constructor
+    ~UASWaypointManager();
 
     /** @name Received message handlers */
     /*@{*/

src/ui/HSIDisplay.cc

@@ -668,6 +668,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
+        disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
         disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
@@ -688,6 +689,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
     connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
+    connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
     connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
@@ -811,6 +813,16 @@ void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, do
     this->yaw = yaw;
 }
 
+void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired)
+{
+    uiXSetCoordinate = xDesired;
+    uiYSetCoordinate = yDesired;
+    uiZSetCoordinate = zDesired;
+    uiYawSet = yawDesired;
+    userXYSetPointSet = true;
+    userSetPointSet = true;
+}
+
 void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec)
 {
     Q_UNUSED(usec);

src/ui/HSIDisplay.h

@@ -57,6 +57,7 @@ public slots:
     void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
     void updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
+    void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
     void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
     void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
     void updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec);

src/ui/HUD.h

@@ -61,7 +61,7 @@ public slots:
     //void paintGL();
 
     /** @brief Set the currently monitored UAS */
-    void setActiveUAS(UASInterface* uas);
+    virtual void setActiveUAS(UASInterface* uas);
 
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
 //    void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);

src/ui/QGCRGBDView.cc

@@ -21,16 +21,40 @@ QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) :
     enableDepthAction->setChecked(depthEnabled);
     connect(enableDepthAction, SIGNAL(triggered(bool)), this, SLOT(enableDepth(bool)));
 
-    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+
+    clearData();
+}
+
+void QGCRGBDView::setActiveUAS(UASInterface* uas)
+{
+    if (this->uas != NULL)
+    {
+        // Disconnect any previously connected active MAV
+        disconnect(this->uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+
+        clearData();
+    }
+
+    if (uas)
+    {
+        // Now connect the new UAS
+        // Setup communication
+        connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+    }
+
+    HUD::setActiveUAS(uas);
 }
 
-void QGCRGBDView::addUAS(UASInterface *uas)
+void QGCRGBDView::clearData(void)
 {
-    // TODO Enable multi-uas support
-    connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+    QImage offlineImg;
+    qDebug() << offlineImg.load(":/images/status/colorbars.png");
+
+    glImage = QGLWidget::convertToGLFormat(offlineImg);
 }
 
-void QGCRGBDView::contextMenuEvent (QContextMenuEvent* event)
+void QGCRGBDView::contextMenuEvent(QContextMenuEvent* event)
 {
     QMenu menu(this);
     // Update actions
@@ -248,7 +272,7 @@ void QGCRGBDView::updateData(UASInterface *uas)
             {
                 if (depth[c] != 0)
                 {
-                    int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f;
+                    int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
                     idx = 127 - idx;
 
                     pixel[0] = colormapJet[idx][2] * 255.0f;

src/ui/QGCRGBDView.h

@@ -12,7 +12,9 @@ public:
 signals:
 
 public slots:
-    void addUAS(UASInterface* uas);
+    void setActiveUAS(UASInterface* uas);
+
+    void clearData(void);
     void enableRGB(bool enabled);
     void enableDepth(bool enabled);
     void updateData(UASInterface *uas);

src/ui/WaypointList.cc

@@ -155,7 +155,10 @@ void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch,
 
 void WaypointList::setUAS(UASInterface* uas)
 {
-    //if (this->uas == NULL && uas != NULL)
+    if (this->uas == NULL && uas != NULL)
+    {
+        WPM = uas->getWaypointManager();
+    }
     if (this->uas == NULL)
     {
         this->uas = uas;