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.cc

@@ -43,6 +43,7 @@
 #include "UASManager.h"
 
 #include "QGC.h"
+#include "gpl.h"
 
 #ifdef QGC_PROTOBUF_ENABLED
 #include <tr1/memory>
@@ -52,15 +53,18 @@
 Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     : Q3DWidget(parent)
     , uas(NULL)
+    , kMessageTimeout(4.0)
     , mode(DEFAULT_MODE)
     , selectedWpIndex(-1)
-    , displayGrid(true)
-    , displayTrail(false)
+    , displayLocalGrid(false)
+    , displayWorldGrid(true)
+    , displayTrails(true)
     , displayImagery(true)
     , displayWaypoints(true)
     , displayRGBD2D(false)
     , displayRGBD3D(true)
     , displayObstacleList(true)
+    , displayPath(true)
     , enableRGBDColor(false)
     , enableTarget(false)
     , followCamera(true)
@@ -76,14 +80,20 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     vehicleModel = PixhawkCheetahGeode::instance();
     egocentricMap->addChild(vehicleModel);
 
-    // generate grid model
-    gridNode = createGrid();
-    rollingMap->addChild(gridNode);
+    // generate grid models
+    localGridNode = createLocalGrid();
+    rollingMap->addChild(localGridNode);
+
+    worldGridNode = createWorldGrid();
+    allocentricMap->addChild(worldGridNode);
 
     // generate empty trail model
-    trailNode = createTrail();
+    trailNode = new osg::Geode;
     rollingMap->addChild(trailNode);
 
+    orientationNode = new osg::Group;
+    rollingMap->addChild(orientationNode);
+
     // generate map model
     mapNode = createMap();
     rollingMap->addChild(mapNode);
@@ -105,6 +115,11 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     obstacleGroupNode = new ObstacleGroupNode;
     obstacleGroupNode->init();
     rollingMap->addChild(obstacleGroupNode);
+
+    // generate path model
+    pathNode = new osg::Geode;
+    pathNode->addDrawable(createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f)));
+    rollingMap->addChild(pathNode);
 #endif
 
     setupHUD();
@@ -132,15 +147,148 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
 void
 Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
 {
-    if (this->uas != NULL && this->uas != uas)
+    if (this->uas == uas)
+    {
+        return;
+    }
+
+    if (this->uas != NULL)
     {
         // Disconnect any previously connected active MAV
-        //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
+        disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64)));
+        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double,double,double,quint64)));
     }
 
+    connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64)));
+    connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double,double,double,quint64)));
+
+    trails.clear();
+    trailNode->removeDrawables(0, trailNode->getNumDrawables());
+    orientationNode->removeChildren(0, orientationNode->getNumChildren());
+
     this->uas = uas;
 }
 
+void
+Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time)
+{
+    if (this->uas->getUASID() != uas->getUASID())
+    {
+        return;
+    }
+
+    if (!trails.contains(component))
+    {
+        trails[component] = QVarLengthArray<osg::Vec3d, 10000>();
+        trailDrawableIdxs[component] = trails.size() - 1;
+
+        osg::Vec4 color((float)qrand() / RAND_MAX,
+                        (float)qrand() / RAND_MAX,
+                        (float)qrand() / RAND_MAX,
+                        0.5);
+        trailNode->addDrawable(createTrail(color));
+
+        double radius = 0.5;
+
+        osg::ref_ptr<osg::Group> group = new osg::Group;
+
+        // cone indicates waypoint orientation
+        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+        double coneRadius = radius / 2.0;
+        osg::ref_ptr<osg::Cone> cone =
+            new osg::Cone(osg::Vec3d(0.0, 0.0, 0.0),
+                          coneRadius, radius * 2.0);
+
+        sd->setShape(cone);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        sd->setColor(color);
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+        pat->addChild(geode);
+        pat->setAttitude(osg::Quat(- M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                                   M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+                                   0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+        group->addChild(pat);
+
+        // cylinder indicates waypoint position
+        sd = new osg::ShapeDrawable;
+        osg::ref_ptr<osg::Cylinder> cylinder =
+            new osg::Cylinder(osg::Vec3d(0.0, 0.0, 0.0),
+                              radius, 0);
+
+        sd->setShape(cylinder);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        sd->setColor(color);
+
+        geode = new osg::Geode;
+        geode->addDrawable(sd);
+        group->addChild(geode);
+
+        pat = new osg::PositionAttitudeTransform;
+        orientationNode->addChild(pat);
+        pat->addChild(group);
+    }
+
+    QVarLengthArray<osg::Vec3d, 10000>& trail = trails[component];
+
+    bool addToTrail = false;
+    if (trail.size() > 0)
+    {
+        if (fabs(x - trail[trail.size() - 1].x()) > 0.01f ||
+                fabs(y - trail[trail.size() - 1].y()) > 0.01f ||
+                fabs(z - trail[trail.size() - 1].z()) > 0.01f)
+        {
+            addToTrail = true;
+        }
+    }
+    else
+    {
+        addToTrail = true;
+    }
+
+    if (addToTrail)
+    {
+        osg::Vec3d p(x, y, z);
+        if (trail.size() == trail.capacity())
+        {
+            memcpy(trail.data(), trail.data() + 1,
+                   (trail.size() - 1) * sizeof(osg::Vec3d));
+            trail[trail.size() - 1] = p;
+        }
+        else
+        {
+            trail.append(p);
+        }
+    }
+}
+
+void
+Pixhawk3DWidget::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time)
+{
+    if (this->uas->getUASID() != uas->getUASID())
+    {
+        return;
+    }
+
+    if (!trails.contains(component))
+    {
+        return;
+    }
+
+    int idx = trailDrawableIdxs[component];
+
+    osg::PositionAttitudeTransform* pat =
+        dynamic_cast<osg::PositionAttitudeTransform*>(orientationNode->getChild(idx));
+
+    pat->setAttitude(osg::Quat(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
+                               0.0, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                               0.0, osg::Vec3d(0.0f, 1.0f, 0.0f)));
+}
+
 void
 Pixhawk3DWidget::selectFrame(QString text)
 {
@@ -159,33 +307,46 @@ Pixhawk3DWidget::selectFrame(QString text)
 }
 
 void
-Pixhawk3DWidget::showGrid(int32_t state)
+Pixhawk3DWidget::showLocalGrid(int32_t state)
+{
+    if (state == Qt::Checked)
+    {
+        displayLocalGrid = true;
+    }
+    else
+    {
+        displayLocalGrid = false;
+    }
+}
+
+void
+Pixhawk3DWidget::showWorldGrid(int32_t state)
 {
     if (state == Qt::Checked)
     {
-        displayGrid = true;
+        displayWorldGrid = true;
     }
     else
     {
-        displayGrid = false;
+        displayWorldGrid = false;
     }
 }
 
 void
-Pixhawk3DWidget::showTrail(int32_t state)
+Pixhawk3DWidget::showTrails(int32_t state)
 {
     if (state == Qt::Checked)
     {
-        if (!displayTrail)
+        if (!displayTrails)
         {
-            trail.clear();
+            trails.clear();
         }
 
-        displayTrail = true;
+        displayTrails = true;
     }
     else
     {
-        displayTrail = false;
+        displayTrails = false;
     }
 }
 
@@ -276,7 +437,7 @@ Pixhawk3DWidget::selectTargetHeading(void)
         p.set(cursorWorldCoords.first, cursorWorldCoords.second);
     }
 
-    target.z() = atan2(p.y() - target.y(), p.x() - target.x());
+    target.setW(atan2(p.y() - target.y(), p.x() - target.x()));
 }
 
 void
@@ -286,6 +447,10 @@ Pixhawk3DWidget::selectTarget(void)
     {
         return;
     }
+    if (!uas->getParamManager())
+    {
+        return;
+    }
 
     if (frame == MAV_FRAME_GLOBAL)
     {
@@ -295,7 +460,14 @@ Pixhawk3DWidget::selectTarget(void)
             getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
                                     altitude);
 
-        target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+        QVariant zTarget;
+        if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
+        {
+            zTarget = -altitude;
+        }
+
+        target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second,
+                           zTarget.toReal(), 0.0);
     }
     else if (frame == MAV_FRAME_LOCAL_NED)
     {
@@ -304,7 +476,14 @@ Pixhawk3DWidget::selectTarget(void)
         std::pair<double,double> cursorWorldCoords =
             getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
 
-        target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+        QVariant zTarget;
+        if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
+        {
+            zTarget = z;
+        }
+
+        target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second,
+                           zTarget.toReal(), 0.0);
     }
 
     enableTarget = true;
@@ -317,8 +496,8 @@ Pixhawk3DWidget::setTarget(void)
 {
     selectTargetHeading();
 
-    uas->setTargetPosition(target.x(), target.y(), 0.0,
-                           osg::RadiansToDegrees(target.z()));
+    uas->setTargetPosition(target.x(), target.y(), target.z(),
+                           osg::RadiansToDegrees(target.w()));
 }
 
 void
@@ -587,13 +766,17 @@ Pixhawk3DWidget::buildLayout(void)
     frameComboBox->addItem("Global");
     frameComboBox->setFixedWidth(70);
 
-    QCheckBox* gridCheckBox = new QCheckBox(this);
-    gridCheckBox->setText("Grid");
-    gridCheckBox->setChecked(displayGrid);
+    QCheckBox* localGridCheckBox = new QCheckBox(this);
+    localGridCheckBox->setText("Local Grid");
+    localGridCheckBox->setChecked(displayLocalGrid);
+
+    QCheckBox* worldGridCheckBox = new QCheckBox(this);
+    worldGridCheckBox->setText("World Grid");
+    worldGridCheckBox->setChecked(displayWorldGrid);
 
     QCheckBox* trailCheckBox = new QCheckBox(this);
-    trailCheckBox->setText("Trail");
-    trailCheckBox->setChecked(displayTrail);
+    trailCheckBox->setText("Trails");
+    trailCheckBox->setChecked(displayTrails);
 
     QCheckBox* waypointsCheckBox = new QCheckBox(this);
     waypointsCheckBox->setText("Waypoints");
@@ -623,17 +806,18 @@ Pixhawk3DWidget::buildLayout(void)
     layout->setMargin(0);
     layout->setSpacing(2);
     layout->addWidget(frameComboBox, 0, 10);
-    layout->addWidget(gridCheckBox, 2, 0);
-    layout->addWidget(trailCheckBox, 2, 1);
-    layout->addWidget(waypointsCheckBox, 2, 2);
-    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 3);
-    layout->addWidget(mapLabel, 2, 4);
-    layout->addWidget(mapComboBox, 2, 5);
-    layout->addWidget(modelLabel, 2, 6);
-    layout->addWidget(modelComboBox, 2, 7);
-    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 8);
-    layout->addWidget(recenterButton, 2, 9);
-    layout->addWidget(followCameraCheckBox, 2, 10);
+    layout->addWidget(localGridCheckBox, 2, 0);
+    layout->addWidget(worldGridCheckBox, 2, 1);
+    layout->addWidget(trailCheckBox, 2, 2);
+    layout->addWidget(waypointsCheckBox, 2, 3);
+    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 4);
+    layout->addWidget(mapLabel, 2, 5);
+    layout->addWidget(mapComboBox, 2, 6);
+    layout->addWidget(modelLabel, 2, 7);
+    layout->addWidget(modelComboBox, 2, 8);
+    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 9);
+    layout->addWidget(recenterButton, 2, 10);
+    layout->addWidget(followCameraCheckBox, 2, 11);
     layout->setRowStretch(0, 1);
     layout->setRowStretch(1, 100);
     layout->setRowStretch(2, 1);
@@ -641,10 +825,12 @@ Pixhawk3DWidget::buildLayout(void)
 
     connect(frameComboBox, SIGNAL(currentIndexChanged(QString)),
             this, SLOT(selectFrame(QString)));
-    connect(gridCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(showGrid(int)));
+    connect(localGridCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(showLocalGrid(int)));
+    connect(worldGridCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(showWorldGrid(int)));
     connect(trailCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(showTrail(int)));
+            this, SLOT(showTrails(int)));
     connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showWaypoints(int)));
     connect(mapComboBox, SIGNAL(currentIndexChanged(int)),
@@ -668,13 +854,16 @@ void
 Pixhawk3DWidget::display(void)
 {
     // set node visibility
-    rollingMap->setChildValue(gridNode, displayGrid);
-    rollingMap->setChildValue(trailNode, displayTrail);
+    allocentricMap->setChildValue(worldGridNode, displayWorldGrid);
+    rollingMap->setChildValue(localGridNode, displayLocalGrid);
+    rollingMap->setChildValue(trailNode, displayTrails);
+    rollingMap->setChildValue(orientationNode, displayTrails);
     rollingMap->setChildValue(mapNode, displayImagery);
     rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
     rollingMap->setChildValue(targetNode, enableTarget);
 #ifdef QGC_PROTOBUF_ENABLED
     rollingMap->setChildValue(obstacleGroupNode, displayObstacleList);
+    rollingMap->setChildValue(pathNode, displayPath);
 #endif
     rollingMap->setChildValue(rgbd3DNode, displayRGBD3D);
     hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
@@ -712,9 +901,9 @@ Pixhawk3DWidget::display(void)
                                          robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                          robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
 
-    if (displayTrail)
+    if (displayTrails)
     {
-        updateTrail(robotX, robotY, robotZ);
+        updateTrails(robotX, robotY, robotZ);
     }
 
     if (frame == MAV_FRAME_GLOBAL && displayImagery)
@@ -729,7 +918,7 @@ Pixhawk3DWidget::display(void)
 
     if (enableTarget)
     {
-        updateTarget(robotX, robotY);
+        updateTarget(robotX, robotY, robotZ);
     }
 
 #ifdef QGC_PROTOBUF_ENABLED
@@ -740,7 +929,12 @@ Pixhawk3DWidget::display(void)
 
     if (displayObstacleList)
     {
-        updateObstacles();
+        updateObstacles(robotX, robotY, robotZ);
+    }
+
+    if (displayPath)
+    {
+        updatePath(robotX, robotY, robotZ);
     }
 #endif
 
@@ -774,6 +968,10 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
         case 'O':
             displayObstacleList = !displayObstacleList;
             break;
+        case 'p':
+        case 'P':
+            displayPath = !displayPath;
+            break;
         }
     }
 
@@ -816,6 +1014,20 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
     Q3DWidget::mousePressEvent(event);
 }
 
+void
+Pixhawk3DWidget::showEvent(QShowEvent* event)
+{
+    Q3DWidget::showEvent(event);
+    emit visibilityChanged(true);
+}
+
+void
+Pixhawk3DWidget::hideEvent(QHideEvent* event)
+{
+    Q3DWidget::hideEvent(event);
+    emit visibilityChanged(false);
+}
+
 void
 Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event)
 {
@@ -908,7 +1120,7 @@ Pixhawk3DWidget::getPosition(double& x, double& y, double& z)
 }
 
 osg::ref_ptr<osg::Geode>
-Pixhawk3DWidget::createGrid(void)
+Pixhawk3DWidget::createLocalGrid(void)
 {
     osg::ref_ptr<osg::Geode> geode(new osg::Geode());
     osg::ref_ptr<osg::Geometry> fineGeometry(new osg::Geometry());
@@ -916,16 +1128,16 @@ Pixhawk3DWidget::createGrid(void)
     geode->addDrawable(fineGeometry);
     geode->addDrawable(coarseGeometry);
 
-    float radius = 10.0f;
+    float radius = 5.0f;
     float resolution = 0.25f;
 
     osg::ref_ptr<osg::Vec3Array> fineCoords(new osg::Vec3Array);
     osg::ref_ptr<osg::Vec3Array> coarseCoords(new osg::Vec3Array);
 
-    // draw a 20m x 20m grid with 0.25m resolution
+    // draw a 10m x 10m grid with 0.25m resolution
     for (float i = -radius; i <= radius; i += resolution)
     {
-        if (fabs(i - floor(i + 0.5f)) < 0.01f)
+        if (fabs(i / 1.0f - floor(i / 1.0f)) < 0.01f)
         {
             coarseCoords->push_back(osg::Vec3(i, -radius, 0.0f));
             coarseCoords->push_back(osg::Vec3(i, radius, 0.0f));
@@ -961,45 +1173,153 @@ Pixhawk3DWidget::createGrid(void)
     fineLinewidth->setWidth(0.25f);
     fineStateset->setAttributeAndModes(fineLinewidth, osg::StateAttribute::ON);
     fineStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    fineStateset->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON);
+    fineStateset->setMode(GL_BLEND, osg::StateAttribute::ON);
     fineGeometry->setStateSet(fineStateset);
 
     osg::ref_ptr<osg::StateSet> coarseStateset(new osg::StateSet);
     osg::ref_ptr<osg::LineWidth> coarseLinewidth(new osg::LineWidth());
-    coarseLinewidth->setWidth(2.0f);
+    coarseLinewidth->setWidth(1.0f);
     coarseStateset->setAttributeAndModes(coarseLinewidth, osg::StateAttribute::ON);
     coarseStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    coarseStateset->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON);
+    coarseStateset->setMode(GL_BLEND, osg::StateAttribute::ON);
     coarseGeometry->setStateSet(coarseStateset);
 
     return geode;
 }
 
 osg::ref_ptr<osg::Geode>
-Pixhawk3DWidget::createTrail(void)
+Pixhawk3DWidget::createWorldGrid(void)
 {
     osg::ref_ptr<osg::Geode> geode(new osg::Geode());
-    trailGeometry = new osg::Geometry();
-    trailGeometry->setUseDisplayList(false);
-    geode->addDrawable(trailGeometry.get());
+    osg::ref_ptr<osg::Geometry> fineGeometry(new osg::Geometry());
+    osg::ref_ptr<osg::Geometry> coarseGeometry(new osg::Geometry());
+    osg::ref_ptr<osg::Geometry> axisGeometry(new osg::Geometry());
+    geode->addDrawable(fineGeometry);
+    geode->addDrawable(coarseGeometry);
+    geode->addDrawable(axisGeometry.get());
+
+    float radius = 20.0f;
+    float resolution = 1.0f;
 
-    trailVertices = new osg::Vec3dArray;
-    trailGeometry->setVertexArray(trailVertices);
+    osg::ref_ptr<osg::Vec3Array> fineCoords(new osg::Vec3Array);
+    osg::ref_ptr<osg::Vec3Array> coarseCoords(new osg::Vec3Array);
+
+    // draw a 40m x 40m grid with 1.0m resolution
+    for (float i = -radius; i <= radius; i += resolution)
+    {
+        if (fabs(i / 5.0f - floor(i / 5.0f)) < 0.01f)
+        {
+            coarseCoords->push_back(osg::Vec3(i, -radius, 0.0f));
+            coarseCoords->push_back(osg::Vec3(i, radius, 0.0f));
+            coarseCoords->push_back(osg::Vec3(-radius, i, 0.0f));
+            coarseCoords->push_back(osg::Vec3(radius, i, 0.0f));
+        }
+        else
+        {
+            fineCoords->push_back(osg::Vec3(i, -radius, 0.0f));
+            fineCoords->push_back(osg::Vec3(i, radius, 0.0f));
+            fineCoords->push_back(osg::Vec3(-radius, i, 0.0f));
+            fineCoords->push_back(osg::Vec3(radius, i, 0.0f));
+        }
+    }
 
-    trailDrawArrays = new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP);
-    trailGeometry->addPrimitiveSet(trailDrawArrays);
+    fineGeometry->setVertexArray(fineCoords);
+    coarseGeometry->setVertexArray(coarseCoords);
 
     osg::ref_ptr<osg::Vec4Array> color(new osg::Vec4Array);
-    color->push_back(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f));
-    trailGeometry->setColorArray(color);
-    trailGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+    color->push_back(osg::Vec4(0.5f, 0.5f, 0.5f, 1.0f));
+    fineGeometry->setColorArray(color);
+    coarseGeometry->setColorArray(color);
+    fineGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+    coarseGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+    fineGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
+                                  0, fineCoords->size()));
+    coarseGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0,
+                                    coarseCoords->size()));
+
+    osg::ref_ptr<osg::StateSet> fineStateset(new osg::StateSet);
+    osg::ref_ptr<osg::LineWidth> fineLinewidth(new osg::LineWidth());
+    fineLinewidth->setWidth(0.1f);
+    fineStateset->setAttributeAndModes(fineLinewidth, osg::StateAttribute::ON);
+    fineStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    fineStateset->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON);
+    fineStateset->setMode(GL_BLEND, osg::StateAttribute::ON);
+    fineGeometry->setStateSet(fineStateset);
+
+    osg::ref_ptr<osg::StateSet> coarseStateset(new osg::StateSet);
+    osg::ref_ptr<osg::LineWidth> coarseLinewidth(new osg::LineWidth());
+    coarseLinewidth->setWidth(2.0f);
+    coarseStateset->setAttributeAndModes(coarseLinewidth, osg::StateAttribute::ON);
+    coarseStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    coarseStateset->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON);
+    coarseStateset->setMode(GL_BLEND, osg::StateAttribute::ON);
+    coarseGeometry->setStateSet(coarseStateset);
+
+    // add axes
+    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.0f, 1.0f, 0.0f);
+    (*coords)[3] = osg::Vec3(1.0f, 0.0f, 0.0f);
+    (*coords)[5] = osg::Vec3(0.0f, 0.0f, -1.0f);
+
+    axisGeometry->setVertexArray(coords);
+
+    osg::Vec4 redColor(1.0f, 0.0f, 0.0f, 0.0f);
+    osg::Vec4 greenColor(0.0f, 1.0f, 0.0f, 0.0f);
+    osg::Vec4 blueColor(0.0f, 0.0f, 1.0f, 0.0f);
+
+    osg::ref_ptr<osg::Vec4Array> axisColors(new osg::Vec4Array(6));
+    (*axisColors)[0] = redColor;
+    (*axisColors)[1] = redColor;
+    (*axisColors)[2] = greenColor;
+    (*axisColors)[3] = greenColor;
+    (*axisColors)[4] = blueColor;
+    (*axisColors)[5] = blueColor;
+
+    axisGeometry->setColorArray(axisColors);
+    axisGeometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
+
+    axisGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 6));
+
+    osg::ref_ptr<osg::StateSet> axisStateset(new osg::StateSet);
+    osg::ref_ptr<osg::LineWidth> axisLinewidth(new osg::LineWidth());
+    axisLinewidth->setWidth(4.0f);
+    axisStateset->setAttributeAndModes(axisLinewidth, osg::StateAttribute::ON);
+    axisStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    axisGeometry->setStateSet(axisStateset);
+
+    return geode;
+}
+
+osg::ref_ptr<osg::Geometry>
+Pixhawk3DWidget::createTrail(const osg::Vec4& color)
+{
+    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
+    geometry->setUseDisplayList(false);
+
+    osg::ref_ptr<osg::Vec3dArray> vertices(new osg::Vec3dArray());
+    geometry->setVertexArray(vertices);
+
+    osg::ref_ptr<osg::DrawArrays> drawArrays(new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP));
+    geometry->addPrimitiveSet(drawArrays);
+
+    osg::ref_ptr<osg::Vec4Array> colorArray(new osg::Vec4Array);
+    colorArray->push_back(color);
+    geometry->setColorArray(colorArray);
+    geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
 
     osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
     osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
     linewidth->setWidth(1.0f);
     stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
     stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
-    trailGeometry->setStateSet(stateset);
+    geometry->setStateSet(stateset);
 
-    return geode;
+    return geometry;
 }
 
 osg::ref_ptr<Imagery>
@@ -1190,54 +1510,39 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
 }
 
 void
-Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
 {
-    if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f)
-    {
-        return;
-    }
+    QMapIterator<int,int> it(trailDrawableIdxs);
 
-    bool addToTrail = false;
-    if (trail.size() > 0)
-    {
-        if (fabs(robotX - trail[trail.size() - 1].x()) > 0.01f ||
-                fabs(robotY - trail[trail.size() - 1].y()) > 0.01f ||
-                fabs(robotZ - trail[trail.size() - 1].z()) > 0.01f)
-        {
-            addToTrail = true;
-        }
-    }
-    else
+    while (it.hasNext())
     {
-        addToTrail = true;
-    }
+        it.next();
 
-    if (addToTrail)
-    {
-        osg::Vec3d p(robotX, robotY, robotZ);
-        if (trail.size() == trail.capacity())
-        {
-            memcpy(trail.data(), trail.data() + 1,
-                   (trail.size() - 1) * sizeof(osg::Vec3d));
-            trail[trail.size() - 1] = p;
-        }
-        else
+        osg::Geometry* geometry = trailNode->getDrawable(it.value())->asGeometry();
+        osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
+
+        osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
+
+        const QVarLengthArray<osg::Vec3d, 10000>& trail = trails.value(it.key());
+
+        for (int i = 0; i < trail.size(); ++i)
         {
-            trail.append(p);
+            vertices->push_back(osg::Vec3d(trail[i].y() - robotY,
+                                           trail[i].x() - robotX,
+                                           -(trail[i].z() - robotZ)));
         }
-    }
 
-    trailVertices->clear();
-    for (int i = 0; i < trail.size(); ++i)
-    {
-        trailVertices->push_back(osg::Vec3d(trail[i].y() - robotY,
-                                            trail[i].x() - robotX,
-                                            -(trail[i].z() - robotZ)));
-    }
+        geometry->setVertexArray(vertices);
+        drawArrays->setFirst(0);
+        drawArrays->setCount(vertices->size());
+        geometry->dirtyBound();
 
-    trailDrawArrays->setFirst(0);
-    trailDrawArrays->setCount(trailVertices->size());
-    trailGeometry->dirtyBound();
+        osg::PositionAttitudeTransform* pat =
+            dynamic_cast<osg::PositionAttitudeTransform*>(orientationNode->getChild(it.value()));
+        pat->setPosition(osg::Vec3(trail[trail.size() - 1].y() - robotY,
+                                   trail[trail.size() - 1].x() - robotX,
+                                   -(trail[trail.size() - 1].z() - robotZ)));
+    }
 }
 
 void
@@ -1324,162 +1629,34 @@ Pixhawk3DWidget::updateWaypoints(void)
 }
 
 void
-Pixhawk3DWidget::updateTarget(double robotX, double robotY)
+Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ)
 {
     osg::PositionAttitudeTransform* pat =
         dynamic_cast<osg::PositionAttitudeTransform*>(targetNode.get());
 
-    pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
-    pat->setAttitude(osg::Quat(target.z() - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+    pat->setPosition(osg::Vec3d(target.y() - robotY,
+                                target.x() - robotX,
+                                -(target.z() - robotZ)));
+    pat->setAttitude(osg::Quat(target.w() - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
                                0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
 
     osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
     osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));
 
-
     sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
 }
 
-float colormap_jet[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}
-};
-
 #ifdef QGC_PROTOBUF_ENABLED
 void
 Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 {
-    px::RGBDImage rgbdImage = uas->getRGBDImage();
-    px::PointCloudXYZRGB pointCloud = uas->getPointCloud();
+    qreal receivedRGBDImageTimestamp, receivedPointCloudTimestamp;
+    px::RGBDImage rgbdImage = uas->getRGBDImage(receivedRGBDImageTimestamp);
+    px::PointCloudXYZRGB pointCloud = uas->getPointCloud(receivedPointCloudTimestamp);
 
-    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0)
+    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0 &&
+        QGC::groundTimeSeconds() - receivedRGBDImageTimestamp < kMessageTimeout)
     {
         rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
                            GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
@@ -1499,9 +1676,11 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
                     int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f;
                     idx = 127 - idx;
 
-                    pixel[0] = colormap_jet[idx][2] * 255.0f;
-                    pixel[1] = colormap_jet[idx][1] * 255.0f;
-                    pixel[2] = colormap_jet[idx][0] * 255.0f;
+                    float r, g, b;
+                    qgc::colormap("jet", idx, r, g, b);
+                    pixel[0] = r * 255.0f;
+                    pixel[1] = g * 255.0f;
+                    pixel[2] = b * 255.0f;
                 }
 
                 pixel += 3;
@@ -1516,9 +1695,15 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
     }
 
     osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();
-
     osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
     osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
+
+    if (QGC::groundTimeSeconds() - receivedPointCloudTimestamp > kMessageTimeout)
+    {
+        geometry->removePrimitiveSet(0, geometry->getNumPrimitiveSets());
+        return;
+    }
+
     for (int i = 0; i < pointCloud.points_size(); ++i)
     {
         const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
@@ -1544,10 +1729,11 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
         {
             double dist = sqrt(x * x + y * y + z * z);
             int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
-            (*colors)[i].set(colormap_jet[colorIndex][0],
-                             colormap_jet[colorIndex][1],
-                             colormap_jet[colorIndex][2],
-                             1.0f);
+
+            float r, g, b;
+            qgc::colormap("jet", colorIndex, r, g, b);
+
+            (*colors)[i].set(r, g, b, 1.0f);
         }
     }
 
@@ -1564,9 +1750,100 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 }
 
 void
-Pixhawk3DWidget::updateObstacles(void)
+Pixhawk3DWidget::updateObstacles(double robotX, double robotY, double robotZ)
 {
-    obstacleGroupNode->update(frame, uas);
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        obstacleGroupNode->clear();
+        return;
+    }
+
+    qreal receivedTimestamp;
+    px::ObstacleList obstacleList = uas->getObstacleList(receivedTimestamp);
+
+    if (QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
+    {
+        obstacleGroupNode->update(robotX, robotY, robotZ, obstacleList);
+    }
+    else
+    {
+        obstacleGroupNode->clear();
+    }
+}
+
+void
+Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
+{
+    qreal receivedTimestamp;
+    px::Path path = uas->getPath(receivedTimestamp);
+
+    osg::Geometry* geometry = pathNode->getDrawable(0)->asGeometry();
+    osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
+    osg::Vec4Array* colorArray = reinterpret_cast<osg::Vec4Array*>(geometry->getColorArray());
+
+    geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
+    osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
+    linewidth->setWidth(2.0f);
+    geometry->getStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
+
+    colorArray->clear();
+
+    osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
+
+    if (QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
+    {
+        // find path length
+        float length = 0.0f;
+        for (int i = 0; i < path.waypoints_size() - 1; ++i)
+        {
+            const px::Waypoint& wp0 = path.waypoints(i);
+            const px::Waypoint& wp1 = path.waypoints(i+1);
+
+            length += qgc::hypot3f(wp0.x() - wp1.x(),
+                                   wp0.y() - wp1.y(),
+                                   wp0.z() - wp1.z());
+        }
+
+        // build path
+        if (path.waypoints_size() > 0)
+        {
+            const px::Waypoint& wp0 = path.waypoints(0);
+
+            vertices->push_back(osg::Vec3d(wp0.y() - robotY,
+                                           wp0.x() - robotX,
+                                           -(wp0.z() - robotZ)));
+
+            float r, g, b;
+            qgc::colormap("autumn", 0, r, g, b);
+            colorArray->push_back(osg::Vec4d(r, g, b, 1.0f));
+        }
+
+        float lengthCurrent = 0.0f;
+        for (int i = 0; i < path.waypoints_size() - 1; ++i)
+        {
+            const px::Waypoint& wp0 = path.waypoints(i);
+            const px::Waypoint& wp1 = path.waypoints(i+1);
+
+            lengthCurrent += qgc::hypot3f(wp0.x() - wp1.x(),
+                                          wp0.y() - wp1.y(),
+                                          wp0.z() - wp1.z());
+
+            vertices->push_back(osg::Vec3d(wp1.y() - robotY,
+                                           wp1.x() - robotX,
+                                           -(wp1.z() - robotZ)));
+
+            int colorIdx = lengthCurrent / length * 127.0f;
+
+            float r, g, b;
+            qgc::colormap("autumn", colorIdx, r, g, b);
+            colorArray->push_back(osg::Vec4f(r, g, b, 1.0f));
+        }
+    }
+
+    geometry->setVertexArray(vertices);
+    drawArrays->setFirst(0);
+    drawArrays->setCount(vertices->size());
+    geometry->dirtyBound();
 }
 
 #endif

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