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;

src/ui/map3D/ObstacleGroupNode.cc

@@ -48,25 +48,21 @@ ObstacleGroupNode::init(void)
 }
 
 void
-ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+ObstacleGroupNode::clear(void)
 {
-    if (!uas || frame == MAV_FRAME_GLOBAL)
-    {
-        return;
-    }
-
-    double robotX = uas->getLocalX();
-    double robotY = uas->getLocalY();
-    double robotZ = uas->getLocalZ();
-
     if (getNumChildren() > 0)
     {
         removeChild(0, getNumChildren());
     }
+}
 
-    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+void
+ObstacleGroupNode::update(double robotX, double robotY, double robotZ,
+                          const px::ObstacleList& obstacleList)
+{
+    clear();
 
-    px::ObstacleList obstacleList = uas->getObstacleList();
+    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
 
     for (int i = 0; i < obstacleList.obstacles_size(); ++i)
     {

src/ui/map3D/ObstacleGroupNode.h

@@ -42,7 +42,9 @@ public:
     ObstacleGroupNode();
 
     void init(void);
-    void update(MAV_FRAME frame, UASInterface* uas);
+    void clear(void);
+    void update(double robotX, double robotY, double robotZ,
+                const px::ObstacleList& obstacleList);
 };
 
 #endif // OBSTACLEGROUPNODE_H

src/ui/map3D/Pixhawk3DWidget.h

@@ -59,11 +59,14 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
+    void addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time);
+    void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time);
 
 private slots:
     void selectFrame(QString text);
-    void showGrid(int state);
-    void showTrail(int state);
+    void showLocalGrid(int state);
+    void showWorldGrid(int state);
+    void showTrails(int state);
     void showWaypoints(int state);
     void selectMapSource(int index);
     void selectVehicleModel(int index);
@@ -87,17 +90,8 @@ protected:
     virtual void display(void);
     virtual void keyPressEvent(QKeyEvent* event);
     virtual void mousePressEvent(QMouseEvent* event);
-    void showEvent(QShowEvent* event)
-    {
-        QWidget::showEvent(event);
-        emit visibilityChanged(true);
-    }
-
-    void hideEvent(QHideEvent* event)
-    {
-        QWidget::hideEvent(event);
-        emit visibilityChanged(false);
-    }
+    virtual void showEvent(QShowEvent* event);
+    virtual void hideEvent(QHideEvent* event);
     virtual void mouseMoveEvent(QMouseEvent* event);
 
     UASInterface* uas;
@@ -115,8 +109,9 @@ private:
                      QString& utmZone);
     void getPosition(double& x, double& y, double& z);
 
-    osg::ref_ptr<osg::Geode> createGrid(void);
-    osg::ref_ptr<osg::Geode> createTrail(void);
+    osg::ref_ptr<osg::Geode> createLocalGrid(void);
+    osg::ref_ptr<osg::Geode> createWorldGrid(void);
+    osg::ref_ptr<osg::Geometry> createTrail(const osg::Vec4& color);
     osg::ref_ptr<Imagery> createMap(void);
     osg::ref_ptr<osg::Geode> createRGBD3D(void);
     osg::ref_ptr<osg::Node> createTarget(void);
@@ -127,14 +122,15 @@ private:
     void updateHUD(double robotX, double robotY, double robotZ,
                    double robotRoll, double robotPitch, double robotYaw,
                    const QString& utmZone);
-    void updateTrail(double robotX, double robotY, double robotZ);
+    void updateTrails(double robotX, double robotY, double robotZ);
     void updateImagery(double originX, double originY, double originZ,
                        const QString& zone);
     void updateWaypoints(void);
-    void updateTarget(double robotX, double robotY);
+    void updateTarget(double robotX, double robotY, double robotZ);
 #ifdef QGC_PROTOBUF_ENABLED
     void updateRGBD(double robotX, double robotY, double robotZ);
-    void updateObstacles(void);
+    void updateObstacles(double robotX, double robotY, double robotZ);
+    void updatePath(double robotX, double robotY, double robotZ);
 #endif
 
     int findWaypoint(const QPoint& mousePos);
@@ -142,6 +138,8 @@ private:
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
 
+    const qreal kMessageTimeout; // message timeout in seconds
+
     enum Mode {
         DEFAULT_MODE,
         MOVE_WAYPOINT_POSITION_MODE,
@@ -151,20 +149,22 @@ private:
     Mode mode;
     int selectedWpIndex;
 
-    bool displayGrid;
-    bool displayTrail;
+    bool displayLocalGrid;
+    bool displayWorldGrid;
+    bool displayTrails;
     bool displayImagery;
     bool displayWaypoints;
     bool displayRGBD2D;
     bool displayRGBD3D;
     bool displayObstacleList;
+    bool displayPath;
     bool enableRGBDColor;
     bool enableTarget;
 
     bool followCamera;
 
-    osg::ref_ptr<osg::Vec3dArray> trailVertices;
-    QVarLengthArray<osg::Vec3d, 10000> trail;
+    QMap<int, QVarLengthArray<osg::Vec3d, 10000> > trails;
+    QMap<int, int> trailDrawableIdxs;
 
     osg::ref_ptr<osg::Node> vehicleModel;
     osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;
@@ -174,22 +174,23 @@ private:
     osg::ref_ptr<ImageWindowGeode> depth2DGeode;
     osg::ref_ptr<osg::Image> rgbImage;
     osg::ref_ptr<osg::Image> depthImage;
-    osg::ref_ptr<osg::Geode> gridNode;
+    osg::ref_ptr<osg::Geode> localGridNode;
+    osg::ref_ptr<osg::Geode> worldGridNode;
     osg::ref_ptr<osg::Geode> trailNode;
-    osg::ref_ptr<osg::Geometry> trailGeometry;
-    osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
+    osg::ref_ptr<osg::Group> orientationNode;
     osg::ref_ptr<Imagery> mapNode;
     osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Node> targetNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
 #ifdef QGC_PROTOBUF_ENABLED
     osg::ref_ptr<ObstacleGroupNode> obstacleGroupNode;
+    osg::ref_ptr<osg::Geode> pathNode;
 #endif
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
     MAV_FRAME frame;
-    osg::Vec3d target;
+    QVector4D target;
     QPoint cachedMousePos;
     double lastRobotX, lastRobotY, lastRobotZ;
 };

src/ui/map3D/Q3DWidget.cc

@@ -148,8 +148,8 @@ Q3DWidget::createRobot(void)
     osg::ref_ptr<osg::Vec3Array> coords(new osg::Vec3Array(6));
     (*coords)[0] = (*coords)[2] = (*coords)[4] =
                                       osg::Vec3(0.0f, 0.0f, 0.0f);
-    (*coords)[1] = osg::Vec3(0.15f, 0.0f, 0.0f);
-    (*coords)[3] = osg::Vec3(0.0f, 0.3f, 0.0f);
+    (*coords)[1] = osg::Vec3(0.0f, 0.3f, 0.0f);
+    (*coords)[3] = osg::Vec3(0.15f, 0.0f, 0.0f);
     (*coords)[5] = osg::Vec3(0.0f, 0.0f, -0.15f);
 
     geometry->setVertexArray(coords);

src/ui/map3D/gpl.cc

+#include "gpl.h"
+
+namespace qgc
+{
+
+double hypot3(double x, double y, double z)
+{
+	return sqrt(square(x) + square(y) + square(z));
+}
+
+float hypot3f(float x, float y, float z)
+{
+	return sqrtf(square(x) + square(y) + square(z));
+}
+
+double d2r(double deg)
+{
+	return deg / 180.0 * M_PI;
+}
+
+float d2r(float deg)
+{
+	return deg / 180.0f * M_PI;
+}
+
+double r2d(double rad)
+{
+	return rad / M_PI * 180.0;
+}
+
+float r2d(float rad)
+{
+	return rad / M_PI * 180.0f;
+}
+
+float colormapAutumn[128][3] =
+{
+		{1.0f,0.f,0.f},
+		{1.0f,0.007874f,0.f},
+		{1.0f,0.015748f,0.f},
+		{1.0f,0.023622f,0.f},
+		{1.0f,0.031496f,0.f},
+		{1.0f,0.03937f,0.f},
+		{1.0f,0.047244f,0.f},
+		{1.0f,0.055118f,0.f},
+		{1.0f,0.062992f,0.f},
+		{1.0f,0.070866f,0.f},
+		{1.0f,0.07874f,0.f},
+		{1.0f,0.086614f,0.f},
+		{1.0f,0.094488f,0.f},
+		{1.0f,0.10236f,0.f},
+		{1.0f,0.11024f,0.f},
+		{1.0f,0.11811f,0.f},
+		{1.0f,0.12598f,0.f},
+		{1.0f,0.13386f,0.f},
+		{1.0f,0.14173f,0.f},
+		{1.0f,0.14961f,0.f},
+		{1.0f,0.15748f,0.f},
+		{1.0f,0.16535f,0.f},
+		{1.0f,0.17323f,0.f},
+		{1.0f,0.1811f,0.f},
+		{1.0f,0.18898f,0.f},
+		{1.0f,0.19685f,0.f},
+		{1.0f,0.20472f,0.f},
+		{1.0f,0.2126f,0.f},
+		{1.0f,0.22047f,0.f},
+		{1.0f,0.22835f,0.f},
+		{1.0f,0.23622f,0.f},
+		{1.0f,0.24409f,0.f},
+		{1.0f,0.25197f,0.f},
+		{1.0f,0.25984f,0.f},
+		{1.0f,0.26772f,0.f},
+		{1.0f,0.27559f,0.f},
+		{1.0f,0.28346f,0.f},
+		{1.0f,0.29134f,0.f},
+		{1.0f,0.29921f,0.f},
+		{1.0f,0.30709f,0.f},
+		{1.0f,0.31496f,0.f},
+		{1.0f,0.32283f,0.f},
+		{1.0f,0.33071f,0.f},
+		{1.0f,0.33858f,0.f},
+		{1.0f,0.34646f,0.f},
+		{1.0f,0.35433f,0.f},
+		{1.0f,0.3622f,0.f},
+		{1.0f,0.37008f,0.f},
+		{1.0f,0.37795f,0.f},
+		{1.0f,0.38583f,0.f},
+		{1.0f,0.3937f,0.f},
+		{1.0f,0.40157f,0.f},
+		{1.0f,0.40945f,0.f},
+		{1.0f,0.41732f,0.f},
+		{1.0f,0.4252f,0.f},
+		{1.0f,0.43307f,0.f},
+		{1.0f,0.44094f,0.f},
+		{1.0f,0.44882f,0.f},
+		{1.0f,0.45669f,0.f},
+		{1.0f,0.46457f,0.f},
+		{1.0f,0.47244f,0.f},
+		{1.0f,0.48031f,0.f},
+		{1.0f,0.48819f,0.f},
+		{1.0f,0.49606f,0.f},
+		{1.0f,0.50394f,0.f},
+		{1.0f,0.51181f,0.f},
+		{1.0f,0.51969f,0.f},
+		{1.0f,0.52756f,0.f},
+		{1.0f,0.53543f,0.f},
+		{1.0f,0.54331f,0.f},
+		{1.0f,0.55118f,0.f},
+		{1.0f,0.55906f,0.f},
+		{1.0f,0.56693f,0.f},
+		{1.0f,0.5748f,0.f},
+		{1.0f,0.58268f,0.f},
+		{1.0f,0.59055f,0.f},
+		{1.0f,0.59843f,0.f},
+		{1.0f,0.6063f,0.f},
+		{1.0f,0.61417f,0.f},
+		{1.0f,0.62205f,0.f},
+		{1.0f,0.62992f,0.f},
+		{1.0f,0.6378f,0.f},
+		{1.0f,0.64567f,0.f},
+		{1.0f,0.65354f,0.f},
+		{1.0f,0.66142f,0.f},
+		{1.0f,0.66929f,0.f},
+		{1.0f,0.67717f,0.f},
+		{1.0f,0.68504f,0.f},
+		{1.0f,0.69291f,0.f},
+		{1.0f,0.70079f,0.f},
+		{1.0f,0.70866f,0.f},
+		{1.0f,0.71654f,0.f},
+		{1.0f,0.72441f,0.f},
+		{1.0f,0.73228f,0.f},
+		{1.0f,0.74016f,0.f},
+		{1.0f,0.74803f,0.f},
+		{1.0f,0.75591f,0.f},
+		{1.0f,0.76378f,0.f},
+		{1.0f,0.77165f,0.f},
+		{1.0f,0.77953f,0.f},
+		{1.0f,0.7874f,0.f},
+		{1.0f,0.79528f,0.f},
+		{1.0f,0.80315f,0.f},
+		{1.0f,0.81102f,0.f},
+		{1.0f,0.8189f,0.f},
+		{1.0f,0.82677f,0.f},
+		{1.0f,0.83465f,0.f},
+		{1.0f,0.84252f,0.f},
+		{1.0f,0.85039f,0.f},
+		{1.0f,0.85827f,0.f},
+		{1.0f,0.86614f,0.f},
+		{1.0f,0.87402f,0.f},
+		{1.0f,0.88189f,0.f},
+		{1.0f,0.88976f,0.f},
+		{1.0f,0.89764f,0.f},
+		{1.0f,0.90551f,0.f},
+		{1.0f,0.91339f,0.f},
+		{1.0f,0.92126f,0.f},
+		{1.0f,0.92913f,0.f},
+		{1.0f,0.93701f,0.f},
+		{1.0f,0.94488f,0.f},
+		{1.0f,0.95276f,0.f},
+		{1.0f,0.96063f,0.f},
+		{1.0f,0.9685f,0.f},
+		{1.0f,0.97638f,0.f},
+		{1.0f,0.98425f,0.f},
+		{1.0f,0.99213f,0.f},
+		{1.0f,1.0f,0.0f}
+};
+
+
+float colormapJet[128][3] =
+{
+		{0.0f,0.0f,0.53125f},
+		{0.0f,0.0f,0.5625f},
+		{0.0f,0.0f,0.59375f},
+		{0.0f,0.0f,0.625f},
+		{0.0f,0.0f,0.65625f},
+		{0.0f,0.0f,0.6875f},
+		{0.0f,0.0f,0.71875f},
+		{0.0f,0.0f,0.75f},
+		{0.0f,0.0f,0.78125f},
+		{0.0f,0.0f,0.8125f},
+		{0.0f,0.0f,0.84375f},
+		{0.0f,0.0f,0.875f},
+		{0.0f,0.0f,0.90625f},
+		{0.0f,0.0f,0.9375f},
+		{0.0f,0.0f,0.96875f},
+		{0.0f,0.0f,1.0f},
+		{0.0f,0.03125f,1.0f},
+		{0.0f,0.0625f,1.0f},
+		{0.0f,0.09375f,1.0f},
+		{0.0f,0.125f,1.0f},
+		{0.0f,0.15625f,1.0f},
+		{0.0f,0.1875f,1.0f},
+		{0.0f,0.21875f,1.0f},
+		{0.0f,0.25f,1.0f},
+		{0.0f,0.28125f,1.0f},
+		{0.0f,0.3125f,1.0f},
+		{0.0f,0.34375f,1.0f},
+		{0.0f,0.375f,1.0f},
+		{0.0f,0.40625f,1.0f},
+		{0.0f,0.4375f,1.0f},
+		{0.0f,0.46875f,1.0f},
+		{0.0f,0.5f,1.0f},
+		{0.0f,0.53125f,1.0f},
+		{0.0f,0.5625f,1.0f},
+		{0.0f,0.59375f,1.0f},
+		{0.0f,0.625f,1.0f},
+		{0.0f,0.65625f,1.0f},
+		{0.0f,0.6875f,1.0f},
+		{0.0f,0.71875f,1.0f},
+		{0.0f,0.75f,1.0f},
+		{0.0f,0.78125f,1.0f},
+		{0.0f,0.8125f,1.0f},
+		{0.0f,0.84375f,1.0f},
+		{0.0f,0.875f,1.0f},
+		{0.0f,0.90625f,1.0f},
+		{0.0f,0.9375f,1.0f},
+		{0.0f,0.96875f,1.0f},
+		{0.0f,1.0f,1.0f},
+		{0.03125f,1.0f,0.96875f},
+		{0.0625f,1.0f,0.9375f},
+		{0.09375f,1.0f,0.90625f},
+		{0.125f,1.0f,0.875f},
+		{0.15625f,1.0f,0.84375f},
+		{0.1875f,1.0f,0.8125f},
+		{0.21875f,1.0f,0.78125f},
+		{0.25f,1.0f,0.75f},
+		{0.28125f,1.0f,0.71875f},
+		{0.3125f,1.0f,0.6875f},
+		{0.34375f,1.0f,0.65625f},
+		{0.375f,1.0f,0.625f},
+		{0.40625f,1.0f,0.59375f},
+		{0.4375f,1.0f,0.5625f},
+		{0.46875f,1.0f,0.53125f},
+		{0.5f,1.0f,0.5f},
+		{0.53125f,1.0f,0.46875f},
+		{0.5625f,1.0f,0.4375f},
+		{0.59375f,1.0f,0.40625f},
+		{0.625f,1.0f,0.375f},
+		{0.65625f,1.0f,0.34375f},
+		{0.6875f,1.0f,0.3125f},
+		{0.71875f,1.0f,0.28125f},
+		{0.75f,1.0f,0.25f},
+		{0.78125f,1.0f,0.21875f},
+		{0.8125f,1.0f,0.1875f},
+		{0.84375f,1.0f,0.15625f},
+		{0.875f,1.0f,0.125f},
+		{0.90625f,1.0f,0.09375f},
+		{0.9375f,1.0f,0.0625f},
+		{0.96875f,1.0f,0.03125f},
+		{1.0f,1.0f,0.0f},
+		{1.0f,0.96875f,0.0f},
+		{1.0f,0.9375f,0.0f},
+		{1.0f,0.90625f,0.0f},
+		{1.0f,0.875f,0.0f},
+		{1.0f,0.84375f,0.0f},
+		{1.0f,0.8125f,0.0f},
+		{1.0f,0.78125f,0.0f},
+		{1.0f,0.75f,0.0f},
+		{1.0f,0.71875f,0.0f},
+		{1.0f,0.6875f,0.0f},
+		{1.0f,0.65625f,0.0f},
+		{1.0f,0.625f,0.0f},
+		{1.0f,0.59375f,0.0f},
+		{1.0f,0.5625f,0.0f},
+		{1.0f,0.53125f,0.0f},
+		{1.0f,0.5f,0.0f},
+		{1.0f,0.46875f,0.0f},
+		{1.0f,0.4375f,0.0f},
+		{1.0f,0.40625f,0.0f},
+		{1.0f,0.375f,0.0f},
+		{1.0f,0.34375f,0.0f},
+		{1.0f,0.3125f,0.0f},
+		{1.0f,0.28125f,0.0f},
+		{1.0f,0.25f,0.0f},
+		{1.0f,0.21875f,0.0f},
+		{1.0f,0.1875f,0.0f},
+		{1.0f,0.15625f,0.0f},
+		{1.0f,0.125f,0.0f},
+		{1.0f,0.09375f,0.0f},
+		{1.0f,0.0625f,0.0f},
+		{1.0f,0.03125f,0.0f},
+		{1.0f,0.0f,0.0f},
+		{0.96875f,0.0f,0.0f},
+		{0.9375f,0.0f,0.0f},
+		{0.90625f,0.0f,0.0f},
+		{0.875f,0.0f,0.0f},
+		{0.84375f,0.0f,0.0f},
+		{0.8125f,0.0f,0.0f},
+		{0.78125f,0.0f,0.0f},
+		{0.75f,0.0f,0.0f},
+		{0.71875f,0.0f,0.0f},
+		{0.6875f,0.0f,0.0f},
+		{0.65625f,0.0f,0.0f},
+		{0.625f,0.0f,0.0f},
+		{0.59375f,0.0f,0.0f},
+		{0.5625f,0.0f,0.0f},
+		{0.53125f,0.0f,0.0f},
+		{0.5f,0.0f,0.0f}
+};
+
+bool colormap(const QString& name, unsigned char idx,
+			  float& r, float& g, float& b)
+{
+    if (idx > 127)
+    {
+        return false;
+    }
+
+	if (name.compare("jet") == 0)
+	{
+		float* color = colormapJet[idx];
+
+		r = color[0];
+		g = color[1];
+		b = color[2];
+
+		return true;
+	}
+	else if (name.compare("autumn") == 0)
+	{
+		float* color = colormapAutumn[idx];
+
+		r = color[0];
+		g = color[1];
+		b = color[2];
+
+		return true;
+	}
+
+	return false;
+}
+
+}

src/ui/map3D/gpl.h

+#ifndef GPL_H
+#define GPL_H
+
+#include <cmath>
+#include <QString>
+
+namespace qgc
+{
+
+template<class T>
+const T clamp(const T& v, const T& a, const T& b)
+{
+    return qMin(b, qMax(a, v));
+}
+
+double hypot3(double x, double y, double z);
+float hypot3f(float x, float y, float z);
+
+template<class T>
+const T normalizeTheta(const T& theta)
+{
+	T normTheta = theta;
+
+    while (normTheta < - M_PI)
+	{
+		normTheta += 2.0 * M_PI;
+	}
+	while (normTheta > M_PI)
+	{
+		normTheta -= 2.0 * M_PI;
+	}
+
+	return normTheta;
+}
+
+double d2r(double deg);
+float d2r(float deg);
+double r2d(double rad);
+float r2d(float rad);
+
+template<class T>
+const T square(const T& x)
+{
+	return x * x;
+}
+
+bool colormap(const QString& name, unsigned char idx,
+			  float& r, float& g, float& b);
+
+}
+
+#endif