Major overhaul of local 3D view: now supports multi-MAV visualization.

qgroundcontrol.pro

@@ -362,7 +362,13 @@ HEADERS += src/MG.h \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h \
     src/ui/QGCPluginHost.h \
     src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h \
-    src/ui/map3D/gpl.h
+    src/ui/map3D/gpl.h \
+    src/ui/map3D/CameraParams.h \
+    src/ui/map3D/ViewParamWidget.h \
+    src/ui/map3D/SystemContainer.h \
+    src/ui/map3D/SystemViewParams.h \
+    src/ui/map3D/GlobalViewParams.h \
+    src/ui/map3D/SystemGroupNode.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
@@ -496,7 +502,13 @@ SOURCES += src/main.cc \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc \
     src/ui/QGCPluginHost.cc \
     src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc \
-    src/ui/map3D/gpl.cc
+    src/ui/map3D/gpl.cc \
+    src/ui/map3D/CameraParams.cc \
+    src/ui/map3D/ViewParamWidget.cc \
+    src/ui/map3D/SystemContainer.cc \
+    src/ui/map3D/SystemViewParams.cc \
+    src/ui/map3D/GlobalViewParams.cc \
+    src/ui/map3D/SystemGroupNode.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

src/ui/MainWindow.cc

@@ -553,7 +553,7 @@ void MainWindow::buildCommonWidgets()
 
 #ifdef QGC_OSG_ENABLED
     if (!_3DWidget) {
-        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
+        _3DWidget         = Q3DWidgetFactory::get("PIXHAWK", this);
         addCentralWidget(_3DWidget, tr("Local 3D"));
     }
 #endif

src/ui/map3D/CameraParams.cc

+#include "CameraParams.h"
+
+CameraParams::CameraParams()
+ : mMinZoomRange(2.0f)
+ , mFov(30.0f)
+ , mMinClipRange(1.0f)
+ , mMaxClipRange(10000.0f)
+{
+
+}
+
+float&
+CameraParams::minZoomRange(void)
+{
+    return mMinZoomRange;
+}
+
+float
+CameraParams::minZoomRange(void) const
+{
+    return mMinZoomRange;
+}
+
+float&
+CameraParams::fov(void)
+{
+    return mFov;
+}
+
+float
+CameraParams::fov(void) const
+{
+    return mFov;
+}
+
+float&
+CameraParams::minClipRange(void)
+{
+    return mMinClipRange;
+}
+
+float
+CameraParams::minClipRange(void) const
+{
+    return mMinClipRange;
+}
+
+float&
+CameraParams::maxClipRange(void)
+{
+    return mMaxClipRange;
+}
+
+float
+CameraParams::maxClipRange(void) const
+{
+    return mMaxClipRange;
+}

src/ui/map3D/CameraParams.h

+#ifndef CAMERAPARAMS_H
+#define CAMERAPARAMS_H
+
+class CameraParams
+{
+public:
+    CameraParams();
+
+    float& minZoomRange(void);
+    float minZoomRange(void) const;
+
+    float& fov(void);
+    float fov(void) const;
+
+    float& minClipRange(void);
+    float minClipRange(void) const;
+
+    float& maxClipRange(void);
+    float maxClipRange(void) const;
+
+private:
+    float mMinZoomRange;
+    float mFov;
+    float mMinClipRange;
+    float mMaxClipRange;
+};
+
+#endif // CAMERAPARAMS_H

src/ui/map3D/GlobalViewParams.cc

+#include "GlobalViewParams.h"
+
+#include <QStringList>
+
+GlobalViewParams::GlobalViewParams()
+ : mDisplayWorldGrid(true)
+ , mImageryType(Imagery::BLANK_MAP)
+ , mFollowCameraId(-1)
+ , mFrame(MAV_FRAME_LOCAL_NED)
+{
+
+}
+
+bool&
+GlobalViewParams::displayWorldGrid(void)
+{
+    return mDisplayWorldGrid;
+}
+
+bool
+GlobalViewParams::displayWorldGrid(void) const
+{
+    return mDisplayWorldGrid;
+}
+
+Imagery::Type&
+GlobalViewParams::imageryType(void)
+{
+    return mImageryType;
+}
+
+Imagery::Type
+GlobalViewParams::imageryType(void) const
+{
+    return mImageryType;
+}
+
+int&
+GlobalViewParams::followCameraId(void)
+{
+    return mFollowCameraId;
+}
+
+int
+GlobalViewParams::followCameraId(void) const
+{
+    return mFollowCameraId;
+}
+
+MAV_FRAME&
+GlobalViewParams::frame(void)
+{
+    return mFrame;
+}
+
+MAV_FRAME
+GlobalViewParams::frame(void) const
+{
+    return mFrame;
+}
+
+void
+GlobalViewParams::followCameraChanged(const QString& text)
+{
+    int followCameraId = -1;
+
+    if (text.compare("None") == 0)
+    {
+        followCameraId = -1;
+    }
+    else
+    {
+        QStringList list = text.split(" ", QString::SkipEmptyParts);
+
+        followCameraId = list.back().toInt();
+    }
+
+    if (followCameraId != mFollowCameraId)
+    {
+        mFollowCameraId = followCameraId;
+        emit followCameraChanged(mFollowCameraId);
+    }
+}
+
+void
+GlobalViewParams::frameChanged(const QString& text)
+{
+    if (text.compare("Global") == 0)
+    {
+       mFrame = MAV_FRAME_GLOBAL;
+    }
+    else if (text.compare("Local") == 0)
+    {
+       mFrame = MAV_FRAME_LOCAL_NED;
+    }
+}
+
+void
+GlobalViewParams::imageryTypeChanged(int index)
+{
+    mImageryType = static_cast<Imagery::Type>(index);
+}
+
+void
+GlobalViewParams::toggleWorldGrid(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayWorldGrid = true;
+    }
+    else
+    {
+        mDisplayWorldGrid = false;
+    }
+}

src/ui/map3D/GlobalViewParams.h

+#ifndef GLOBALVIEWPARAMS_H
+#define GLOBALVIEWPARAMS_H
+
+#include <QObject>
+#include <QString>
+#include <QVector>
+
+#include "QGCMAVLink.h"
+#include "Imagery.h"
+
+class GlobalViewParams : public QObject
+{
+    Q_OBJECT
+
+public:
+    GlobalViewParams();
+
+    bool& displayWorldGrid(void);
+    bool displayWorldGrid(void) const;
+
+    Imagery::Type& imageryType(void);
+    Imagery::Type imageryType(void) const;
+
+    int& followCameraId(void);
+    int followCameraId(void) const;
+
+    MAV_FRAME& frame(void);
+    MAV_FRAME frame(void) const;
+
+public slots:
+    void followCameraChanged(const QString& text);
+    void frameChanged(const QString &text);
+    void imageryTypeChanged(int index);
+    void toggleWorldGrid(int state);
+
+signals:
+    void followCameraChanged(int systemId);
+
+private:
+    bool mDisplayWorldGrid;
+    Imagery::Type mImageryType;
+    int mFollowCameraId;
+    MAV_FRAME mFrame;
+};
+
+typedef QSharedPointer<GlobalViewParams> GlobalViewParamsPtr;
+
+#endif // GLOBALVIEWPARAMS_H

src/ui/map3D/ImageWindowGeode.cc

@@ -32,19 +32,19 @@
 #include "ImageWindowGeode.h"
 
 ImageWindowGeode::ImageWindowGeode()
-    : border(5)
+ : mBorder(5)
+ , mImage(new osg::Image)
 {
 
 }
 
 void
 ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
-                       osg::ref_ptr<osg::Image>& image,
                        osg::ref_ptr<osgText::Font>& font)
 {
     // image
     osg::ref_ptr<osg::Geometry> imageGeometry = new osg::Geometry;
-    imageVertices = new osg::Vec3Array(4);
+    mImageVertices = new osg::Vec3Array(4);
 
     osg::ref_ptr<osg::Vec2Array> textureCoords = new osg::Vec2Array;
     textureCoords->push_back(osg::Vec2(0.0f, 1.0f));
@@ -57,15 +57,15 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
     imageGeometry->setColorArray(imageColors);
     imageGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
 
-    imageGeometry->setVertexArray(imageVertices);
+    imageGeometry->setVertexArray(mImageVertices);
     imageGeometry->setTexCoordArray(0, textureCoords);
 
     imageGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
-                                   0, imageVertices->size()));
+                                   0, mImageVertices->size()));
 
     osg::ref_ptr<osg::Texture2D> texture = new osg::Texture2D;
     texture->setDataVariance(osg::Object::DYNAMIC);
-    texture->setImage(image);
+    texture->setImage(mImage);
     texture->setResizeNonPowerOfTwoHint(false);
 
     imageGeometry->getOrCreateStateSet()->
@@ -74,10 +74,10 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
 
     // background
     osg::ref_ptr<osg::Geometry> backgroundGeometry = new osg::Geometry;
-    backgroundVertices = new osg::Vec3Array(4);
-    backgroundGeometry->setVertexArray(backgroundVertices);
+    mBackgroundVertices = new osg::Vec3Array(4);
+    backgroundGeometry->setVertexArray(mBackgroundVertices);
     backgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
-                                        0, backgroundVertices->size()));
+                                        0, mBackgroundVertices->size()));
     osg::ref_ptr<osg::Vec4Array> backgroundColors(new osg::Vec4Array);
     backgroundColors->push_back(backgroundColor);
     backgroundGeometry->setColorArray(backgroundColors);
@@ -85,16 +85,16 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
     backgroundGeometry->setUseDisplayList(false);
 
     // caption
-    text = new osgText::Text;
-    text->setText(caption.toStdString().c_str());
-    text->setCharacterSize(11);
-    text->setFont(font);
-    text->setAxisAlignment(osgText::Text::SCREEN);
-    text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+    mText = new osgText::Text;
+    mText->setText(caption.toStdString().c_str());
+    mText->setCharacterSize(11);
+    mText->setFont(font);
+    mText->setAxisAlignment(osgText::Text::SCREEN);
+    mText->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
 
     addDrawable(imageGeometry);
     addDrawable(backgroundGeometry);
-    addDrawable(text);
+    addDrawable(mText);
 
     setAttributes(0, 0, 0, 0);
 }
@@ -102,20 +102,26 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
 void
 ImageWindowGeode::setAttributes(int x, int y, int width, int height)
 {
-    int imageWidth = width - border * 2;
-    int imageHeight = height - border * 2 - 15;
-    int imageXPosition = x + border;
-    int imageYPosition = y + border;
-
-    imageVertices->at(0) = osg::Vec3(imageXPosition, imageYPosition, 0);
-    imageVertices->at(1) = osg::Vec3(imageXPosition + imageWidth, imageYPosition, 0);
-    imageVertices->at(2) = osg::Vec3(imageXPosition + imageWidth, imageYPosition + imageHeight, 0);
-    imageVertices->at(3) = osg::Vec3(imageXPosition, imageYPosition + imageHeight, 0);
-
-    text->setPosition(osg::Vec3(imageXPosition, imageYPosition + imageHeight + 5, 0));
-
-    backgroundVertices->at(0) = osg::Vec3(x, y, -1);
-    backgroundVertices->at(1) = osg::Vec3(x + width, y, -1);
-    backgroundVertices->at(2) = osg::Vec3(x + width, y + height, -1);
-    backgroundVertices->at(3) = osg::Vec3(x, y + height, -1);
+    int imageWidth = width - mBorder * 2;
+    int imageHeight = height - mBorder * 2 - 15;
+    int imageXPosition = x + mBorder;
+    int imageYPosition = y + mBorder;
+
+    mImageVertices->at(0) = osg::Vec3(imageXPosition, imageYPosition, 0);
+    mImageVertices->at(1) = osg::Vec3(imageXPosition + imageWidth, imageYPosition, 0);
+    mImageVertices->at(2) = osg::Vec3(imageXPosition + imageWidth, imageYPosition + imageHeight, 0);
+    mImageVertices->at(3) = osg::Vec3(imageXPosition, imageYPosition + imageHeight, 0);
+
+    mText->setPosition(osg::Vec3(imageXPosition, imageYPosition + imageHeight + 5, 0));
+
+    mBackgroundVertices->at(0) = osg::Vec3(x, y, -1);
+    mBackgroundVertices->at(1) = osg::Vec3(x + width, y, -1);
+    mBackgroundVertices->at(2) = osg::Vec3(x + width, y + height, -1);
+    mBackgroundVertices->at(3) = osg::Vec3(x, y + height, -1);
+}
+
+osg::ref_ptr<osg::Image>&
+ImageWindowGeode::image(void)
+{
+    return mImage;
 }

src/ui/map3D/ImageWindowGeode.h

@@ -42,17 +42,18 @@ class ImageWindowGeode : public osg::Geode
 public:
     ImageWindowGeode();
     void init(const QString& caption, const osg::Vec4& backgroundColor,
-              osg::ref_ptr<osg::Image>& image,
               osg::ref_ptr<osgText::Font>& font);
 
     void setAttributes(int x, int y, int width, int height);
+    osg::ref_ptr<osg::Image>& image(void);
 
 private:
-    int border;
+    int mBorder;
 
-    osg::ref_ptr<osg::Vec3Array> imageVertices;
-    osg::ref_ptr<osg::Vec3Array> backgroundVertices;
-    osg::ref_ptr<osgText::Text> text;
+    osg::ref_ptr<osg::Image> mImage;
+    osg::ref_ptr<osg::Vec3Array> mImageVertices;
+    osg::ref_ptr<osg::Vec3Array> mBackgroundVertices;
+    osg::ref_ptr<osgText::Text> mText;
 };
 
 #endif // IMAGEWINDOWGEODE_H

src/ui/map3D/Imagery.cc

@@ -46,14 +46,14 @@ Imagery::Imagery()
 
 }
 
-Imagery::ImageryType
+Imagery::Type
 Imagery::getImageryType(void) const
 {
     return currentImageryType;
 }
 
 void
-Imagery::setImageryType(ImageryType type)
+Imagery::setImageryType(Imagery::Type type)
 {
     currentImageryType = type;
 }

src/ui/map3D/Imagery.h

@@ -41,7 +41,7 @@ This file is part of the QGROUNDCONTROL project
 class Imagery : public osg::Geode
 {
 public:
-    enum ImageryType {
+    enum Type {
         BLANK_MAP = 0,
         GOOGLE_MAP = 1,
         GOOGLE_SATELLITE = 2,
@@ -50,8 +50,8 @@ public:
 
     Imagery();
 
-    ImageryType getImageryType(void) const;
-    void setImageryType(ImageryType type);
+    Type getImageryType(void) const;
+    void setImageryType(Type type);
     void setOffset(double xOffset, double yOffset);
 
     void prefetch2D(double windowWidth, double windowHeight,
@@ -104,7 +104,7 @@ private:
 
     QScopedPointer<TextureCache> textureCache;
 
-    ImageryType currentImageryType;
+    Type currentImageryType;
 
     double xOffset;
     double yOffset;

src/ui/map3D/Pixhawk3DWidget.cc

@@ -39,6 +39,7 @@
 #include <osg/LineWidth>
 #include <osg/ShapeDrawable>
 
+#include "../MainWindow.h"
 #include "PixhawkCheetahGeode.h"
 #include "UASManager.h"
 
@@ -51,189 +52,122 @@
 #endif
 
 Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
-    : Q3DWidget(parent)
-    , uas(NULL)
-    , kMessageTimeout(4.0)
-    , mode(DEFAULT_MODE)
-    , selectedWpIndex(-1)
-    , displayLocalGrid(false)
-    , displayWorldGrid(true)
-    , displayTrails(true)
-    , displayImagery(true)
-    , displayWaypoints(true)
-    , displayRGBD2D(false)
-    , displayRGBD3D(true)
-    , displayObstacleList(true)
-    , displayPath(true)
-    , enableRGBDColor(false)
-    , enableTarget(false)
-    , followCamera(true)
-    , frame(MAV_FRAME_LOCAL_NED)
-    , lastRobotX(0.0f)
-    , lastRobotY(0.0f)
-    , lastRobotZ(0.0f)
+ : kMessageTimeout(4.0)
+ , mMode(DEFAULT_MODE)
+ , mSelectedWpIndex(-1)
+ , mActiveSystemId(-1)
+ , mActiveUAS(NULL)
+ , mGlobalViewParams(new GlobalViewParams)
+ , mFollowCameraId(-1)
+ , mInitCameraPos(false)
+ , m3DWidget(new Q3DWidget(this))
+ , mViewParamWidget(new ViewParamWidget(mGlobalViewParams, mSystemViewParamMap, this, parent))
 {
-    setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f);
-    init(15.0f);
+    connect(m3DWidget, SIGNAL(sizeChanged(int,int)), this, SLOT(sizeChanged(int,int)));
+    connect(m3DWidget, SIGNAL(update()), this, SLOT(update()));
 
-    // generate Pixhawk Cheetah model
-    vehicleModel = PixhawkCheetahGeode::instance();
-    egocentricMap->addChild(vehicleModel);
+    m3DWidget->setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f);
+    m3DWidget->init(15.0f);
+    m3DWidget->handleDeviceEvents() = false;
 
-    // generate grid models
-    localGridNode = createLocalGrid();
-    rollingMap->addChild(localGridNode);
-
-    worldGridNode = createWorldGrid();
-    allocentricMap->addChild(worldGridNode);
-
-    // generate empty trail model
-    trailNode = new osg::Geode;
-    rollingMap->addChild(trailNode);
-
-    orientationNode = new osg::Group;
-    rollingMap->addChild(orientationNode);
+    mWorldGridNode = createWorldGrid();
+    m3DWidget->worldMap()->addChild(mWorldGridNode, false);
 
     // generate map model
-    mapNode = createMap();
-    rollingMap->addChild(mapNode);
+    mImageryNode = createImagery();
+    m3DWidget->worldMap()->addChild(mImageryNode, false);
 
-    // generate waypoint model
-    waypointGroupNode = new WaypointGroupNode;
-    waypointGroupNode->init();
-    rollingMap->addChild(waypointGroupNode);
-
-    // generate target model
-    targetNode = createTarget();
-    rollingMap->addChild(targetNode);
-
-    // generate RGBD model
-    rgbd3DNode = createRGBD3D();
-    rollingMap->addChild(rgbd3DNode);
+    setupHUD();
 
-#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    obstacleGroupNode = new ObstacleGroupNode;
-    obstacleGroupNode->init();
-    rollingMap->addChild(obstacleGroupNode);
+    buildLayout();
 
-    // generate path model
-    pathNode = new osg::Geode;
-    pathNode->addDrawable(createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f)));
-    rollingMap->addChild(pathNode);
-#endif
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
+            this, SLOT(activeSystemChanged(UASInterface*)));
+    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)),
+            this, SLOT(systemCreated(UASInterface*)));
+    connect(mGlobalViewParams.data(), SIGNAL(followCameraChanged(int)),
+            this, SLOT(followCameraChanged(int)));
 
-    setupHUD();
+    MainWindow* parentWindow = qobject_cast<MainWindow*>(parent);
+    parentWindow->addDockWidget(Qt::LeftDockWidgetArea, mViewParamWidget);
 
-    // find available vehicle models in models folder
-    vehicleModels = findVehicleModels();
+    mViewParamWidget->hide();
 
-    buildLayout();
+    setFocusPolicy(Qt::StrongFocus);
+    setMouseTracking(true);
+}
 
-    updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "32N");
+Pixhawk3DWidget::~Pixhawk3DWidget()
+{
 
-    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
-            this, SLOT(setActiveUAS(UASInterface*)));
 }
 
-Pixhawk3DWidget::~Pixhawk3DWidget()
+void
+Pixhawk3DWidget::activeSystemChanged(UASInterface* uas)
 {
+    mActiveSystemId = uas->getUASID();
+
+    mActiveUAS = uas;
 
+    mMode = DEFAULT_MODE;
 }
 
-/**
- *
- * @param uas the UAS/MAV to monitor/display with the HUD
- */
 void
-Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
+Pixhawk3DWidget::systemCreated(UASInterface *uas)
 {
-    if (this->uas == uas)
+    int systemId = uas->getUASID();
+
+    if (mSystemContainerMap.contains(systemId))
     {
         return;
     }
 
-    if (this->uas != NULL)
-    {
-        // Disconnect any previously connected active MAV
-        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)));
-    }
+    mSystemViewParamMap.insert(systemId, SystemViewParamsPtr(new SystemViewParams(systemId)));
+    mSystemContainerMap.insert(systemId, SystemContainer());
 
-    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)));
+    connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)),
+            this, SLOT(localPositionChanged(UASInterface*,int,double,double,double,quint64)));
+    connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)),
+            this, SLOT(attitudeChanged(UASInterface*,int,double,double,double,quint64)));
 
-    trails.clear();
-    trailNode->removeDrawables(0, trailNode->getNumDrawables());
-    orientationNode->removeChildren(0, orientationNode->getNumChildren());
+    initializeSystem(systemId, uas->getColor());
 
-    this->uas = uas;
+    emit systemCreatedSignal(uas);
 }
 
 void
-Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time)
+Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
+                                      double x, double y, double z,
+                                      quint64 time)
 {
-    if (this->uas->getUASID() != uas->getUASID())
+    int systemId = uas->getUASID();
+
+    if (!mSystemContainerMap.contains(systemId))
     {
         return;
     }
 
-    if (!trails.contains(component))
+    SystemContainer& systemData = mSystemContainerMap[systemId];
+
+    // update system position
+    m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z));
+
+    // update trail data
+    if (!systemData.trailMap().contains(component))
     {
-        trails[component] = QVarLengthArray<osg::Vec3d, 10000>();
-        trailDrawableIdxs[component] = trails.size() - 1;
+        systemData.trailMap().insert(component, QVector<osg::Vec3d>());
+        systemData.trailMap()[component].reserve(10000);
+        systemData.trailIndexMap().insert(component,
+                                          systemData.trailMap().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);
+        systemData.trailNode()->addDrawable(createTrail(color));
     }
 
-    QVarLengthArray<osg::Vec3d, 10000>& trail = trails[component];
+    QVector<osg::Vec3d>& trail = systemData.trailMap()[component];
 
     bool addToTrail = false;
     if (trail.size() > 0)
@@ -267,228 +201,199 @@ Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double
 }
 
 void
-Pixhawk3DWidget::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time)
+Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component,
+                                 double roll, double pitch, double yaw,
+                                 quint64 time)
 {
-    if (this->uas->getUASID() != uas->getUASID())
-    {
-        return;
-    }
+    int systemId = uas->getUASID();
 
-    if (!trails.contains(component))
+    if (!mSystemContainerMap.contains(systemId))
     {
         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)));
+    // update system attitude
+    osg::Quat q(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
+                pitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                roll, osg::Vec3d(0.0f, 1.0f, 0.0f));
+    m3DWidget->systemGroup(systemId)->attitude()->setAttitude(q);
 }
 
 void
-Pixhawk3DWidget::selectFrame(QString text)
+Pixhawk3DWidget::showViewParamWindow(void)
 {
-    if (text.compare("Global") == 0)
+    if (!mViewParamWidget->isVisible())
     {
-        frame = MAV_FRAME_GLOBAL;
+        mViewParamWidget->show();
     }
-    else if (text.compare("Local") == 0)
-    {
-        frame = MAV_FRAME_LOCAL_NED;
-    }
-
-    getPosition(lastRobotX, lastRobotY, lastRobotZ);
-
-    recenter();
 }
 
 void
-Pixhawk3DWidget::showLocalGrid(int32_t state)
+Pixhawk3DWidget::followCameraChanged(int systemId)
 {
-    if (state == Qt::Checked)
+    if (systemId == -1)
     {
-        displayLocalGrid = true;
+        mFollowCameraId = -1;
     }
-    else
-    {
-        displayLocalGrid = false;
-    }
-}
 
-void
-Pixhawk3DWidget::showWorldGrid(int32_t state)
-{
-    if (state == Qt::Checked)
+    UASInterface* uas = UASManager::instance()->getUASForId(systemId);
+    if (!uas)
     {
-        displayWorldGrid = true;
+        return;
     }
-    else
+
+    if (mFollowCameraId != systemId)
     {
-        displayWorldGrid = false;
+        double x = 0.0, y = 0.0, z = 0.0;
+        getPosition(uas, mGlobalViewParams->frame(), x, y, z);
+
+        mCameraPos = QVector3D(x, y, z);
+
+        m3DWidget->recenterCamera(y, x, -z);
+
+        mFollowCameraId = systemId;
     }
 }
 
 void
-Pixhawk3DWidget::showTrails(int32_t state)
+Pixhawk3DWidget::recenterActiveCamera(void)
 {
-    if (state == Qt::Checked)
+    if (mFollowCameraId != -1)
     {
-        if (!displayTrails)
+        UASInterface* uas = UASManager::instance()->getUASForId(mFollowCameraId);
+        if (!uas)
         {
-            trails.clear();
+            return;
         }
 
-        displayTrails = true;
-    }
-    else
-    {
-        displayTrails = false;
-    }
-}
+        double x = 0.0, y = 0.0, z = 0.0;
+        getPosition(uas, mGlobalViewParams->frame(), x, y, z);
 
-void
-Pixhawk3DWidget::showWaypoints(int state)
-{
-    if (state == Qt::Checked)
-    {
-        displayWaypoints = true;
-    }
-    else
-    {
-        displayWaypoints = false;
+        mCameraPos = QVector3D(x, y, z);
+
+        m3DWidget->recenterCamera(y, x, -z);
     }
 }
 
 void
-Pixhawk3DWidget::selectMapSource(int index)
+Pixhawk3DWidget::modelChanged(int systemId, int index)
 {
-    mapNode->setImageryType(static_cast<Imagery::ImageryType>(index));
-
-    if (mapNode->getImageryType() == Imagery::BLANK_MAP)
+    if (!mSystemContainerMap.contains(systemId))
     {
-        displayImagery = false;
-    }
-    else
-    {
-        displayImagery = true;
+        return;
     }
-}
 
-void
-Pixhawk3DWidget::selectVehicleModel(int index)
-{
-    egocentricMap->removeChild(vehicleModel);
-    vehicleModel = vehicleModels.at(index);
-    egocentricMap->addChild(vehicleModel);
+    SystemContainer& systemData = mSystemContainerMap[systemId];
+    osg::ref_ptr<SystemGroupNode>& systemGroupNode = m3DWidget->systemGroup(systemId);
+
+    systemGroupNode->egocentricMap()->removeChild(systemData.modelNode());
+    systemData.modelNode() = systemData.models().at(index);
+    systemGroupNode->egocentricMap()->addChild(systemData.modelNode());
 }
 
 void
-Pixhawk3DWidget::recenter(void)
+Pixhawk3DWidget::setBirdView(void)
 {
-    double robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
-    getPosition(robotX, robotY, robotZ);
+    mViewParamWidget->setFollowCameraId(-1);
 
-    recenterCamera(robotY, robotX, -robotZ);
-}
+    mCameraPos = QVector3D(0.0, 0.0, -100.0);
 
-void
-Pixhawk3DWidget::toggleFollowCamera(int32_t state)
-{
-    if (state == Qt::Checked)
-    {
-        followCamera = true;
-    }
-    else
-    {
-        followCamera = false;
-    }
+    m3DWidget->rotateCamera(0.0, 0.0, 0.0);
+    m3DWidget->recenterCamera(mCameraPos.y(), mCameraPos.x(), -mCameraPos.z());
 }
 
 void
 Pixhawk3DWidget::selectTargetHeading(void)
 {
-    if (!uas)
+    if (!mActiveUAS)
     {
         return;
     }
 
     osg::Vec2d p;
 
-    if (frame == MAV_FRAME_GLOBAL)
+    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
     {
-        double altitude = uas->getAltitude();
+        double altitude = mActiveUAS->getAltitude();
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude);
 
-        p.set(cursorWorldCoords.first, cursorWorldCoords.second);
+        p.set(cursorWorldCoords.x(), cursorWorldCoords.y());
     }
-    else if (frame == MAV_FRAME_LOCAL_NED)
+    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
     {
-        double z = uas->getLocalZ();
+        double z = mActiveUAS->getLocalZ();
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);
 
-        p.set(cursorWorldCoords.first, cursorWorldCoords.second);
+        p.set(cursorWorldCoords.x(), cursorWorldCoords.y());
     }
 
+    SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()];
+    QVector4D& target = systemData.target();
+
     target.setW(atan2(p.y() - target.y(), p.x() - target.x()));
 }
 
 void
 Pixhawk3DWidget::selectTarget(void)
 {
-    if (!uas)
+    if (!mActiveUAS)
     {
         return;
     }
-    if (!uas->getParamManager())
+    if (!mActiveUAS->getParamManager())
     {
         return;
     }
 
-    if (frame == MAV_FRAME_GLOBAL)
+    SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()];
+    QVector4D& target = systemData.target();
+
+    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
     {
-        double altitude = uas->getAltitude();
+        double altitude = mActiveUAS->getAltitude();
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
-                                    altitude);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(mCachedMousePos, altitude);
 
         QVariant zTarget;
-        if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
+        if (!mActiveUAS->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
         {
             zTarget = -altitude;
         }
 
-        target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second,
+        target = QVector4D(cursorWorldCoords.x(), cursorWorldCoords.y(),
                            zTarget.toReal(), 0.0);
     }
-    else if (frame == MAV_FRAME_LOCAL_NED)
+    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
     {
-        double z = uas->getLocalZ();
+        double z = mActiveUAS->getLocalZ();
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(mCachedMousePos, -z);
 
         QVariant zTarget;
-        if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
+        if (!mActiveUAS->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
         {
             zTarget = z;
         }
 
-        target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second,
+        target = QVector4D(cursorWorldCoords.x(), cursorWorldCoords.y(),
                            zTarget.toReal(), 0.0);
     }
 
-    enableTarget = true;
+    int systemId = mActiveUAS->getUASID();
 
-    mode = SELECT_TARGET_HEADING_MODE;
+    QMap<int, SystemViewParamsPtr>::iterator it = mSystemViewParamMap.find(systemId);
+    if (it != mSystemViewParamMap.end())
+    {
+        it.value()->displayTarget() = true;
+    }
+
+    mMode = SELECT_TARGET_HEADING_MODE;
 }
 
 void
@@ -496,127 +401,129 @@ Pixhawk3DWidget::setTarget(void)
 {
     selectTargetHeading();
 
-    uas->setTargetPosition(target.x(), target.y(), target.z(),
-                           osg::RadiansToDegrees(target.w()));
+    SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()];
+    QVector4D& target = systemData.target();
+
+    mActiveUAS->setTargetPosition(target.x(), target.y(), target.z(),
+                                  osg::RadiansToDegrees(target.w()));
 }
 
 void
 Pixhawk3DWidget::insertWaypoint(void)
 {
-    if (!uas)
+    if (!mActiveUAS)
     {
         return;
     }
 
     Waypoint* wp = NULL;
-    if (frame == MAV_FRAME_GLOBAL)
+    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
     {
-        double latitude = uas->getLatitude();
-        double longitude = uas->getLongitude();
-        double altitude = uas->getAltitude();
+        double latitude = mActiveUAS->getLatitude();
+        double longitude = mActiveUAS->getLongitude();
+        double altitude = mActiveUAS->getAltitude();
         double x, y;
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
-                                    altitude);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(mCachedMousePos, altitude);
 
-        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
+        Imagery::UTMtoLL(cursorWorldCoords.x(), cursorWorldCoords.y(), utmZone,
                          latitude, longitude);
 
         wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25);
     }
-    else if (frame == MAV_FRAME_LOCAL_NED)
+    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
     {
-        double z = uas->getLocalZ();
+        double z = mActiveUAS->getLocalZ();
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(mCachedMousePos, -z);
 
-        wp = new Waypoint(0, cursorWorldCoords.first,
-                          cursorWorldCoords.second, z, 0.0, 0.25);
+        wp = new Waypoint(0, cursorWorldCoords.x(),
+                          cursorWorldCoords.y(), z, 0.0, 0.25);
     }
 
     if (wp)
     {
-        wp->setFrame(frame);
-        uas->getWaypointManager()->addWaypointEditable(wp);
+        wp->setFrame(mGlobalViewParams->frame());
+        mActiveUAS->getWaypointManager()->addWaypointEditable(wp);
     }
 
-    selectedWpIndex = wp->getId();
-    mode = MOVE_WAYPOINT_HEADING_MODE;
+    mSelectedWpIndex = wp->getId();
+    mMode = MOVE_WAYPOINT_HEADING_MODE;
 }
 
 void
 Pixhawk3DWidget::moveWaypointPosition(void)
 {
-    if (mode != MOVE_WAYPOINT_POSITION_MODE)
+    if (mMode != MOVE_WAYPOINT_POSITION_MODE)
     {
-        mode = MOVE_WAYPOINT_POSITION_MODE;
+        mMode = MOVE_WAYPOINT_POSITION_MODE;
         return;
     }
 
-    if (!uas)
+    if (!mActiveUAS)
     {
         return;
     }
 
     const QVector<Waypoint *> waypoints =
-        uas->getWaypointManager()->getWaypointEditableList();
-    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+        mActiveUAS->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(mSelectedWpIndex);
 
-    if (frame == MAV_FRAME_GLOBAL)
+    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
     {
-        double latitude = uas->getLatitude();
-        double longitude = uas->getLongitude();
-        double altitude = uas->getAltitude();
+        double latitude = mActiveUAS->getLatitude();
+        double longitude = mActiveUAS->getLongitude();
+        double altitude = mActiveUAS->getAltitude();
         double x, y;
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude);
 
-        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second,
+        Imagery::UTMtoLL(cursorWorldCoords.x(), cursorWorldCoords.y(),
                          utmZone, latitude, longitude);
 
         waypoint->setX(longitude);
         waypoint->setY(latitude);
     }
-    else if (frame == MAV_FRAME_LOCAL_NED)
+    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
     {
-        double z = uas->getLocalZ();
+        double z = mActiveUAS->getLocalZ();
 
-        std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+        QPointF cursorWorldCoords =
+            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);
 
-        waypoint->setX(cursorWorldCoords.first);
-        waypoint->setY(cursorWorldCoords.second);
+        waypoint->setX(cursorWorldCoords.x());
+        waypoint->setY(cursorWorldCoords.y());
     }
 }
 
 void
 Pixhawk3DWidget::moveWaypointHeading(void)
 {
-    if (mode != MOVE_WAYPOINT_HEADING_MODE)
+    if (mMode != MOVE_WAYPOINT_HEADING_MODE)
     {
-        mode = MOVE_WAYPOINT_HEADING_MODE;
+        mMode = MOVE_WAYPOINT_HEADING_MODE;
         return;
     }
 
-    if (!uas)
+    if (!mActiveUAS)
     {
         return;
     }
 
     const QVector<Waypoint *> waypoints =
-        uas->getWaypointManager()->getWaypointEditableList();
-    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+        mActiveUAS->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(mSelectedWpIndex);
 
     double x = 0.0, y = 0.0, z = 0.0;
 
-    if (frame == MAV_FRAME_GLOBAL)
+    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
     {
         double latitude = waypoint->getY();
         double longitude = waypoint->getX();
@@ -624,16 +531,16 @@ Pixhawk3DWidget::moveWaypointHeading(void)
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
     }
-    else if (frame == MAV_FRAME_LOCAL_NED)
+    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
     {
-        z = uas->getLocalZ();
+        z = mActiveUAS->getLocalZ();
     }
 
-    std::pair<double,double> cursorWorldCoords =
-        getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+    QPointF cursorWorldCoords =
+        m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);
 
-    double yaw = atan2(cursorWorldCoords.second - waypoint->getY(),
-                       cursorWorldCoords.first - waypoint->getX());
+    double yaw = atan2(cursorWorldCoords.y() - waypoint->getY(),
+                       cursorWorldCoords.x() - waypoint->getX());
     yaw = osg::RadiansToDegrees(yaw);
 
     waypoint->setYaw(yaw);
@@ -642,41 +549,41 @@ Pixhawk3DWidget::moveWaypointHeading(void)
 void
 Pixhawk3DWidget::deleteWaypoint(void)
 {
-    if (uas)
+    if (mActiveUAS)
     {
-        uas->getWaypointManager()->removeWaypoint(selectedWpIndex);
+        mActiveUAS->getWaypointManager()->removeWaypoint(mSelectedWpIndex);
     }
 }
 
 void
 Pixhawk3DWidget::setWaypointAltitude(void)
 {
-    if (!uas)
+    if (!mActiveUAS)
     {
         return;
     }
 
     bool ok;
     const QVector<Waypoint *> waypoints =
-        uas->getWaypointManager()->getWaypointEditableList();
-    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+        mActiveUAS->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(mSelectedWpIndex);
 
     double altitude = waypoint->getZ();
-    if (frame == MAV_FRAME_LOCAL_NED)
+    if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
     {
         altitude = -altitude;
     }
 
     double newAltitude =
-        QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
+        QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(mSelectedWpIndex),
                                 tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
     if (ok)
     {
-        if (frame == MAV_FRAME_GLOBAL)
+        if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
         {
             waypoint->setZ(newAltitude);
         }
-        else if (frame == MAV_FRAME_LOCAL_NED)
+        else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
         {
             waypoint->setZ(-newAltitude);
         }
@@ -686,36 +593,210 @@ Pixhawk3DWidget::setWaypointAltitude(void)
 void
 Pixhawk3DWidget::clearAllWaypoints(void)
 {
-    if (uas)
+    if (mActiveUAS)
     {
         const QVector<Waypoint *> waypoints =
-            uas->getWaypointManager()->getWaypointEditableList();
+            mActiveUAS->getWaypointManager()->getWaypointEditableList();
         for (int i = waypoints.size() - 1; i >= 0; --i)
         {
-            uas->getWaypointManager()->removeWaypoint(i);
+            mActiveUAS->getWaypointManager()->removeWaypoint(i);
         }
     }
 }
 
-QVector< osg::ref_ptr<osg::Node> >
-Pixhawk3DWidget::findVehicleModels(void)
+void
+Pixhawk3DWidget::sizeChanged(int width, int height)
+{
+    resizeHUD(width, height);
+}
+
+void
+Pixhawk3DWidget::update(void)
+{
+    MAV_FRAME frame = mGlobalViewParams->frame();
+
+    // set node visibility
+    m3DWidget->worldMap()->setChildValue(mWorldGridNode,
+                                        mGlobalViewParams->displayWorldGrid());
+    if (mGlobalViewParams->imageryType() == Imagery::BLANK_MAP)
+    {
+        m3DWidget->worldMap()->setChildValue(mImageryNode, false);
+    }
+    else
+    {
+        m3DWidget->worldMap()->setChildValue(mImageryNode, true);
+    }
+
+    // set system-specific node visibility
+    QMutableMapIterator<int, SystemViewParamsPtr> it(mSystemViewParamMap);
+    while (it.hasNext())
+    {
+        it.next();
+
+        osg::ref_ptr<SystemGroupNode>& systemNode = m3DWidget->systemGroup(it.key());
+        SystemContainer& systemData = mSystemContainerMap[it.key()];
+        const SystemViewParamsPtr& systemViewParams = it.value();
+
+        osg::ref_ptr<osg::Switch>& rollingMap = systemNode->rollingMap();
+        rollingMap->setChildValue(systemData.localGridNode(),
+                                  systemViewParams->displayLocalGrid());
+        rollingMap->setChildValue(systemData.pointCloudNode(),
+                                  systemViewParams->displayPointCloud());
+        rollingMap->setChildValue(systemData.targetNode(),
+                                  systemViewParams->displayTarget());
+        rollingMap->setChildValue(systemData.trailNode(),
+                                  systemViewParams->displayTrails());
+        rollingMap->setChildValue(systemData.waypointGroupNode(),
+                                  systemViewParams->displayWaypoints());
+
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+        rollingMap->setChildValue(systemData.obstacleGroupNode(),
+                                  systemViewParams->displayObstacleList());
+        rollingMap->setChildValue(systemData.plannedPathNode(),
+                                  systemViewParams->displayPlannedPath());
+
+        m3DWidget->hudGroup()->setChildValue(systemData.depthImageNode(),
+                                             systemViewParams->displayRGBD());
+        m3DWidget->hudGroup()->setChildValue(systemData.rgbImageNode(),
+                                             systemViewParams->displayRGBD());
+#endif
+    }
+
+    mImageryNode->setImageryType(mGlobalViewParams->imageryType());
+
+    if (mFollowCameraId != -1)
+    {
+        UASInterface* uas = UASManager::instance()->getUASForId(mFollowCameraId);
+        if (uas)
+        {
+            double x = 0.0, y = 0.0, z = 0.0;
+            getPosition(uas, mGlobalViewParams->frame(), x, y, z);
+
+            double dx = y - mCameraPos.y();
+            double dy = x - mCameraPos.x();
+            double dz = mCameraPos.z() - z;
+
+            m3DWidget->moveCamera(dx, dy, dz);
+
+            mCameraPos = QVector3D(x, y, z);
+        }
+    }
+    else
+    {
+        if (!mInitCameraPos && mActiveUAS)
+        {
+            double x = 0.0, y = 0.0, z = 0.0;
+            getPosition(mActiveUAS, frame, x, y, z);
+
+            m3DWidget->recenterCamera(y, x, -z);
+
+            mCameraPos = QVector3D(x, y, z);
+
+            setBirdView();
+            mInitCameraPos = true;
+        }
+    }
+
+    // update system-specific data
+    it.toFront();
+    while (it.hasNext())
+    {
+        it.next();
+
+        int systemId = it.key();
+
+        UASInterface* uas = UASManager::instance()->getUASForId(systemId);
+
+        osg::ref_ptr<SystemGroupNode>& systemNode = m3DWidget->systemGroup(systemId);
+        SystemContainer& systemData = mSystemContainerMap[systemId];
+        SystemViewParamsPtr& systemViewParams = it.value();
+
+        double x = 0.0;
+        double y = 0.0;
+        double z = 0.0;
+        double roll = 0.0;
+        double pitch = 0.0;
+        double yaw = 0.0;
+
+        getPose(uas, frame, x, y, z, roll, pitch, yaw);
+
+        if (systemViewParams->displayTarget())
+        {
+            if (systemData.target().isNull())
+            {
+                systemViewParams->displayTarget() = false;
+            }
+            else
+            {
+                updateTarget(uas, frame, x, y, z, systemData.target(),
+                             systemData.targetNode());
+            }
+        }
+        if (systemViewParams->displayTrails())
+        {
+            updateTrails(x, y, z, systemData.trailNode(),
+                         systemData.trailMap(), systemData.trailIndexMap());
+        }
+        else
+        {
+            systemData.trailMap().clear();
+        }
+        if (systemViewParams->displayWaypoints())
+        {
+            updateWaypoints(uas, frame, systemData.waypointGroupNode());
+        }
+
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+        if (systemViewParams->displayObstacleList())
+        {
+            updateObstacles(uas, frame, x, y, z, systemData.obstacleGroupNode());
+        }
+        if (systemViewParams->displayPlannedPath())
+        {
+            updatePlannedPath(uas, frame, x, y, z, systemData.plannedPathNode());
+        }
+        if (systemViewParams->displayPointCloud())
+        {
+            updatePointCloud(uas, frame, x, y, z, systemData.pointCloudNode(),
+                             systemViewParams->colorPointCloudByDistance());
+        }
+        if (systemViewParams->displayRGBD())
+        {
+            updateRGBD(uas, frame, systemData.rgbImageNode(),
+                       systemData.depthImageNode());
+        }
+#endif
+    }
+
+    if (frame == MAV_FRAME_GLOBAL &&
+        mGlobalViewParams->imageryType() != Imagery::BLANK_MAP)
+    {
+//        updateImagery(robotX, robotY, robotZ, utmZone);
+    }
+
+    updateHUD(mActiveUAS, frame);
+
+    layout()->update();
+}
+
+void
+Pixhawk3DWidget::addModels(QVector< osg::ref_ptr<osg::Node> >& models,
+                           const QColor& systemColor)
 {
     QDir directory("models");
     QStringList files = directory.entryList(QStringList("*.osg"), QDir::Files);
 
-    QVector< osg::ref_ptr<osg::Node> > nodes;
-
     // add Pixhawk Bravo model
-    nodes.push_back(PixhawkCheetahGeode::instance());
+    models.push_back(PixhawkCheetahGeode::create(systemColor));
 
     // add sphere of 0.05m radius
     osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.05f);
     osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
-    sphereDrawable->setColor(osg::Vec4f(0.5f, 0.0f, 0.5f, 1.0f));
+    sphereDrawable->setColor(osg::Vec4f(systemColor.redF(), systemColor.greenF(), systemColor.blueF(), 1.0f));
     osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
     sphereGeode->addDrawable(sphereDrawable);
     sphereGeode->setName("Sphere (0.1m)");
-    nodes.push_back(sphereGeode);
+    models.push_back(sphereGeode);
 
     // add all other models in folder
     for (int i = 0; i < files.size(); ++i)
@@ -725,280 +806,107 @@ Pixhawk3DWidget::findVehicleModels(void)
 
         if (node)
         {
-            nodes.push_back(node);
+            models.push_back(node);
         }
         else
         {
             printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
         }
     }
-
-//    QStringList dirs = directory.entryList(QDir::Dirs);
-//    // Add models in subfolders
-//    for (int i = 0; i < dirs.size(); ++i)
-//    {
-//        // Handling the current directory
-//        QStringList currFiles = QDir(dirs[i]).entryList(QStringList("*.ac"), QDir::Files);
-
-//        // Load the file
-//        osg::ref_ptr<osg::Node> node =
-//                osgDB::readNodeFile(directory.absoluteFilePath(currFiles.first()).toStdString().c_str());
-
-//        if (node)
-//        {
-
-//        nodes.push_back(node);
-//        }
-//        else
-//        {
-//            printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
-//        }
-//    }
-
-    return nodes;
 }
 
 void
 Pixhawk3DWidget::buildLayout(void)
 {
-    QComboBox* frameComboBox = new QComboBox(this);
-    frameComboBox->addItem("Local");
-    frameComboBox->addItem("Global");
-    frameComboBox->setFixedWidth(70);
-
-    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("Trails");
-    trailCheckBox->setChecked(displayTrails);
+    QPushButton* viewParamWindowButton = new QPushButton(this);
+    viewParamWindowButton->setText("View Parameters");
 
-    QCheckBox* waypointsCheckBox = new QCheckBox(this);
-    waypointsCheckBox->setText("Waypoints");
-    waypointsCheckBox->setChecked(displayWaypoints);
-
-    QLabel* mapLabel = new QLabel("Map", this);
-    QComboBox* mapComboBox = new QComboBox(this);
-    mapComboBox->addItem("None");
-    mapComboBox->addItem("Map (Google)");
-    mapComboBox->addItem("Satellite (Google)");
-
-    QLabel* modelLabel = new QLabel("Vehicle", this);
-    QComboBox* modelComboBox = new QComboBox(this);
-    for (int i = 0; i < vehicleModels.size(); ++i)
-    {
-        modelComboBox->addItem(vehicleModels[i]->getName().c_str());
-    }
+    QHBoxLayout* layoutTop = new QHBoxLayout;
+    layoutTop->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding));
+    layoutTop->addWidget(viewParamWindowButton);
 
     QPushButton* recenterButton = new QPushButton(this);
     recenterButton->setText("Recenter Camera");
 
-    QCheckBox* followCameraCheckBox = new QCheckBox(this);
-    followCameraCheckBox->setText("Follow Camera");
-    followCameraCheckBox->setChecked(followCamera);
+    QPushButton* birdViewButton = new QPushButton(this);
+    birdViewButton->setText("Bird's Eye View");
+
+    QHBoxLayout* layoutBottom = new QHBoxLayout;
+    layoutBottom->addWidget(recenterButton);
+    layoutBottom->addWidget(birdViewButton);
+    layoutBottom->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding));
 
     QGridLayout* layout = new QGridLayout(this);
     layout->setMargin(0);
     layout->setSpacing(2);
-    layout->addWidget(frameComboBox, 0, 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->addLayout(layoutTop, 0, 0);
+    layout->addWidget(m3DWidget, 1, 0);
+    layout->addLayout(layoutBottom, 2, 0);
     layout->setRowStretch(0, 1);
     layout->setRowStretch(1, 100);
     layout->setRowStretch(2, 1);
-    setLayout(layout);
-
-    connect(frameComboBox, SIGNAL(currentIndexChanged(QString)),
-            this, SLOT(selectFrame(QString)));
-    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(showTrails(int)));
-    connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(showWaypoints(int)));
-    connect(mapComboBox, SIGNAL(currentIndexChanged(int)),
-            this, SLOT(selectMapSource(int)));
-    connect(modelComboBox, SIGNAL(currentIndexChanged(int)),
-            this, SLOT(selectVehicleModel(int)));
-    connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter()));
-    connect(followCameraCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(toggleFollowCamera(int)));
-}
-
-void
-Pixhawk3DWidget::resizeGL(int width, int height)
-{
-    Q3DWidget::resizeGL(width, height);
 
-    resizeHUD();
+    connect(viewParamWindowButton, SIGNAL(clicked()),
+            this, SLOT(showViewParamWindow()));
+    connect(recenterButton, SIGNAL(clicked()),
+            this, SLOT(recenterActiveCamera()));
+    connect(birdViewButton, SIGNAL(clicked()),
+            this, SLOT(setBirdView()));
 }
 
 void
-Pixhawk3DWidget::display(void)
+Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
 {
-    // set node visibility
-    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);
-#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    rollingMap->setChildValue(obstacleGroupNode, displayObstacleList);
-    rollingMap->setChildValue(pathNode, displayPath);
-#endif
-    rollingMap->setChildValue(rgbd3DNode, displayRGBD3D);
-    hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
-    hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
-
-    if (!uas)
+    QWidget::keyPressEvent(event);
+    if (event->isAccepted())
     {
         return;
     }
 
-    double robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw;
-    QString utmZone;
-    getPose(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
-
-    if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f)
-    {
-        lastRobotX = robotX;
-        lastRobotY = robotY;
-        lastRobotZ = robotZ;
-
-        recenterCamera(robotY, robotX, -robotZ);
-    }
-
-    if (followCamera)
-    {
-        double dx = robotY - lastRobotY;
-        double dy = robotX - lastRobotX;
-        double dz = lastRobotZ - robotZ;
-
-        moveCamera(dx, dy, dz);
-    }
-
-    robotPosition->setPosition(osg::Vec3d(robotY, robotX, -robotZ));
-    robotAttitude->setAttitude(osg::Quat(-robotYaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
-                                         robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
-                                         robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
-
-    if (displayTrails)
-    {
-        updateTrails(robotX, robotY, robotZ);
-    }
-
-    if (frame == MAV_FRAME_GLOBAL && displayImagery)
-    {
-        updateImagery(robotX, robotY, robotZ, utmZone);
-    }
-
-    if (displayWaypoints)
-    {
-        updateWaypoints();
-    }
-
-    if (enableTarget)
-    {
-        updateTarget(robotX, robotY, robotZ);
-    }
-
-#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    if (displayRGBD2D || displayRGBD3D)
-    {
-        updateRGBD(robotX, robotY, robotZ);
-    }
-
-    if (displayObstacleList)
-    {
-        updateObstacles(robotX, robotY, robotZ);
-    }
-
-    if (displayPath)
-    {
-        updatePath(robotX, robotY, robotZ);
-    }
-#endif
-
-    updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
-
-    lastRobotX = robotX;
-    lastRobotY = robotY;
-    lastRobotZ = robotZ;
-
-    layout()->update();
+    m3DWidget->handleKeyPressEvent(event);
 }
 
 void
-Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
+Pixhawk3DWidget::keyReleaseEvent(QKeyEvent* event)
 {
-    if (!event->text().isEmpty())
+    QWidget::keyReleaseEvent(event);
+    if (event->isAccepted())
     {
-        switch (*(event->text().toAscii().data()))
-        {
-        case '1':
-            displayRGBD2D = !displayRGBD2D;
-            break;
-        case '2':
-            displayRGBD3D = !displayRGBD3D;
-            break;
-        case 'c':
-        case 'C':
-            enableRGBDColor = !enableRGBDColor;
-            break;
-        case 'o':
-        case 'O':
-            displayObstacleList = !displayObstacleList;
-            break;
-        case 'p':
-        case 'P':
-            displayPath = !displayPath;
-            break;
-        }
+        return;
     }
 
-    Q3DWidget::keyPressEvent(event);
+    m3DWidget->handleKeyReleaseEvent(event);
 }
 
 void
 Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
+    QWidget::mousePressEvent(event);
+    if (event->isAccepted())
+    {
+        return;
+    }
+
     if (event->button() == Qt::LeftButton)
     {
-        if (mode == SELECT_TARGET_HEADING_MODE)
+        if (mMode == SELECT_TARGET_HEADING_MODE)
         {
             setTarget();
+            event->accept();
         }
 
-        if (mode != DEFAULT_MODE)
+        if (mMode != DEFAULT_MODE)
         {
-            mode = DEFAULT_MODE;
+            mMode = DEFAULT_MODE;
+            event->accept();
         }
 
         if (event->modifiers() == Qt::ShiftModifier)
         {
-            selectedWpIndex = findWaypoint(event->pos());
-            if (selectedWpIndex == -1)
+            mSelectedWpIndex = findWaypoint(event->pos());
+            if (mSelectedWpIndex == -1)
             {
-                cachedMousePos = event->pos();
+                mCachedMousePos = event->pos();
 
                 showInsertWaypointMenu(event->globalPos());
             }
@@ -1006,51 +914,149 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
             {
                 showEditWaypointMenu(event->globalPos());
             }
+            event->accept();
 
             return;
         }
     }
 
-    Q3DWidget::mousePressEvent(event);
+    m3DWidget->handleMousePressEvent(event);
 }
 
 void
-Pixhawk3DWidget::showEvent(QShowEvent* event)
+Pixhawk3DWidget::mouseReleaseEvent(QMouseEvent* event)
 {
-    Q3DWidget::showEvent(event);
-    emit visibilityChanged(true);
-}
+    QWidget::mouseReleaseEvent(event);
+    if (event->isAccepted())
+    {
+        return;
+    }
 
-void
-Pixhawk3DWidget::hideEvent(QHideEvent* event)
-{
-    Q3DWidget::hideEvent(event);
-    emit visibilityChanged(false);
+    m3DWidget->handleMouseReleaseEvent(event);
 }
 
 void
 Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event)
 {
-    if (mode == SELECT_TARGET_HEADING_MODE)
+    QWidget::mouseMoveEvent(event);
+    if (event->isAccepted())
+    {
+        return;
+    }
+
+    if (mMode == SELECT_TARGET_HEADING_MODE)
     {
         selectTargetHeading();
+        event->accept();
     }
-    if (mode == MOVE_WAYPOINT_POSITION_MODE)
+    if (mMode == MOVE_WAYPOINT_POSITION_MODE)
     {
         moveWaypointPosition();
+        event->accept();
     }
-    if (mode == MOVE_WAYPOINT_HEADING_MODE)
+    if (mMode == MOVE_WAYPOINT_HEADING_MODE)
     {
         moveWaypointHeading();
+        event->accept();
+    }
+
+    m3DWidget->handleMouseMoveEvent(event);
+}
+
+void
+Pixhawk3DWidget::wheelEvent(QWheelEvent* event)
+{
+    QWidget::wheelEvent(event);
+    if (event->isAccepted())
+    {
+        return;
+    }
+
+    m3DWidget->handleWheelEvent(event);
+}
+
+void
+Pixhawk3DWidget::showEvent(QShowEvent* event)
+{
+    emit visibilityChanged(true);
+}
+
+void
+Pixhawk3DWidget::hideEvent(QHideEvent* event)
+{
+    emit visibilityChanged(false);
+}
+
+void
+Pixhawk3DWidget::initializeSystem(int systemId, const QColor& systemColor)
+{
+    SystemViewParamsPtr& systemViewParams = mSystemViewParamMap[systemId];
+    SystemContainer& systemData = mSystemContainerMap[systemId];
+    osg::ref_ptr<SystemGroupNode>& systemNode = m3DWidget->systemGroup(systemId);
+
+    // generate grid model
+    systemData.localGridNode() = createLocalGrid();
+    systemNode->rollingMap()->addChild(systemData.localGridNode(), false);
+
+    // generate point cloud model
+    systemData.pointCloudNode() = createPointCloud();
+    systemNode->rollingMap()->addChild(systemData.pointCloudNode(), false);
+
+    // generate target model
+    systemData.targetNode() = createTarget(systemColor);
+    systemNode->rollingMap()->addChild(systemData.targetNode(), false);
+
+    // generate empty trail model
+    systemData.trailNode() = new osg::Geode;
+    systemNode->rollingMap()->addChild(systemData.trailNode(), false);
+
+    // generate waypoint model
+    systemData.waypointGroupNode() = new WaypointGroupNode(systemColor);
+    systemData.waypointGroupNode()->init();
+    systemNode->rollingMap()->addChild(systemData.waypointGroupNode(), false);
+
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+    systemData.obstacleGroupNode() = new ObstacleGroupNode;
+    systemData.obstacleGroupNode()->init();
+    systemNode->rollingMap()->addChild(systemData.obstacleGroupNode(), false);
+
+    // generate path model
+    systemData.plannedPathNode() = new osg::Geode;
+    systemData.plannedPathNode()->addDrawable(createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f)));
+    systemNode->rollingMap()->addChild(systemData.plannedPathNode(), false);
+#endif
+
+    systemData.rgbImageNode() = new ImageWindowGeode;
+    systemData.rgbImageNode()->init("RGB Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                                    m3DWidget->font());
+    m3DWidget->hudGroup()->addChild(systemData.rgbImageNode(), false);
+
+    systemData.depthImageNode() = new ImageWindowGeode;
+    systemData.depthImageNode()->init("Depth Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                                      m3DWidget->font());
+    m3DWidget->hudGroup()->addChild(systemData.depthImageNode(), false);
+
+    // find available models
+    addModels(systemData.models(), systemColor);
+    systemViewParams->modelNames();
+    for (int i = 0; i < systemData.models().size(); ++i)
+    {
+        systemViewParams->modelNames().push_back(systemData.models()[i]->getName().c_str());
     }
 
-    Q3DWidget::mouseMoveEvent(event);
+    systemData.modelNode() = systemData.models().front();
+    systemNode->egocentricMap()->addChild(systemData.modelNode());
+
+    connect(systemViewParams.data(), SIGNAL(modelChangedSignal(int,int)),
+            this, SLOT(modelChanged(int,int)));
 }
 
 void
-Pixhawk3DWidget::getPose(double& x, double& y, double& z,
+Pixhawk3DWidget::getPose(UASInterface* uas,
+                         MAV_FRAME frame,
+                         double& x, double& y, double& z,
                          double& roll, double& pitch, double& yaw,
-                         QString& utmZone)
+                         QString& utmZone) const
 {
     if (!uas)
     {
@@ -1079,16 +1085,20 @@ Pixhawk3DWidget::getPose(double& x, double& y, double& z,
 }
 
 void
-Pixhawk3DWidget::getPose(double& x, double& y, double& z,
-                         double& roll, double& pitch, double& yaw)
+Pixhawk3DWidget::getPose(UASInterface* uas,
+                         MAV_FRAME frame,
+                         double& x, double& y, double& z,
+                         double& roll, double& pitch, double& yaw) const
 {
     QString utmZone;
-    getPose(x, y, z, roll, pitch, yaw);
+    getPose(uas, frame, x, y, z, roll, pitch, yaw, utmZone);
 }
 
 void
-Pixhawk3DWidget::getPosition(double& x, double& y, double& z,
-                             QString& utmZone)
+Pixhawk3DWidget::getPosition(UASInterface* uas,
+                             MAV_FRAME frame,
+                             double& x, double& y, double& z,
+                             QString& utmZone) const
 {
     if (!uas)
     {
@@ -1113,10 +1123,12 @@ Pixhawk3DWidget::getPosition(double& x, double& y, double& z,
 }
 
 void
-Pixhawk3DWidget::getPosition(double& x, double& y, double& z)
+Pixhawk3DWidget::getPosition(UASInterface* uas,
+                             MAV_FRAME frame,
+                             double& x, double& y, double& z) const
 {
     QString utmZone;
-    getPosition(x, y, z, utmZone);
+    getPosition(uas, frame, x, y, z, utmZone);
 }
 
 osg::ref_ptr<osg::Geode>
@@ -1323,13 +1335,13 @@ Pixhawk3DWidget::createTrail(const osg::Vec4& color)
 }
 
 osg::ref_ptr<Imagery>
-Pixhawk3DWidget::createMap(void)
+Pixhawk3DWidget::createImagery(void)
 {
     return osg::ref_ptr<Imagery>(new Imagery());
 }
 
 osg::ref_ptr<osg::Geode>
-Pixhawk3DWidget::createRGBD3D(void)
+Pixhawk3DWidget::createPointCloud(void)
 {
     int frameSize = 752 * 480;
 
@@ -1350,7 +1362,7 @@ Pixhawk3DWidget::createRGBD3D(void)
 }
 
 osg::ref_ptr<osg::Node>
-Pixhawk3DWidget::createTarget(void)
+Pixhawk3DWidget::createTarget(const QColor& color)
 {
     osg::ref_ptr<osg::PositionAttitudeTransform> pat =
         new osg::PositionAttitudeTransform;
@@ -1359,7 +1371,7 @@ Pixhawk3DWidget::createTarget(void)
 
     osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f);
     osg::ref_ptr<osg::ShapeDrawable> coneDrawable = new osg::ShapeDrawable(cone);
-    coneDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
+    coneDrawable->setColor(osg::Vec4f(color.redF(), color.greenF(), color.blueF(), 1.0f));
     coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
     osg::ref_ptr<osg::Geode> coneGeode = new osg::Geode;
     coneGeode->addDrawable(coneDrawable);
@@ -1377,98 +1389,110 @@ Pixhawk3DWidget::setupHUD(void)
     hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.5f));
     hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 1.0f));
 
-    hudBackgroundGeometry = new osg::Geometry;
-    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
-                                           0, 4));
-    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
-                                           4, 4));
-    hudBackgroundGeometry->setColorArray(hudColors);
-    hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET);
-    hudBackgroundGeometry->setUseDisplayList(false);
-
-    statusText = new osgText::Text;
-    statusText->setCharacterSize(11);
-    statusText->setFont(font);
-    statusText->setAxisAlignment(osgText::Text::SCREEN);
-    statusText->setColor(osg::Vec4(255, 255, 255, 1));
+    mHudBackgroundGeometry = new osg::Geometry;
+    mHudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                            0, 4));
+    mHudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                            4, 4));
+    mHudBackgroundGeometry->setColorArray(hudColors);
+    mHudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET);
+    mHudBackgroundGeometry->setUseDisplayList(false);
+
+    mStatusText = new osgText::Text;
+    mStatusText->setCharacterSize(11);
+    mStatusText->setFont(m3DWidget->font());
+    mStatusText->setAxisAlignment(osgText::Text::SCREEN);
+    mStatusText->setColor(osg::Vec4(255, 255, 255, 1));
 
     osg::ref_ptr<osg::Geode> statusGeode = new osg::Geode;
-    statusGeode->addDrawable(hudBackgroundGeometry);
-    statusGeode->addDrawable(statusText);
-    hudGroup->addChild(statusGeode);
-
-    rgbImage = new osg::Image;
-    rgb2DGeode = new ImageWindowGeode;
-    rgb2DGeode->init("RGB Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
-                     rgbImage, font);
-    hudGroup->addChild(rgb2DGeode);
-
-    depthImage = new osg::Image;
-    depth2DGeode = new ImageWindowGeode;
-    depth2DGeode->init("Depth Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
-                       depthImage, font);
-    hudGroup->addChild(depth2DGeode);
-
-    scaleGeode = new HUDScaleGeode;
-    scaleGeode->init(font);
-    hudGroup->addChild(scaleGeode);
+    statusGeode->addDrawable(mHudBackgroundGeometry);
+    statusGeode->addDrawable(mStatusText);
+    m3DWidget->hudGroup()->addChild(statusGeode);
+
+    mScaleGeode = new HUDScaleGeode;
+    mScaleGeode->init(m3DWidget->font());
+    m3DWidget->hudGroup()->addChild(mScaleGeode);
 }
 
 void
-Pixhawk3DWidget::resizeHUD(void)
+Pixhawk3DWidget::resizeHUD(int width, int height)
 {
     int topHUDHeight = 25;
     int bottomHUDHeight = 25;
 
-    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
+    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(mHudBackgroundGeometry->getVertexArray());
     if (vertices == NULL || vertices->size() != 8)
     {
         osg::ref_ptr<osg::Vec3Array> newVertices = new osg::Vec3Array(8);
-        hudBackgroundGeometry->setVertexArray(newVertices);
+        mHudBackgroundGeometry->setVertexArray(newVertices);
 
-        vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
+        vertices = static_cast<osg::Vec3Array*>(mHudBackgroundGeometry->getVertexArray());
     }
 
-    (*vertices)[0] = osg::Vec3(0, height(), -1);
-    (*vertices)[1] = osg::Vec3(width(), height(), -1);
-    (*vertices)[2] = osg::Vec3(width(), height() - topHUDHeight, -1);
-    (*vertices)[3] = osg::Vec3(0, height() - topHUDHeight, -1);
+    (*vertices)[0] = osg::Vec3(0, height, -1);
+    (*vertices)[1] = osg::Vec3(width, height, -1);
+    (*vertices)[2] = osg::Vec3(width, height - topHUDHeight, -1);
+    (*vertices)[3] = osg::Vec3(0, height - topHUDHeight, -1);
     (*vertices)[4] = osg::Vec3(0, 0, -1);
-    (*vertices)[5] = osg::Vec3(width(), 0, -1);
-    (*vertices)[6] = osg::Vec3(width(), bottomHUDHeight, -1);
+    (*vertices)[5] = osg::Vec3(width, 0, -1);
+    (*vertices)[6] = osg::Vec3(width, bottomHUDHeight, -1);
     (*vertices)[7] = osg::Vec3(0, bottomHUDHeight, -1);
 
-    statusText->setPosition(osg::Vec3(10, height() - 15, -1.5));
+    mStatusText->setPosition(osg::Vec3(10, height - 15, -1.5));
 
-    if (rgb2DGeode.valid() && depth2DGeode.valid())
+    QMutableMapIterator<int, SystemContainer> it(mSystemContainerMap);
+    while (it.hasNext())
     {
-        int windowWidth = (width() - 20) / 2;
-        int windowHeight = 3 * windowWidth / 4;
-        rgb2DGeode->setAttributes(10, (height() - windowHeight) / 2,
-                                  windowWidth, windowHeight);
-        depth2DGeode->setAttributes(width() / 2, (height() - windowHeight) / 2,
-                                    windowWidth, windowHeight);
+        it.next();
+
+        SystemContainer& systemData = it.value();
+
+        if (systemData.rgbImageNode().valid() &&
+            systemData.depthImageNode().valid())
+        {
+            int windowWidth = (width - 20) / 2;
+            int windowHeight = 3 * windowWidth / 4;
+            systemData.rgbImageNode()->setAttributes(10,
+                                                     (height - windowHeight) / 2,
+                                                     windowWidth,
+                                                     windowHeight);
+            systemData.depthImageNode()->setAttributes(width / 2,
+                                                       (height - windowHeight) / 2,
+                                                       windowWidth,
+                                                       windowHeight);
+        }
     }
 }
 
 void
-Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
-                           double robotRoll, double robotPitch, double robotYaw,
-                           const QString& utmZone)
+Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame)
 {
-    std::pair<double,double> cursorPosition =
-        getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
+    // display pose of current system
+    double x = 0.0;
+    double y = 0.0;
+    double z = 0.0;
+    double roll = 0.0;
+    double pitch = 0.0;
+    double yaw = 0.0;
+    QString utmZone;
+
+    getPose(uas, frame, x, y, z, roll, pitch, yaw, utmZone);
+
+    QPointF cursorPosition =
+        m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);
 
     std::ostringstream oss;
     oss.setf(std::ios::fixed, std::ios::floatfield);
     oss.precision(2);
+    oss << "MAV " << uas->getUASID() << ": ";
+
     if (frame == MAV_FRAME_GLOBAL)
     {
         double latitude, longitude;
-        Imagery::UTMtoLL(robotX, robotY, utmZone, latitude, longitude);
+        Imagery::UTMtoLL(x, y, utmZone, latitude, longitude);
 
         double cursorLatitude, cursorLongitude;
-        Imagery::UTMtoLL(cursorPosition.first, cursorPosition.second,
+        Imagery::UTMtoLL(cursorPosition.x(), cursorPosition.y(),
                          utmZone, cursorLatitude, cursorLongitude);
 
         oss.precision(6);
@@ -1476,10 +1500,10 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
             " Lon = " << longitude;
 
         oss.precision(2);
-        oss << " Altitude = " << -robotZ <<
-            " r = " << robotRoll <<
-            " p = " << robotPitch <<
-            " y = " << robotYaw;
+        oss << " Altitude = " << -z <<
+            " r = " << roll <<
+            " p = " << pitch <<
+            " y = " << yaw;
 
         oss.precision(6);
         oss << " Cursor [" << cursorLatitude <<
@@ -1487,32 +1511,36 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
     }
     else if (frame == MAV_FRAME_LOCAL_NED)
     {
-        oss << " x = " << robotX <<
-            " y = " << robotY <<
-            " z = " << robotZ <<
-            " r = " << robotRoll <<
-            " p = " << robotPitch <<
-            " y = " << robotYaw <<
-            " Cursor [" << cursorPosition.first <<
-            " " << cursorPosition.second << "]";
+        oss << " x = " << x <<
+            " y = " << y <<
+            " z = " << z <<
+            " r = " << roll <<
+            " p = " << pitch <<
+            " y = " << yaw <<
+            " Cursor [" << cursorPosition.x() <<
+            " " << cursorPosition.y() << "]";
     }
 
-    statusText->setText(oss.str());
+    mStatusText->setText(oss.str());
 
     bool darkBackground = true;
-    if (mapNode->getImageryType() == Imagery::GOOGLE_MAP)
+    if (mImageryNode->getImageryType() == Imagery::GOOGLE_MAP)
     {
         darkBackground = false;
     }
 
-    scaleGeode->update(height(), cameraParams.cameraFov,
-                       cameraManipulator->getDistance(), darkBackground);
+    mScaleGeode->update(height(), m3DWidget->cameraParams().fov(),
+                        m3DWidget->cameraManipulator()->getDistance(),
+                        darkBackground);
 }
 
 void
-Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ,
+                              osg::ref_ptr<osg::Geode>& trailNode,
+                              QMap<int, QVector<osg::Vec3d> >& trailMap,
+                              QMap<int, int>& trailIndexMap)
 {
-    QMapIterator<int,int> it(trailDrawableIdxs);
+    QMapIterator<int,int> it(trailIndexMap);
 
     while (it.hasNext())
     {
@@ -1523,7 +1551,7 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
 
         osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
 
-        const QVarLengthArray<osg::Vec3d, 10000>& trail = trails.value(it.key());
+        const QVector<osg::Vec3d>& trail = trailMap.value(it.key());
 
         for (int i = 0; i < trail.size(); ++i)
         {
@@ -1536,12 +1564,6 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
         drawArrays->setFirst(0);
         drawArrays->setCount(vertices->size());
         geometry->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)));
     }
 }
 
@@ -1549,22 +1571,22 @@ void
 Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
                                const QString& zone)
 {
-    if (mapNode->getImageryType() == Imagery::BLANK_MAP)
+    if (mImageryNode->getImageryType() == Imagery::BLANK_MAP)
     {
         return;
     }
 
-    double viewingRadius = cameraManipulator->getDistance() * 10.0;
+    double viewingRadius = m3DWidget->cameraManipulator()->getDistance() * 10.0;
     if (viewingRadius < 100.0)
     {
         viewingRadius = 100.0;
     }
 
     double minResolution = 0.25;
-    double centerResolution = cameraManipulator->getDistance() / 50.0;
+    double centerResolution = m3DWidget->cameraManipulator()->getDistance() / 50.0;
     double maxResolution = 1048576.0;
 
-    Imagery::ImageryType imageryType = mapNode->getImageryType();
+    Imagery::Type imageryType = mImageryNode->getImageryType();
     switch (imageryType)
     {
     case Imagery::GOOGLE_MAP:
@@ -1592,44 +1614,41 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
         resolution = maxResolution;
     }
 
-    mapNode->draw3D(viewingRadius,
-                    resolution,
-                    cameraManipulator->getCenter().y(),
-                    cameraManipulator->getCenter().x(),
-                    originX,
-                    originY,
-                    originZ,
-                    zone);
+    mImageryNode->draw3D(viewingRadius,
+                         resolution,
+                         m3DWidget->cameraManipulator()->getCenter().y(),
+                         m3DWidget->cameraManipulator()->getCenter().x(),
+                         originX,
+                         originY,
+                         originZ,
+                         zone);
 
     // prefetch map tiles
     if (resolution / 2.0 >= minResolution)
     {
-        mapNode->prefetch3D(viewingRadius / 2.0,
-                            resolution / 2.0,
-                            cameraManipulator->getCenter().y(),
-                            cameraManipulator->getCenter().x(),
-                            zone);
+        mImageryNode->prefetch3D(viewingRadius / 2.0,
+                                 resolution / 2.0,
+                                 m3DWidget->cameraManipulator()->getCenter().y(),
+                                 m3DWidget->cameraManipulator()->getCenter().x(),
+                                 zone);
     }
     if (resolution * 2.0 <= maxResolution)
     {
-        mapNode->prefetch3D(viewingRadius * 2.0,
-                            resolution * 2.0,
-                            cameraManipulator->getCenter().y(),
-                            cameraManipulator->getCenter().x(),
-                            zone);
+        mImageryNode->prefetch3D(viewingRadius * 2.0,
+                                 resolution * 2.0,
+                                 m3DWidget->cameraManipulator()->getCenter().y(),
+                                 m3DWidget->cameraManipulator()->getCenter().x(),
+                                 zone);
     }
 
-    mapNode->update();
-}
-
-void
-Pixhawk3DWidget::updateWaypoints(void)
-{
-    waypointGroupNode->update(frame, uas);
+    mImageryNode->update();
 }
 
 void
-Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updateTarget(UASInterface* uas, MAV_FRAME frame,
+                              double robotX, double robotY, double robotZ,
+                              QVector4D& target,
+                              osg::ref_ptr<osg::Node>& targetNode)
 {
     osg::PositionAttitudeTransform* pat =
         dynamic_cast<osg::PositionAttitudeTransform*>(targetNode.get());
@@ -1643,114 +1662,21 @@ Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ)
 
     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));
 }
 
-#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
 void
-Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updateWaypoints(UASInterface* uas, MAV_FRAME frame,
+                                 osg::ref_ptr<WaypointGroupNode>& waypointGroupNode)
 {
-    qreal receivedRGBDImageTimestamp, receivedPointCloudTimestamp;
-    px::RGBDImage rgbdImage = uas->getRGBDImage(receivedRGBDImageTimestamp);
-    px::PointCloudXYZRGB pointCloud = uas->getPointCloud(receivedPointCloudTimestamp);
-
-    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,
-                           reinterpret_cast<unsigned char *>(&(*(rgbdImage.mutable_imagedata1()))[0]),
-                           osg::Image::NO_DELETE);
-        rgbImage->dirty();
-
-        QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0);
-        for (uint32_t r = 0; r < rgbdImage.rows(); ++r)
-        {
-            const float* depth = reinterpret_cast<const float*>(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2());
-            uint8_t* pixel = reinterpret_cast<uint8_t*>(coloredDepth.data()) + r * rgbdImage.cols() * 3;
-            for (uint32_t c = 0; c < rgbdImage.cols(); ++c)
-            {
-                if (depth[c] != 0)
-                {
-                    int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f;
-                    idx = 127 - idx;
-
-                    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;
-            }
-        }
-
-        depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
-                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
-                             reinterpret_cast<unsigned char *>(coloredDepth.data()),
-                             osg::Image::NO_DELETE);
-        depthImage->dirty();
-    }
-
-    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);
-
-        double x = p.x() - robotX;
-        double y = p.y() - robotY;
-        double z = p.z() - robotZ;
-
-
-        (*vertices)[i].set(y, x, -z);
-
-        if (enableRGBDColor)
-        {
-            float rgb = p.rgb();
-
-            float b = *(reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
-            float g = *(1 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
-            float r = *(2 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
-
-            (*colors)[i].set(r, g, b, 1.0f);
-        }
-        else
-        {
-            double dist = sqrt(x * x + y * y + z * z);
-            int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
-
-            float r, g, b;
-            qgc::colormap("jet", colorIndex, r, g, b);
-
-            (*colors)[i].set(r, g, b, 1.0f);
-        }
-    }
-
-    if (geometry->getNumPrimitiveSets() == 0)
-    {
-        geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
-                                  0, pointCloud.points_size()));
-    }
-    else
-    {
-        osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
-        drawarrays->setCount(pointCloud.points_size());
-    }
+    waypointGroupNode->update(uas, frame);
 }
 
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+
 void
-Pixhawk3DWidget::updateObstacles(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updateObstacles(UASInterface* uas, MAV_FRAME frame,
+                                 double robotX, double robotY, double robotZ,
+                                 osg::ref_ptr<ObstacleGroupNode>& obstacleGroupNode)
 {
     if (frame == MAV_FRAME_GLOBAL)
     {
@@ -1772,12 +1698,14 @@ Pixhawk3DWidget::updateObstacles(double robotX, double robotY, double robotZ)
 }
 
 void
-Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updatePlannedPath(UASInterface* uas, MAV_FRAME frame,
+                                   double robotX, double robotY, double robotZ,
+                                   osg::ref_ptr<osg::Geode>& plannedPathNode)
 {
     qreal receivedTimestamp;
     px::Path path = uas->getPath(receivedTimestamp);
 
-    osg::Geometry* geometry = pathNode->getDrawable(0)->asGeometry();
+    osg::Geometry* geometry = plannedPathNode->getDrawable(0)->asGeometry();
     osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
     osg::Vec4Array* colorArray = reinterpret_cast<osg::Vec4Array*>(geometry->getColorArray());
 
@@ -1846,25 +1774,149 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
     geometry->dirtyBound();
 }
 
+void
+Pixhawk3DWidget::updateRGBD(UASInterface* uas, MAV_FRAME frame,
+                            osg::ref_ptr<ImageWindowGeode>& rgbImageNode,
+                            osg::ref_ptr<ImageWindowGeode>& depthImageNode)
+{
+    qreal receivedTimestamp;
+    px::RGBDImage rgbdImage = uas->getRGBDImage(receivedTimestamp);
+
+    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0 &&
+        QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
+    {
+        rgbImageNode->image()->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
+                                        GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
+                                        reinterpret_cast<unsigned char *>(&(*(rgbdImage.mutable_imagedata1()))[0]),
+                                        osg::Image::NO_DELETE);
+        rgbImageNode->image()->dirty();
+
+        QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0);
+        for (uint32_t r = 0; r < rgbdImage.rows(); ++r)
+        {
+            const float* depth = reinterpret_cast<const float*>(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2());
+            uint8_t* pixel = reinterpret_cast<uint8_t*>(coloredDepth.data()) + r * rgbdImage.cols() * 3;
+            for (uint32_t c = 0; c < rgbdImage.cols(); ++c)
+            {
+                if (depth[c] != 0)
+                {
+                    int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f;
+                    idx = 127 - idx;
+
+                    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;
+            }
+        }
+
+        depthImageNode->image()->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
+                                          GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+                                          reinterpret_cast<unsigned char *>(coloredDepth.data()),
+                                          osg::Image::NO_DELETE);
+        depthImageNode->image()->dirty();
+    }
+}
+
+void
+Pixhawk3DWidget::updatePointCloud(UASInterface* uas, MAV_FRAME frame,
+                                  double robotX, double robotY, double robotZ,
+                                  osg::ref_ptr<osg::Geode>& pointCloudNode,
+                                  bool colorPointCloudByDistance)
+{
+    qreal receivedTimestamp;
+    px::PointCloudXYZRGB pointCloud = uas->getPointCloud(receivedTimestamp);
+
+    osg::Geometry* geometry = pointCloudNode->getDrawable(0)->asGeometry();
+    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
+    osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
+
+    if (QGC::groundTimeSeconds() - receivedTimestamp > kMessageTimeout)
+    {
+        geometry->removePrimitiveSet(0, geometry->getNumPrimitiveSets());
+        return;
+    }
+
+    for (int i = 0; i < pointCloud.points_size(); ++i)
+    {
+        const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
+
+        double x = p.x() - robotX;
+        double y = p.y() - robotY;
+        double z = p.z() - robotZ;
+
+
+        (*vertices)[i].set(y, x, -z);
+
+        if (!colorPointCloudByDistance)
+        {
+            float rgb = p.rgb();
+
+            float b = *(reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
+            float g = *(1 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
+            float r = *(2 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
+
+            (*colors)[i].set(r, g, b, 1.0f);
+        }
+        else
+        {
+            double dist = sqrt(x * x + y * y + z * z);
+            int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
+
+            float r, g, b;
+            qgc::colormap("jet", colorIndex, r, g, b);
+
+            (*colors)[i].set(r, g, b, 1.0f);
+        }
+    }
+
+    if (geometry->getNumPrimitiveSets() == 0)
+    {
+        geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
+                                  0, pointCloud.points_size()));
+    }
+    else
+    {
+        osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
+        drawarrays->setCount(pointCloud.points_size());
+    }
+}
+
 #endif
 
 int
 Pixhawk3DWidget::findWaypoint(const QPoint& mousePos)
 {
-    if (getSceneData())
+    if (!m3DWidget->getSceneData() || !mActiveUAS)
     {
-        osgUtil::LineSegmentIntersector::Intersections intersections;
+        return -1;
+    }
 
-        if (computeIntersections(mousePos.x(), height() - mousePos.y(),
-                                 intersections))
+    SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()];
+    osg::ref_ptr<WaypointGroupNode>& waypointGroupNode = systemData.waypointGroupNode();
+
+    osgUtil::LineSegmentIntersector::Intersections intersections;
+
+    QPoint widgetMousePos = m3DWidget->mapFromParent(mousePos);
+
+    if (m3DWidget->computeIntersections(widgetMousePos.x(),
+                                        m3DWidget->height() - widgetMousePos.y(),
+                                        intersections))
+    {
+        for (osgUtil::LineSegmentIntersector::Intersections::iterator
+             it = intersections.begin(); it != intersections.end(); it++)
         {
-            for (osgUtil::LineSegmentIntersector::Intersections::iterator
-                    it = intersections.begin(); it != intersections.end(); it++)
+            for (uint i = 0 ; i < it->nodePath.size(); ++i)
             {
-                for (uint i = 0 ; i < it->nodePath.size(); ++i)
+                osg::Node* node = it->nodePath[i];
+                std::string nodeName = node->getName();
+                if (nodeName.substr(0, 2).compare("wp") == 0)
                 {
-                    std::string nodeName = it->nodePath[i]->getName();
-                    if (nodeName.substr(0, 2).compare("wp") == 0)
+                    if (node->getParent(0)->getParent(0) == waypointGroupNode.get())
                     {
                         return atoi(nodeName.substr(2).c_str());
                     }
@@ -1879,11 +1931,11 @@ Pixhawk3DWidget::findWaypoint(const QPoint& mousePos)
 bool
 Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
 {
-    if (getSceneData())
+    if (m3DWidget->getSceneData())
     {
         osgUtil::LineSegmentIntersector::Intersections intersections;
 
-        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        if (m3DWidget->computeIntersections(mouseX, height() - mouseY, intersections))
         {
             for (osgUtil::LineSegmentIntersector::Intersections::iterator
                     it = intersections.begin(); it != intersections.end(); it++)
@@ -1919,16 +1971,16 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
     QMenu menu;
 
     QString text;
-    text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
+    text = QString("Move waypoint %1").arg(QString::number(mSelectedWpIndex));
     menu.addAction(text, this, SLOT(moveWaypointPosition()));
 
-    text = QString("Change heading of waypoint %1").arg(QString::number(selectedWpIndex));
+    text = QString("Change heading of waypoint %1").arg(QString::number(mSelectedWpIndex));
     menu.addAction(text, this, SLOT(moveWaypointHeading()));
 
-    text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
+    text = QString("Change altitude of waypoint %1").arg(QString::number(mSelectedWpIndex));
     menu.addAction(text, this, SLOT(setWaypointAltitude()));
 
-    text = QString("Delete waypoint %1").arg(QString::number(selectedWpIndex));
+    text = QString("Delete waypoint %1").arg(QString::number(mSelectedWpIndex));
     menu.addAction(text, this, SLOT(deleteWaypoint()));
 
     menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));

src/ui/map3D/Pixhawk3DWidget.h

@@ -36,20 +36,16 @@
 
 #include "HUDScaleGeode.h"
 #include "Imagery.h"
-#include "ImageWindowGeode.h"
-#include "WaypointGroupNode.h"
-#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    #include "ObstacleGroupNode.h"
-#endif
-
 #include "Q3DWidget.h"
+#include "SystemContainer.h"
+#include "ViewParamWidget.h"
 
 class UASInterface;
 
 /**
  * @brief A 3D View widget which displays vehicle-centric information.
  **/
-class Pixhawk3DWidget : public Q3DWidget
+class Pixhawk3DWidget : public QWidget
 {
     Q_OBJECT
 
@@ -58,20 +54,20 @@ public:
     ~Pixhawk3DWidget();
 
 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);
+    void activeSystemChanged(UASInterface* uas);
+    void systemCreated(UASInterface* uas);
+    void localPositionChanged(UASInterface* uas, int component, double x, double y, double z, quint64 time);
+    void attitudeChanged(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time);
+
+signals:
+    void systemCreatedSignal(UASInterface* uas);
 
 private slots:
-    void selectFrame(QString text);
-    void showLocalGrid(int state);
-    void showWorldGrid(int state);
-    void showTrails(int state);
-    void showWaypoints(int state);
-    void selectMapSource(int index);
-    void selectVehicleModel(int index);
-    void recenter(void);
-    void toggleFollowCamera(int state);
+    void showViewParamWindow(void);
+    void followCameraChanged(int systemId);
+    void recenterActiveCamera(void);
+    void modelChanged(int systemId, int index);
+    void setBirdView(void);
 
     void selectTargetHeading(void);
     void selectTarget(void);
@@ -83,54 +79,84 @@ private slots:
     void setWaypointAltitude(void);
     void clearAllWaypoints(void);
 
+    void sizeChanged(int width, int height);
+    void update(void);
+
 protected:
-    QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
+    void addModels(QVector< osg::ref_ptr<osg::Node> >& models,
+                   const QColor& systemColor);
     void buildLayout(void);
-    virtual void resizeGL(int width, int height);
-    virtual void display(void);
-    virtual void keyPressEvent(QKeyEvent* event);
-    virtual void mousePressEvent(QMouseEvent* event);
-    virtual void showEvent(QShowEvent* event);
-    virtual void hideEvent(QHideEvent* event);
-    virtual void mouseMoveEvent(QMouseEvent* event);
 
-    UASInterface* uas;
+    void keyPressEvent(QKeyEvent* event);
+    void keyReleaseEvent(QKeyEvent* event);
+    void mousePressEvent(QMouseEvent* event);
+    void mouseReleaseEvent(QMouseEvent* event);
+    void mouseMoveEvent(QMouseEvent* event);
+    void wheelEvent(QWheelEvent* event);
+
+    void showEvent(QShowEvent* event);
+    void hideEvent(QHideEvent* event);
 
 signals:
     void visibilityChanged(bool visible);
 
 private:
-    void getPose(double& x, double& y, double& z,
+    void initializeSystem(int systemId, const QColor& systemColor);
+
+    void getPose(UASInterface* uas,
+                 MAV_FRAME frame,
+                 double& x, double& y, double& z,
                  double& roll, double& pitch, double& yaw,
-                 QString& utmZone);
-    void getPose(double& x, double& y, double& z,
-                 double& roll, double& pitch, double& yaw);
-    void getPosition(double& x, double& y, double& z,
-                     QString& utmZone);
-    void getPosition(double& x, double& y, double& z);
+                 QString& utmZone) const;
+    void getPose(UASInterface* uas,
+                 MAV_FRAME frame,
+                 double& x, double& y, double& z,
+                 double& roll, double& pitch, double& yaw) const;
+    void getPosition(UASInterface* uas,
+                     MAV_FRAME frame,
+                     double& x, double& y, double& z,
+                     QString& utmZone) const;
+    void getPosition(UASInterface* uas,
+                     MAV_FRAME frame,
+                     double& x, double& y, double& z) const;
 
     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);
+    osg::ref_ptr<Imagery> createImagery(void);
+    osg::ref_ptr<osg::Geode> createPointCloud(void);
+    osg::ref_ptr<osg::Node> createTarget(const QColor& color);
 
     void setupHUD(void);
-    void resizeHUD(void);
+    void resizeHUD(int width, int height);
 
-    void updateHUD(double robotX, double robotY, double robotZ,
-                   double robotRoll, double robotPitch, double robotYaw,
-                   const QString& utmZone);
-    void updateTrails(double robotX, double robotY, double robotZ);
+    void updateHUD(UASInterface* uas, MAV_FRAME frame);
     void updateImagery(double originX, double originY, double originZ,
                        const QString& zone);
-    void updateWaypoints(void);
-    void updateTarget(double robotX, double robotY, double robotZ);
+    void updateTarget(UASInterface* uas, MAV_FRAME frame,
+                      double robotX, double robotY, double robotZ,
+                      QVector4D& target,
+                      osg::ref_ptr<osg::Node>& targetNode);
+    void updateTrails(double robotX, double robotY, double robotZ,
+                      osg::ref_ptr<osg::Geode>& trailNode,
+                      QMap<int, QVector<osg::Vec3d> >& trailMap,
+                      QMap<int, int>& trailIndexMap);
+    void updateWaypoints(UASInterface* uas, MAV_FRAME frame,
+                         osg::ref_ptr<WaypointGroupNode>& waypointGroupNode);
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    void updateRGBD(double robotX, double robotY, double robotZ);
-    void updateObstacles(double robotX, double robotY, double robotZ);
-    void updatePath(double robotX, double robotY, double robotZ);
+    void updateRGBD(UASInterface* uas, MAV_FRAME frame,
+                    osg::ref_ptr<ImageWindowGeode>& rgbImageNode,
+                    osg::ref_ptr<ImageWindowGeode>& depthImageNode);
+    void updatePointCloud(UASInterface* uas, MAV_FRAME frame,
+                          double robotX, double robotY, double robotZ,
+                          osg::ref_ptr<osg::Geode>& pointCloudNode,
+                          bool colorPointCloudByDistance);
+    void updateObstacles(UASInterface* uas, MAV_FRAME frame,
+                         double robotX, double robotY, double robotZ,
+                         osg::ref_ptr<ObstacleGroupNode>& obstacleGroupNode);
+    void updatePlannedPath(UASInterface* uas, MAV_FRAME frame,
+                           double robotX, double robotY, double robotZ,
+                           osg::ref_ptr<osg::Geode>& plannedPathNode);
 #endif
 
     int findWaypoint(const QPoint& mousePos);
@@ -146,53 +172,33 @@ private:
         MOVE_WAYPOINT_HEADING_MODE,
         SELECT_TARGET_HEADING_MODE
     };
-    Mode mode;
-    int selectedWpIndex;
-
-    bool displayLocalGrid;
-    bool displayWorldGrid;
-    bool displayTrails;
-    bool displayImagery;
-    bool displayWaypoints;
-    bool displayRGBD2D;
-    bool displayRGBD3D;
-    bool displayObstacleList;
-    bool displayPath;
-    bool enableRGBDColor;
-    bool enableTarget;
-
-    bool followCamera;
-
-    QMap<int, QVarLengthArray<osg::Vec3d, 10000> > trails;
-    QMap<int, int> trailDrawableIdxs;
-
-    osg::ref_ptr<osg::Node> vehicleModel;
-    osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;
-    osg::ref_ptr<osgText::Text> statusText;
-    osg::ref_ptr<HUDScaleGeode> scaleGeode;
-    osg::ref_ptr<ImageWindowGeode> rgb2DGeode;
-    osg::ref_ptr<ImageWindowGeode> depth2DGeode;
-    osg::ref_ptr<osg::Image> rgbImage;
-    osg::ref_ptr<osg::Image> depthImage;
-    osg::ref_ptr<osg::Geode> localGridNode;
-    osg::ref_ptr<osg::Geode> worldGridNode;
-    osg::ref_ptr<osg::Geode> trailNode;
-    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;
-#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    osg::ref_ptr<ObstacleGroupNode> obstacleGroupNode;
-    osg::ref_ptr<osg::Geode> pathNode;
-#endif
+    Mode mMode;
+    int mSelectedWpIndex;
+
+    int mActiveSystemId;
+    UASInterface* mActiveUAS;
+
+    GlobalViewParamsPtr mGlobalViewParams;
+
+    // maps system id to system-specific view parameters
+    QMap<int, SystemViewParamsPtr> mSystemViewParamMap;
+
+    // maps system id to system-specific data
+    QMap<int, SystemContainer> mSystemContainerMap;
+
+    osg::ref_ptr<osg::Geometry> mHudBackgroundGeometry;
+    osg::ref_ptr<Imagery> mImageryNode;
+    osg::ref_ptr<HUDScaleGeode> mScaleGeode;
+    osg::ref_ptr<osgText::Text> mStatusText;
+    osg::ref_ptr<osg::Geode> mWorldGridNode;
 
-    QVector< osg::ref_ptr<osg::Node> > vehicleModels;
+    QPoint mCachedMousePos;
+    int mFollowCameraId;
+    QVector3D mCameraPos;
+    bool mInitCameraPos;
 
-    MAV_FRAME frame;
-    QVector4D target;
-    QPoint cachedMousePos;
-    double lastRobotX, lastRobotY, lastRobotZ;
+    Q3DWidget* m3DWidget;
+    ViewParamWidget* mViewParamWidget;
 };
 
 #endif // PIXHAWK3DWIDGET_H

src/ui/map3D/PixhawkCheetahGeode.cc

@@ -59335,7 +59335,7 @@ void SelectMaterial(int i)
 
 };
 
-osg::ref_ptr<osg::Geode> PixhawkCheetahGeode::_instance;
+osg::ref_ptr<osg::Geode> PixhawkCheetahGeode::mInstance;
 
 PixhawkCheetahGeode::PixhawkCheetahGeode()
 {
@@ -59345,15 +59345,17 @@ PixhawkCheetahGeode::PixhawkCheetahGeode()
 osg::ref_ptr<osg::Geode>
 PixhawkCheetahGeode::instance(void)
 {
-    if (!_instance.valid()) {
-        _instance = create(1.0f, 1.0f, 1.0f);
+    if (!mInstance.valid())
+    {
+        QColor color(255, 255, 255);
+        mInstance = create(color);
     }
 
-    return _instance;
+    return mInstance;
 }
 
 osg::ref_ptr<osg::Geode>
-PixhawkCheetahGeode::create(float red, float green, float blue)
+PixhawkCheetahGeode::create(const QColor& color)
 {
     osg::ref_ptr<osg::Geode> geode(new osg::Geode());
     geode->setName("Pixhawk Bravo");
@@ -59365,11 +59367,13 @@ PixhawkCheetahGeode::create(float red, float green, float blue)
     osg::ref_ptr<osg::Vec2Array> osgTextures(new osg::Vec2Array);
 
     unsigned int numFaces = sizeof(face_indicies)/sizeof(face_indicies[0]);
-    for (unsigned int i = 0; i < numFaces; ++i) {
+    for (unsigned int i = 0; i < numFaces; ++i)
+    {
         osg::ref_ptr<osg::DrawElementsUInt> face(
             new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES, 0));
 
-        for (int j = 0; j < 3; ++j) {
+        for (int j = 0; j < 3; ++j)
+        {
             int vi = face_indicies[i][j];     // Vertice index
             int ni = face_indicies[i][j + 3]; // Normal index
             int ti = face_indicies[i][j + 6]; // Texture index
@@ -59396,7 +59400,7 @@ PixhawkCheetahGeode::create(float red, float green, float blue)
     geometry->setTexCoordArray(0, osgTextures.get());
 
     osg::ref_ptr<osg::Vec4Array> colors(new osg::Vec4Array);
-    colors->push_back(osg::Vec4(red, green, blue, 1.0f));
+    colors->push_back(osg::Vec4(color.redF(), color.greenF(), color.blueF(), 1.0f));
 
     geometry->setColorArray(colors.get());
     geometry->setColorBinding(osg::Geometry::BIND_OVERALL);

src/ui/map3D/PixhawkCheetahGeode.h

@@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project
 #define PIXHAWKCHEETAHGEODE_H_
 
 #include <osg/Geode>
+#include <QColor>
 
 /**
  * @brief The PixhawkCheetahGeode class.
@@ -52,7 +53,6 @@ public:
      */
     static osg::ref_ptr<osg::Geode> instance(void);
 
-private:
     /**
      * @brief Creates an OpenSceneGraph geode which renders a Pixhawk Cheetah MAV.
      * @param red Red intensity of the MAV model.
@@ -61,9 +61,10 @@ private:
      *
      * @return A smart pointer to the geode.
      **/
-    static osg::ref_ptr<osg::Geode> create(float red, float green, float blue);
+    static osg::ref_ptr<osg::Geode> create(const QColor& color);
 
-    static osg::ref_ptr<osg::Geode> _instance;
+private:
+    static osg::ref_ptr<osg::Geode> mInstance;
 };
 
 #endif

src/ui/map3D/Q3DWidget.cc

@@ -44,21 +44,12 @@ This file is part of the QGROUNDCONTROL project
 
 Q3DWidget::Q3DWidget(QWidget* parent)
     : QGLWidget(parent)
-    , root(new osg::Group())
-    , allocentricMap(new osg::Switch())
-    , rollingMap(new osg::Switch())
-    , egocentricMap(new osg::Switch())
-    , robotPosition(new osg::PositionAttitudeTransform())
-    , robotAttitude(new osg::PositionAttitudeTransform())
-    , hudGroup(new osg::Switch())
-    , hudProjectionMatrix(new osg::Projection)
-    , fps(30.0f)
-{
-    // set initial camera parameters
-    cameraParams.minZoomRange = 2.0f;
-    cameraParams.cameraFov = 30.0f;
-    cameraParams.minClipRange = 1.0f;
-    cameraParams.maxClipRange = 10000.0f;
+    , mHandleDeviceEvents(true)
+    , mRoot(new osg::Group())
+    , mHudGroup(new osg::Switch())
+    , mHudProjectionMatrix(new osg::Projection)
+    , mFps(30.0f)
+{
 #ifdef QGC_OSG_QT_ENABLED
     osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
     fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
@@ -66,13 +57,12 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
     fontImpl = 0;//new osgText::Font::Font("images/Vera.ttf");
 #endif
-    font = new osgText::Font(fontImpl);
+    mFont = new osgText::Font(fontImpl);
 
-    osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
+    mOsgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
 
     setThreadingModel(osgViewer::Viewer::SingleThreaded);
 
-    setFocusPolicy(Qt::StrongFocus);
     setMouseTracking(true);
 }
 
@@ -84,149 +74,63 @@ Q3DWidget::~Q3DWidget()
 void
 Q3DWidget::init(float fps)
 {
-    this->fps = fps;
+    mFps = fps;
 
-    getCamera()->setGraphicsContext(osgGW);
+    getCamera()->setGraphicsContext(mOsgGW);
 
     // manually specify near and far clip planes
     getCamera()->setComputeNearFarMode(osg::CullSettings::DO_NOT_COMPUTE_NEAR_FAR);
 
     setLightingMode(osg::View::SKY_LIGHT);
 
-    // set up various maps
-    // allocentric - world map
-    // rolling - map aligned to the world axes and centered on the robot
-    // egocentric - vehicle-centric map
-    root->addChild(allocentricMap);
-    allocentricMap->addChild(robotPosition);
-    robotPosition->addChild(rollingMap);
-    rollingMap->addChild(robotAttitude);
-    robotAttitude->addChild(egocentricMap);
+    setSceneData(mRoot);
 
-    setSceneData(root);
+    mWorldMap = new osg::Switch;
+    mRoot->addChild(mWorldMap);
 
     // set up HUD
-    root->addChild(createHUD());
-
-    // set up robot
-    egocentricMap->addChild(createRobot());
+    mRoot->addChild(createHUD());
 
     // set up camera control
-    cameraManipulator = new GCManipulator();
-    setCameraManipulator(cameraManipulator);
-    cameraManipulator->setMinZoomRange(cameraParams.minZoomRange);
-    cameraManipulator->setDistance(cameraParams.minZoomRange * 2.0);
+    mCameraManipulator = new GCManipulator();
+    setCameraManipulator(mCameraManipulator);
+    mCameraManipulator->setMinZoomRange(mCameraParams.minZoomRange());
+    mCameraManipulator->setDistance(mCameraParams.minZoomRange() * 2.0);
 
-    connect(&timer, SIGNAL(timeout()), this, SLOT(redraw()));
+    connect(&mTimer, SIGNAL(timeout()), this, SLOT(redraw()));
     // DO NOT START TIMER IN INITIALIZATION! IT IS STARTED IN THE SHOW EVENT
 }
 
-void Q3DWidget::showEvent(QShowEvent* event)
-{
-    // React only to internal (pre/post-display)
-    // events
-    Q_UNUSED(event)
-    timer.start(static_cast<int>(floorf(1000.0f / fps)));
-}
-
-void Q3DWidget::hideEvent(QHideEvent* event)
-{
-    // React only to internal (pre/post-display)
-    // events
-    Q_UNUSED(event)
-    timer.stop();
-}
-
-osg::ref_ptr<osg::Geode>
-Q3DWidget::createRobot(void)
-{
-    // draw x,y,z-axes
-    osg::ref_ptr<osg::Geode> geode(new osg::Geode());
-    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
-    geode->addDrawable(geometry.get());
-
-    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, 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);
-
-    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> color(new osg::Vec4Array(6));
-    (*color)[0] = redColor;
-    (*color)[1] = redColor;
-    (*color)[2] = greenColor;
-    (*color)[3] = greenColor;
-    (*color)[4] = blueColor;
-    (*color)[5] = blueColor;
-
-    geometry->setColorArray(color);
-    geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
-
-    geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 6));
-
-    osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
-    osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
-    linewidth->setWidth(3.0f);
-    stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
-    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
-    geometry->setStateSet(stateset);
-
-    return geode;
-}
-
-osg::ref_ptr<osg::Node>
-Q3DWidget::createHUD(void)
-{
-    hudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width(),
-                                   0.0, height(),
-                                   -10.0, 10.0));
-
-    osg::ref_ptr<osg::MatrixTransform> hudModelViewMatrix(
-        new osg::MatrixTransform);
-    hudModelViewMatrix->setMatrix(osg::Matrix::identity());
-    hudModelViewMatrix->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
-
-    hudProjectionMatrix->addChild(hudModelViewMatrix);
-    hudModelViewMatrix->addChild(hudGroup);
-
-    osg::ref_ptr<osg::StateSet> hudStateSet(new osg::StateSet);
-    hudGroup->setStateSet(hudStateSet);
-    hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
-    hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
-    hudStateSet->setMode(GL_BLEND, osg::StateAttribute::ON);
-    hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
-    hudStateSet->setRenderBinDetails(11, "RenderBin");
-
-    return hudProjectionMatrix;
-}
-
 void
 Q3DWidget::setCameraParams(float minZoomRange, float cameraFov,
                            float minClipRange, float maxClipRange)
 {
-    cameraParams.minZoomRange = minZoomRange;
-    cameraParams.cameraFov = cameraFov;
-    cameraParams.minClipRange = minClipRange;
-    cameraParams.maxClipRange = maxClipRange;
+    mCameraParams.minZoomRange() = minZoomRange;
+    mCameraParams.fov() = cameraFov;
+    mCameraParams.minClipRange() = minClipRange;
+    mCameraParams.maxClipRange() = maxClipRange;
 }
 
 void
 Q3DWidget::moveCamera(double dx, double dy, double dz)
 {
-    cameraManipulator->move(dx, dy, dz);
+    mCameraManipulator->move(dx, dy, dz);
 }
 
 void
 Q3DWidget::recenterCamera(double x, double y, double z)
 {
-    cameraManipulator->setCenter(osg::Vec3d(x, y, z));
+    mCameraManipulator->setCenter(osg::Vec3d(x, y, z));
+}
+
+void
+Q3DWidget::rotateCamera(double roll, double pitch, double yaw)
+{
+    osg::Quat q(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
+                pitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                roll, osg::Vec3d(0.0f, 1.0f, 0.0f));
+
+    mCameraManipulator->setRotation(q);
 }
 
 void
@@ -236,21 +140,33 @@ Q3DWidget::setDisplayMode3D(void)
                     / static_cast<double>(height());
 
     getCamera()->setViewport(new osg::Viewport(0, 0, width(), height()));
-    getCamera()->setProjectionMatrixAsPerspective(cameraParams.cameraFov,
-            aspect,
-            cameraParams.minClipRange,
-            cameraParams.maxClipRange);
+    getCamera()->setProjectionMatrixAsPerspective(mCameraParams.fov(),
+                                                  aspect,
+                                                  mCameraParams.minClipRange(),
+                                                  mCameraParams.maxClipRange());
+}
+
+osg::ref_ptr<osg::Switch>&
+Q3DWidget::hudGroup(void)
+{
+    return mHudGroup;
+}
+
+QPoint
+Q3DWidget::mouseCursorCoords(void)
+{
+    return mapFromGlobal(cursor().pos());
 }
 
-std::pair<double,double>
-Q3DWidget::getGlobalCursorPosition(int32_t cursorX, int32_t cursorY, double z)
+QPointF
+Q3DWidget::worldCursorPosition(const QPoint& cursorPos, double worldZ) const
 {
     osgUtil::LineSegmentIntersector::Intersections intersections;
 
     // normalize cursor position to value between -1 and 1
-    double x = -1.0f + static_cast<double>(2 * cursorX)
+    double x = -1.0f + static_cast<double>(2 * cursorPos.x())
                / static_cast<double>(width());
-    double y = -1.0f + static_cast<double>(2 * (height() - cursorY))
+    double y = -1.0f + static_cast<double>(2 * (height() - cursorPos.y()))
                / static_cast<double>(height());
 
     // compute matrix which transforms screen coordinates to world coordinates
@@ -264,103 +180,109 @@ Q3DWidget::getGlobalCursorPosition(int32_t cursorX, int32_t cursorY, double z)
     osg::ref_ptr<osg::LineSegment> line =
         new osg::LineSegment(nearPoint, farPoint);
 
-    osg::Plane p(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, z));
+    osg::Plane p(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, worldZ));
 
     osg::Vec3d projectedPoint;
-    getPlaneLineIntersection(p.asVec4(), *line, projectedPoint);
+    planeLineIntersection(p.asVec4(), *line, projectedPoint);
 
-    return std::make_pair(projectedPoint.y(), projectedPoint.x());
+    return QPointF(projectedPoint.y(), projectedPoint.x());
 }
 
-void
-Q3DWidget::redraw(void)
+CameraParams&
+Q3DWidget::cameraParams(void)
 {
-#if (QGC_EVENTLOOP_DEBUG)
-    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
-#endif
-    updateGL();
+    return mCameraParams;
 }
 
-int
-Q3DWidget::getMouseX(void)
+osg::ref_ptr<GCManipulator>&
+Q3DWidget::cameraManipulator(void)
 {
-    return mapFromGlobal(cursor().pos()).x();
+    return mCameraManipulator;
 }
 
-int
-Q3DWidget::getMouseY(void)
+osg::ref_ptr<osgText::Font>&
+Q3DWidget::font(void)
 {
-    return mapFromGlobal(cursor().pos()).y();
+    return mFont;
 }
 
-void
-Q3DWidget::resizeGL(int width, int height)
+osg::ref_ptr<osg::Switch>&
+Q3DWidget::worldMap(void)
 {
-    hudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width,
-                                   0.0, height,
-                                   -10.0, 10.0));
-
-    osgGW->getEventQueue()->windowResize(0, 0, width, height);
-    osgGW->resized(0 , 0, width, height);
+    return mWorldMap;
 }
 
-void
-Q3DWidget::paintGL(void)
+osg::ref_ptr<SystemGroupNode>&
+Q3DWidget::systemGroup(int systemId)
 {
-    setDisplayMode3D();
+    if (!mSystemGroups.contains(systemId))
+    {
+        osg::ref_ptr<SystemGroupNode> newSystem = new SystemGroupNode;
+        mRoot->addChild(newSystem);
 
-    getCamera()->setClearColor(osg::Vec4f(0.0f, 0.0f, 0.0f, 0.0f));
-    getCamera()->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-    display();
+        mSystemGroups.insert(systemId, newSystem);
+    }
 
-    frame();
+    return mSystemGroups[systemId];
 }
 
-void
-Q3DWidget::display(void)
+bool&
+Q3DWidget::handleDeviceEvents(void)
 {
-
+    return mHandleDeviceEvents;
 }
 
 void
-Q3DWidget::keyPressEvent(QKeyEvent* event)
+Q3DWidget::handleKeyPressEvent(QKeyEvent* event)
 {
-    QWidget::keyPressEvent(event);
-    if (event->isAccepted()) {
+    if (event->isAccepted())
+    {
         return;
     }
 
-    if (event->text().isEmpty()) {
-        osgGW->getEventQueue()->keyPress(convertKey(event->key()));
-    } else {
-        osgGW->getEventQueue()->keyPress(
+    if (event->text().isEmpty())
+    {
+        mOsgGW->getEventQueue()->keyPress(convertKey(event->key()));
+    }
+    else
+    {
+        mOsgGW->getEventQueue()->keyPress(
             static_cast<osgGA::GUIEventAdapter::KeySymbol>(
                 *(event->text().toAscii().data())));
     }
 }
 
 void
-Q3DWidget::keyReleaseEvent(QKeyEvent* event)
+Q3DWidget::handleKeyReleaseEvent(QKeyEvent* event)
 {
-    QWidget::keyReleaseEvent(event);
-    if (event->isAccepted()) {
+    if (event->isAccepted())
+    {
         return;
     }
-    if (event->text().isEmpty()) {
-        osgGW->getEventQueue()->keyRelease(convertKey(event->key()));
-    } else {
-        osgGW->getEventQueue()->keyRelease(
+
+    if (event->text().isEmpty())
+    {
+        mOsgGW->getEventQueue()->keyRelease(convertKey(event->key()));
+    }
+    else
+    {
+        mOsgGW->getEventQueue()->keyRelease(
             static_cast<osgGA::GUIEventAdapter::KeySymbol>(
                 *(event->text().toAscii().data())));
     }
 }
 
 void
-Q3DWidget::mousePressEvent(QMouseEvent* event)
+Q3DWidget::handleMousePressEvent(QMouseEvent* event)
 {
+    if (event->isAccepted())
+    {
+        return;
+    }
+
     int button = 0;
-    switch (event->button()) {
+    switch (event->button())
+    {
     case Qt::LeftButton:
         button = 1;
         break;
@@ -374,16 +296,22 @@ Q3DWidget::mousePressEvent(QMouseEvent* event)
         button = 0;
         break;
     default:
-    {}
+        {}
     }
-    osgGW->getEventQueue()->mouseButtonPress(event->x(), event->y(), button);
+    mOsgGW->getEventQueue()->mouseButtonPress(event->x(), event->y(), button);
 }
 
 void
-Q3DWidget::mouseReleaseEvent(QMouseEvent* event)
+Q3DWidget::handleMouseReleaseEvent(QMouseEvent* event)
 {
+    if (event->isAccepted())
+    {
+        return;
+    }
+
     int button = 0;
-    switch (event->button()) {
+    switch (event->button())
+    {
     case Qt::LeftButton:
         button = 1;
         break;
@@ -397,29 +325,208 @@ Q3DWidget::mouseReleaseEvent(QMouseEvent* event)
         button = 0;
         break;
     default:
-    {}
+        {}
+    }
+    mOsgGW->getEventQueue()->mouseButtonRelease(event->x(), event->y(), button);
+}
+
+void
+Q3DWidget::handleMouseMoveEvent(QMouseEvent* event)
+{
+    if (event->isAccepted())
+    {
+        return;
+    }
+
+    mOsgGW->getEventQueue()->mouseMotion(event->x(), event->y());
+}
+
+void
+Q3DWidget::handleWheelEvent(QWheelEvent* event)
+{
+    if (event->isAccepted())
+    {
+        return;
+    }
+
+    mOsgGW->getEventQueue()->mouseScroll((event->delta() > 0) ?
+                                         osgGA::GUIEventAdapter::SCROLL_UP :
+                                         osgGA::GUIEventAdapter::SCROLL_DOWN);
+}
+
+void
+Q3DWidget::redraw(void)
+{
+    emit update();
+#if (QGC_EVENTLOOP_DEBUG)
+    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
+#endif
+    updateGL();
+}
+
+void
+Q3DWidget::resizeGL(int width, int height)
+{
+    mHudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width,
+                                    0.0, height,
+                                    -10.0, 10.0));
+
+    mOsgGW->getEventQueue()->windowResize(0, 0, width, height);
+    mOsgGW->resized(0 , 0, width, height);
+
+    emit sizeChanged(width, height);
+}
+
+void
+Q3DWidget::keyPressEvent(QKeyEvent* event)
+{
+    QWidget::keyPressEvent(event);
+
+    if (mHandleDeviceEvents)
+    {
+        handleKeyPressEvent(event);
+    }
+    else
+    {
+        event->ignore();
+    }
+}
+
+void
+Q3DWidget::keyReleaseEvent(QKeyEvent* event)
+{
+    QWidget::keyReleaseEvent(event);
+
+    if (mHandleDeviceEvents)
+    {
+        handleKeyReleaseEvent(event);
+    }
+    else
+    {
+        event->ignore();
+    }
+}
+
+void
+Q3DWidget::mousePressEvent(QMouseEvent* event)
+{
+    QWidget::mousePressEvent(event);
+
+    if (mHandleDeviceEvents)
+    {
+        handleMousePressEvent(event);
+    }
+    else
+    {
+        event->ignore();
+    }
+}
+
+void
+Q3DWidget::mouseReleaseEvent(QMouseEvent* event)
+{
+    QWidget::mouseReleaseEvent(event);
+
+    if (mHandleDeviceEvents)
+    {
+        handleMouseReleaseEvent(event);
+    }
+    else
+    {
+        event->ignore();
     }
-    osgGW->getEventQueue()->mouseButtonRelease(event->x(), event->y(), button);
 }
 
 void
 Q3DWidget::mouseMoveEvent(QMouseEvent* event)
 {
-    osgGW->getEventQueue()->mouseMotion(event->x(), event->y());
+    QWidget::mouseMoveEvent(event);
+
+    if (mHandleDeviceEvents)
+    {
+        handleMouseMoveEvent(event);
+    }
+    else
+    {
+        event->ignore();
+    }
 }
 
 void
 Q3DWidget::wheelEvent(QWheelEvent* event)
 {
-    osgGW->getEventQueue()->mouseScroll((event->delta() > 0) ?
-                                        osgGA::GUIEventAdapter::SCROLL_UP :
-                                        osgGA::GUIEventAdapter::SCROLL_DOWN);
+    QWidget::wheelEvent(event);
+
+    if (mHandleDeviceEvents)
+    {
+        handleWheelEvent(event);
+    }
+    else
+    {
+        event->ignore();
+    }
+}
+
+void
+Q3DWidget::showEvent(QShowEvent* event)
+{
+    // React only to internal (pre/post-display)
+    // events
+    Q_UNUSED(event)
+    mTimer.start(static_cast<int>(floorf(1000.0f / mFps)));
+}
+
+void
+Q3DWidget::hideEvent(QHideEvent* event)
+{
+    // React only to internal (pre/post-display)
+    // events
+    Q_UNUSED(event)
+    mTimer.stop();
+}
+
+osg::ref_ptr<osg::Node>
+Q3DWidget::createHUD(void)
+{
+    mHudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width(),
+                                    0.0, height(),
+                                    -10.0, 10.0));
+
+    osg::ref_ptr<osg::MatrixTransform> hudModelViewMatrix(
+        new osg::MatrixTransform);
+    hudModelViewMatrix->setMatrix(osg::Matrix::identity());
+    hudModelViewMatrix->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+
+    mHudProjectionMatrix->addChild(hudModelViewMatrix);
+    hudModelViewMatrix->addChild(mHudGroup);
+
+    osg::ref_ptr<osg::StateSet> hudStateSet(new osg::StateSet);
+    mHudGroup->setStateSet(hudStateSet);
+    hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
+    hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    hudStateSet->setMode(GL_BLEND, osg::StateAttribute::ON);
+    hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
+    hudStateSet->setRenderBinDetails(11, "RenderBin");
+
+    return mHudProjectionMatrix;
+}
+
+void
+Q3DWidget::paintGL(void)
+{
+    setDisplayMode3D();
+
+    getCamera()->setClearColor(osg::Vec4f(0.0f, 0.0f, 0.0f, 0.0f));
+    getCamera()->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+    frame();
 }
 
 osgGA::GUIEventAdapter::KeySymbol
 Q3DWidget::convertKey(int key) const
 {
-    switch (key) {
+    switch (key)
+    {
     case Qt::Key_Space :
         return osgGA::GUIEventAdapter::KEY_Space;
     case Qt::Key_Backspace :
@@ -606,9 +713,9 @@ Q3DWidget::convertKey(int key) const
 }
 
 bool
-Q3DWidget::getPlaneLineIntersection(const osg::Vec4d& plane,
-                                    const osg::LineSegment& line,
-                                    osg::Vec3d& isect)
+Q3DWidget::planeLineIntersection(const osg::Vec4d& plane,
+                                 const osg::LineSegment& line,
+                                 osg::Vec3d& isect) const
 {
     osg::Vec3d lineStart = line.start();
     osg::Vec3d lineEnd = line.end();
@@ -620,7 +727,8 @@ Q3DWidget::getPlaneLineIntersection(const osg::Vec4d& plane,
     const double denominator = plane[0] * deltaX
                                + plane[1] * deltaY
                                + plane[2] * deltaZ;
-    if (!denominator) {
+    if (!denominator)
+    {
         return false;
     }
 

src/ui/map3D/Q3DWidget.h

@@ -41,7 +41,9 @@ This file is part of the QGROUNDCONTROL project
 #include <osgText/Font>
 #include <osgViewer/Viewer>
 
+#include "CameraParams.h"
 #include "GCManipulator.h"
+#include "SystemGroupNode.h"
 
 /**
  * @brief Definition of the class Q3DWidget.
@@ -96,11 +98,24 @@ public:
      */
     void recenterCamera(double x, double y, double z);
 
+    /**
+      * @brief Rotates the camera.
+      */
+    void rotateCamera(double roll, double pitch, double yaw);
+
     /**
      * @brief Sets up 3D display mode.
      */
     void setDisplayMode3D(void);
 
+    osg::ref_ptr<osg::Switch>& hudGroup(void);
+
+    /**
+     * @brief Get screen coordinates of mouse cursor.
+     * @return screen coordinates of mouse cursor.
+     */
+    QPoint mouseCursorCoords(void);
+
     /**
      * @brief Gets the world 3D coordinates of the cursor.
      * The function projects the 2D cursor position to a line in world space
@@ -108,50 +123,45 @@ public:
      * which contains the point (0, 0, z);
      * @param cursorX x-coordinate of the cursor.
      * @param cursorY y-coordinate of the cursor.
-     * @param z z-coordinate of the point in the plane.
-     * @return A pair of values containing the world 3D cursor coordinates.
+     * @param worldZ z-coordinate of the point in the plane.
+     * @return World (x,y) cursor coordinates.
      */
-    std::pair<double,double> getGlobalCursorPosition(int32_t cursorX,
-            int32_t cursorY,
-            double z);
+     QPointF worldCursorPosition(const QPoint& cursorPos, double worldZ) const;
 
-protected slots:
-    /**
-     * @brief Updates the widget.
-     */
-    void redraw(void);
+     CameraParams& cameraParams(void);
 
-protected:
+     osg::ref_ptr<GCManipulator>& cameraManipulator(void);
 
-    /** @brief Start widget updating */
-    void showEvent(QShowEvent* event);
-    /** @brief Stop widget updating */
-    void hideEvent(QHideEvent* event);
+     osg::ref_ptr<osgText::Font>& font(void);
 
-    /**
-     * @brief Get base robot geode.
-     * @return Smart pointer to the geode.
-     */
-    osg::ref_ptr<osg::Geode> createRobot(void);
+     osg::ref_ptr<osg::Switch>& worldMap(void);
+     osg::ref_ptr<SystemGroupNode>& systemGroup(int systemId);
 
-    /**
-     * @brief Get base HUD geode.
-     * @return Smart pointer to the geode.
-     */
-    osg::ref_ptr<osg::Node> createHUD(void);
+     bool& handleDeviceEvents(void);
 
-    /**
-     * @brief Get screen x-coordinate of mouse cursor.
-     * @return screen x-coordinate of mouse cursor.
-     */
-    int getMouseX(void);
+     void handleKeyPressEvent(QKeyEvent* event);
+
+     void handleKeyReleaseEvent(QKeyEvent* event);
+
+     void handleMousePressEvent(QMouseEvent* event);
+
+     void handleMouseReleaseEvent(QMouseEvent* event);
+
+     void handleMouseMoveEvent(QMouseEvent* event);
 
+     void handleWheelEvent(QWheelEvent* event);
+
+protected slots:
     /**
-     * @brief Get screen y-coordinate of mouse cursor.
-     * @return screen y-coordinate of mouse cursor.
+     * @brief Updates the widget.
      */
-    int getMouseY(void);
+    void redraw(void);
 
+signals:
+    void sizeChanged(int width, int height);
+    void update(void);
+
+protected:
     /**
      * @brief Handle widget resize event.
      * @param width New width of widget.
@@ -159,65 +169,58 @@ protected:
      */
     virtual void resizeGL(int width, int height);
 
-    /**
-     * @brief Handle widget paint event.
-     */
-    virtual void paintGL(void);
-
-    /**
-     * @brief This function is a container for user-defined rendering.
-     * All code to render objects should be in this function.
-     */
-    virtual void display(void);
-
     /**
      * @brief Processes key press events.
-     * If this handler is reimplemented, it is very important that you call the
-     * base class implementation.
      * @param event Key press event.
      */
     virtual void keyPressEvent(QKeyEvent* event);
 
     /**
      * @brief Processes key release events.
-     * If this handler is reimplemented, it is very important that you call the
-     * base class implementation.
      * @param event Key release event.
      */
     virtual void keyReleaseEvent(QKeyEvent* event);
 
     /**
      * @brief Processes mouse press events.
-     * If this handler is reimplemented, it is very important that you call the
-     * base class implementation.
      * @param event Mouse press event.
      */
     virtual void mousePressEvent(QMouseEvent* event);
 
     /**
      * @brief Processes mouse release events.
-     * If this handler is reimplemented, it is very important that you call the
-     * base class implementation.
      * @param event Mouse release event.
      */
     virtual void mouseReleaseEvent(QMouseEvent* event);
 
     /**
      * @brief Processes mouse move events.
-     * If this handler is reimplemented, it is very important that you call the
-     * base class implementation.
      * @param event Mouse move event.
      */
     virtual void mouseMoveEvent(QMouseEvent* event);
 
     /**
      * @brief Processes mouse wheel events.
-     * If this handler is reimplemented, it is very important that you call the
-     * base class implementation.
      * @param event Mouse wheel event.
      */
     virtual void wheelEvent(QWheelEvent* event);
 
+    /** @brief Start widget updating */
+    void showEvent(QShowEvent* event);
+    /** @brief Stop widget updating */
+    void hideEvent(QHideEvent* event);
+
+    /**
+     * @brief Get base HUD geode.
+     * @return Smart pointer to the geode.
+     */
+    osg::ref_ptr<osg::Node> createHUD(void);
+
+    /**
+     * @brief Handle widget paint event.
+     */
+    virtual void paintGL(void);
+
     /**
      * @brief Converts Qt-defined key to OSG-defined key.
      * @param key Qt-defined key.
@@ -231,37 +234,29 @@ protected:
      * @param line Line representation.
      * @return 3D point which lies at the intersection of the line and plane.
      */
-    bool getPlaneLineIntersection(const osg::Vec4d& plane,
-                                  const osg::LineSegment& line,
-                                  osg::Vec3d& isect);
+    bool planeLineIntersection(const osg::Vec4d& plane,
+                               const osg::LineSegment& line,
+                               osg::Vec3d& isect) const;
 
-    osg::ref_ptr<osg::Group> root; /**< Root node of scene graph. */
-    osg::ref_ptr<osg::Switch> allocentricMap;
-    osg::ref_ptr<osg::Switch> rollingMap;
-    osg::ref_ptr<osg::Switch> egocentricMap;
-    osg::ref_ptr<osg::PositionAttitudeTransform> robotPosition;
-    osg::ref_ptr<osg::PositionAttitudeTransform> robotAttitude;
+    bool mHandleDeviceEvents;
 
-    osg::ref_ptr<osg::Switch> hudGroup; /**< A group which contains renderable HUD objects. */
-    osg::ref_ptr<osg::Projection> hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */
+    osg::ref_ptr<osg::Group> mRoot; /**< Root node of scene graph. */
+    osg::ref_ptr<osg::Switch> mWorldMap;
+    QMap<int, osg::ref_ptr<SystemGroupNode> > mSystemGroups;
 
-    osg::ref_ptr<osgViewer::GraphicsWindowEmbedded> osgGW; /**< A class which manages OSG graphics windows and events. */
+    osg::ref_ptr<osg::Switch> mHudGroup; /**< A group which contains renderable HUD objects. */
+    osg::ref_ptr<osg::Projection> mHudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */
 
-    osg::ref_ptr<GCManipulator> cameraManipulator; /**< Camera manipulator. */
+    osg::ref_ptr<osgViewer::GraphicsWindowEmbedded> mOsgGW; /**< A class which manages OSG graphics windows and events. */
 
-    QTimer timer; /**< Timer which draws graphics based on specified fps. */
+    osg::ref_ptr<GCManipulator> mCameraManipulator; /**< Camera manipulator. */
 
-    struct CameraParams {
-        float minZoomRange;
-        float cameraFov;
-        float minClipRange;
-        float maxClipRange;
-    };
+    QTimer mTimer; /**< Timer which draws graphics based on specified fps. */
 
-    CameraParams cameraParams; /**< Struct representing camera parameters. */
-    float fps;
+    CameraParams mCameraParams; /**< Struct representing camera parameters. */
+    float mFps;
 
-    osg::ref_ptr<osgText::Font> font;
+    osg::ref_ptr<osgText::Font> mFont;
 };
 
 #endif // Q3DWIDGET_H

src/ui/map3D/Q3DWidgetFactory.cc

@@ -37,10 +37,10 @@ This file is part of the QGROUNDCONTROL project
 #endif
 
 QPointer<QWidget>
-Q3DWidgetFactory::get(const std::string& type)
+Q3DWidgetFactory::get(const std::string& type, QWidget* parent)
 {
     if (type == "PIXHAWK") {
-        return QPointer<QWidget>(new Pixhawk3DWidget());
+        return QPointer<QWidget>(new Pixhawk3DWidget(parent));
     }
 #ifdef QGC_OSGEARTH_ENABLED
     else if (type == "MAP3D") {
@@ -48,6 +48,6 @@ Q3DWidgetFactory::get(const std::string& type)
     }
 #endif
     else {
-        return QPointer<QWidget>(new Q3DWidget());
+        return QPointer<QWidget>(new Q3DWidget(parent));
     }
 }

src/ui/map3D/Q3DWidgetFactory.h

@@ -50,7 +50,7 @@ public:
      * @return A smart pointer to the Q3DWidget instance.
      */
 
-    static QPointer<QWidget> get(const std::string& type);
+    static QPointer<QWidget> get(const std::string& type, QWidget* parent);
 };
 
 #endif // Q3DWIDGETFACTORY_H

src/ui/map3D/SystemContainer.cc

+#include "SystemContainer.h"
+
+#include <osg/PositionAttitudeTransform>
+
+SystemContainer::SystemContainer()
+{
+
+}
+
+QVector4D&
+SystemContainer::target(void)
+{
+    return mTarget;
+}
+
+QVector< osg::ref_ptr<osg::Node> >&
+SystemContainer::models(void)
+{
+    return mModels;
+}
+
+QMap<int, QVector<osg::Vec3d> >&
+SystemContainer::trailMap(void)
+{
+    return mTrailMap;
+}
+
+QMap<int, int>&
+SystemContainer::trailIndexMap(void)
+{
+    return mTrailIndexMap;
+}
+
+osg::ref_ptr<ImageWindowGeode>&
+SystemContainer::depthImageNode(void)
+{
+    return mDepthImageNode;
+}
+
+osg::ref_ptr<osg::Geode>&
+SystemContainer::localGridNode(void)
+{
+    return mLocalGridNode;
+}
+
+osg::ref_ptr<osg::Node>&
+SystemContainer::modelNode(void)
+{
+    return mModelNode;
+}
+
+osg::ref_ptr<osg::Geode>&
+SystemContainer::pointCloudNode(void)
+{
+    return mPointCloudNode;
+}
+
+osg::ref_ptr<ImageWindowGeode>&
+SystemContainer::rgbImageNode(void)
+{
+    return mRGBImageNode;
+}
+
+osg::ref_ptr<osg::Node>&
+SystemContainer::targetNode(void)
+{
+    return mTargetNode;
+}
+
+osg::ref_ptr<osg::Geode>&
+SystemContainer::trailNode(void)
+{
+    return mTrailNode;
+}
+
+osg::ref_ptr<WaypointGroupNode>&
+SystemContainer::waypointGroupNode(void)
+{
+    return mWaypointGroupNode;
+}
+
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+
+osg::ref_ptr<ObstacleGroupNode>&
+SystemContainer::obstacleGroupNode(void)
+{
+    return mObstacleGroupNode;
+}
+
+osg::ref_ptr<osg::Geode>&
+SystemContainer::plannedPathNode(void)
+{
+    return mPlannedPathNode;
+}
+
+#endif

src/ui/map3D/SystemContainer.h

+#ifndef SYSTEMCONTAINER_H
+#define SYSTEMCONTAINER_H
+
+#include <osg/Geode>
+#include <QMap>
+#include <QVarLengthArray>
+#include <QVector4D>
+
+#include "ImageWindowGeode.h"
+#include "WaypointGroupNode.h"
+
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+#include "ObstacleGroupNode.h"
+#endif
+
+class SystemContainer
+{
+public:
+    SystemContainer();
+
+    QVector4D& target(void);
+
+    QVector< osg::ref_ptr<osg::Node> >& models(void);
+
+    QMap<int, QVector<osg::Vec3d> >& trailMap(void);
+    QMap<int, int>& trailIndexMap(void);
+
+    osg::ref_ptr<ImageWindowGeode>& depthImageNode(void);
+    osg::ref_ptr<osg::Geode>& localGridNode(void);
+    osg::ref_ptr<osg::Node>& modelNode(void);
+    osg::ref_ptr<osg::Geode>& pointCloudNode(void);
+    osg::ref_ptr<ImageWindowGeode>& rgbImageNode(void);
+    osg::ref_ptr<osg::Node>& targetNode(void);
+    osg::ref_ptr<osg::Geode>& trailNode(void);
+    osg::ref_ptr<WaypointGroupNode>& waypointGroupNode(void);
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+    osg::ref_ptr<ObstacleGroupNode>& obstacleGroupNode(void);
+    osg::ref_ptr<osg::Geode>& plannedPathNode(void);
+#endif
+
+private:
+    QVector4D mTarget;
+
+    QVector< osg::ref_ptr<osg::Node> > mModels;
+
+    // map component id to component-specific trail
+    QMap<int, QVector<osg::Vec3d> > mTrailMap;
+    QMap<int, int> mTrailIndexMap;
+
+    // osg structures
+    osg::ref_ptr<ImageWindowGeode> mDepthImageNode;
+    osg::ref_ptr<osg::Geode> mLocalGridNode;
+    osg::ref_ptr<osg::Node> mModelNode;
+    osg::ref_ptr<osg::Geode> mPointCloudNode;
+    osg::ref_ptr<ImageWindowGeode> mRGBImageNode;
+    osg::ref_ptr<osg::Node> mTargetNode;
+    osg::ref_ptr<osg::Geode> mTrailNode;
+    osg::ref_ptr<WaypointGroupNode> mWaypointGroupNode;
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+    osg::ref_ptr<ObstacleGroupNode> mObstacleGroupNode;
+    osg::ref_ptr<osg::Geode> mPlannedPathNode;
+#endif
+};
+
+#endif // SYSTEMCONTAINER_H

src/ui/map3D/SystemGroupNode.cc

+#include "SystemGroupNode.h"
+
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/LineWidth>
+
+SystemGroupNode::SystemGroupNode()
+ : mAllocentricMap(new osg::Switch())
+ , mRollingMap(new osg::Switch())
+ , mEgocentricMap(new osg::Switch())
+ , mPosition(new osg::PositionAttitudeTransform())
+ , mAttitude(new osg::PositionAttitudeTransform())
+{
+    addChild(mAllocentricMap);
+
+    // set up various maps
+    // allocentric - world map
+    // rolling - map aligned to the world axes and centered on the robot
+    // egocentric - vehicle-centric map
+    mAllocentricMap->addChild(mPosition);
+    mPosition->addChild(mRollingMap);
+    mRollingMap->addChild(mAttitude);
+    mAttitude->addChild(mEgocentricMap);
+
+    // set up robot
+    mEgocentricMap->addChild(createAxisGeode());
+}
+
+osg::ref_ptr<osg::Switch>&
+SystemGroupNode::allocentricMap(void)
+{
+    return mAllocentricMap;
+}
+
+osg::ref_ptr<osg::Switch>&
+SystemGroupNode::rollingMap(void)
+{
+    return mRollingMap;
+}
+
+osg::ref_ptr<osg::Switch>&
+SystemGroupNode::egocentricMap(void)
+{
+    return mEgocentricMap;
+}
+
+osg::ref_ptr<osg::PositionAttitudeTransform>&
+SystemGroupNode::position(void)
+{
+    return mPosition;
+}
+
+osg::ref_ptr<osg::PositionAttitudeTransform>&
+SystemGroupNode::attitude(void)
+{
+    return mAttitude;
+}
+
+osg::ref_ptr<osg::Node>
+SystemGroupNode::createAxisGeode(void)
+{
+    // draw x,y,z-axes
+    osg::ref_ptr<osg::Geode> geode(new osg::Geode());
+    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
+    geode->addDrawable(geometry.get());
+
+    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, 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);
+
+    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> color(new osg::Vec4Array(6));
+    (*color)[0] = redColor;
+    (*color)[1] = redColor;
+    (*color)[2] = greenColor;
+    (*color)[3] = greenColor;
+    (*color)[4] = blueColor;
+    (*color)[5] = blueColor;
+
+    geometry->setColorArray(color);
+    geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
+
+    geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 6));
+
+    osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
+    osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
+    linewidth->setWidth(3.0f);
+    stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
+    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    geometry->setStateSet(stateset);
+
+    return geode;
+}

src/ui/map3D/SystemGroupNode.h

+#ifndef SYSTEMGROUPNODE_H
+#define SYSTEMGROUPNODE_H
+
+#include <osg/PositionAttitudeTransform>
+#include <osg/Switch>
+
+class SystemGroupNode : public osg::Group
+{
+public:
+    SystemGroupNode();
+
+    osg::ref_ptr<osg::Switch>& allocentricMap(void);
+    osg::ref_ptr<osg::Switch>& rollingMap(void);
+    osg::ref_ptr<osg::Switch>& egocentricMap(void);
+    osg::ref_ptr<osg::PositionAttitudeTransform>& position(void);
+    osg::ref_ptr<osg::PositionAttitudeTransform>& attitude(void);
+
+private:
+    osg::ref_ptr<osg::Node> createAxisGeode(void);
+
+    osg::ref_ptr<osg::Switch> mAllocentricMap;
+    osg::ref_ptr<osg::Switch> mRollingMap;
+    osg::ref_ptr<osg::Switch> mEgocentricMap;
+    osg::ref_ptr<osg::PositionAttitudeTransform> mPosition;
+    osg::ref_ptr<osg::PositionAttitudeTransform> mAttitude;
+};
+
+#endif // SYSTEMGROUPNODE_H

src/ui/map3D/SystemViewParams.cc

+#include "SystemViewParams.h"
+
+SystemViewParams::SystemViewParams(int systemId)
+ : mSystemId(systemId)
+ , mColorPointCloudByDistance(false)
+ , mDisplayLocalGrid(false)
+ , mDisplayObstacleList(true)
+ , mDisplayPlannedPath(true)
+ , mDisplayPointCloud(true)
+ , mDisplayRGBD(false)
+ , mDisplayTarget(true)
+ , mDisplayTrails(true)
+ , mDisplayWaypoints(true)
+ , mModelIndex(-1)
+{
+
+}
+
+bool&
+SystemViewParams::colorPointCloudByDistance(void)
+{
+    return mColorPointCloudByDistance;
+}
+
+bool
+SystemViewParams::colorPointCloudByDistance(void) const
+{
+    return mColorPointCloudByDistance;
+}
+
+bool&
+SystemViewParams::displayLocalGrid(void)
+{
+    return mDisplayLocalGrid;
+}
+
+bool
+SystemViewParams::displayLocalGrid(void) const
+{
+    return mDisplayLocalGrid;
+}
+
+bool&
+SystemViewParams::displayObstacleList(void)
+{
+    return mDisplayObstacleList;
+}
+
+bool
+SystemViewParams::displayObstacleList(void) const
+{
+    return mDisplayObstacleList;
+}
+
+bool&
+SystemViewParams::displayPlannedPath(void)
+{
+    return mDisplayPlannedPath;
+}
+
+bool
+SystemViewParams::displayPlannedPath(void) const
+{
+    return mDisplayPlannedPath;
+}
+
+bool&
+SystemViewParams::displayPointCloud(void)
+{
+    return mDisplayPointCloud;
+}
+
+bool
+SystemViewParams::displayPointCloud(void) const
+{
+    return mDisplayPointCloud;
+}
+
+bool&
+SystemViewParams::displayRGBD(void)
+{
+    return mDisplayRGBD;
+}
+
+bool
+SystemViewParams::displayRGBD(void) const
+{
+    return mDisplayRGBD;
+}
+
+bool&
+SystemViewParams::displayTarget(void)
+{
+    return mDisplayTarget;
+}
+
+bool
+SystemViewParams::displayTarget(void) const
+{
+    return mDisplayTarget;
+}
+
+bool&
+SystemViewParams::displayTrails(void)
+{
+    return mDisplayTrails;
+}
+
+bool
+SystemViewParams::displayTrails(void) const
+{
+    return mDisplayTrails;
+}
+
+bool&
+SystemViewParams::displayWaypoints(void)
+{
+    return mDisplayWaypoints;
+}
+
+bool
+SystemViewParams::displayWaypoints(void) const
+{
+    return mDisplayWaypoints;
+}
+
+int&
+SystemViewParams::modelIndex(void)
+{
+    return mModelIndex;
+}
+
+int
+SystemViewParams::modelIndex(void) const
+{
+    return mModelIndex;
+}
+
+QVector<QString>&
+SystemViewParams::modelNames(void)
+{
+    return mModelNames;
+}
+
+const QVector<QString>&
+SystemViewParams::modelNames(void) const
+{
+    return mModelNames;
+}
+
+void
+SystemViewParams::modelChanged(int index)
+{
+    mModelIndex = index;
+
+    emit modelChangedSignal(mSystemId, index);
+}
+
+void
+SystemViewParams::toggleColorPointCloud(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mColorPointCloudByDistance = true;
+    }
+    else
+    {
+        mColorPointCloudByDistance = false;
+    }
+}
+
+void
+SystemViewParams::toggleLocalGrid(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayLocalGrid = true;
+    }
+    else
+    {
+        mDisplayLocalGrid = false;
+    }
+}
+
+void
+SystemViewParams::toggleObstacleList(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayObstacleList = true;
+    }
+    else
+    {
+        mDisplayObstacleList = false;
+    }
+}
+
+void
+SystemViewParams::togglePlannedPath(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayPlannedPath = true;
+    }
+    else
+    {
+        mDisplayPlannedPath = false;
+    }
+}
+
+void
+SystemViewParams::togglePointCloud(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayPointCloud = true;
+    }
+    else
+    {
+        mDisplayPointCloud = false;
+    }
+}
+
+void
+SystemViewParams::toggleRGBD(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayRGBD = true;
+    }
+    else
+    {
+        mDisplayRGBD = false;
+    }
+}
+
+void
+SystemViewParams::toggleTarget(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayTarget = true;
+    }
+    else
+    {
+        mDisplayTarget = false;
+    }
+}
+
+void
+SystemViewParams::toggleTrails(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayTrails = true;
+    }
+    else
+    {
+        mDisplayTrails = false;
+    }
+}
+
+void
+SystemViewParams::toggleWaypoints(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplayWaypoints = true;
+    }
+    else
+    {
+        mDisplayWaypoints = false;
+    }
+}

src/ui/map3D/SystemViewParams.h

+#ifndef SYSTEMVIEWPARAMS_H
+#define SYSTEMVIEWPARAMS_H
+
+#include <QObject>
+#include <QSharedPointer>
+#include <QVector>
+
+class SystemViewParams : public QObject
+{
+    Q_OBJECT
+
+public:
+    SystemViewParams(int systemId);
+
+    bool& colorPointCloudByDistance(void);
+    bool colorPointCloudByDistance(void) const;
+
+    bool& displayLocalGrid(void);
+    bool displayLocalGrid(void) const;
+
+    bool& displayObstacleList(void);
+    bool displayObstacleList(void) const;
+
+    bool& displayPlannedPath(void);
+    bool displayPlannedPath(void) const;
+
+    bool& displayPointCloud(void);
+    bool displayPointCloud(void) const;
+
+    bool& displayRGBD(void);
+    bool displayRGBD(void) const;
+
+    bool& displayTarget(void);
+    bool displayTarget(void) const;
+
+    bool& displayTrails(void);
+    bool displayTrails(void) const;
+
+    bool& displayWaypoints(void);
+    bool displayWaypoints(void) const;
+
+    int& modelIndex(void);
+    int modelIndex(void) const;
+
+    QVector<QString>& modelNames(void);
+    const QVector<QString>& modelNames(void) const;
+
+public slots:
+    void modelChanged(int index);
+    void toggleColorPointCloud(int state);
+    void toggleLocalGrid(int state);
+    void toggleObstacleList(int state);
+    void togglePlannedPath(int state);
+    void togglePointCloud(int state);
+    void toggleRGBD(int state);
+    void toggleTarget(int state);
+    void toggleTrails(int state);
+    void toggleWaypoints(int state);
+
+signals:
+    void modelChangedSignal(int systemId, int index);
+
+private:
+    int mSystemId;
+
+    bool mColorPointCloudByDistance;
+    bool mDisplayLocalGrid;
+    bool mDisplayObstacleList;
+    bool mDisplayPlannedPath;
+    bool mDisplayPointCloud;
+    bool mDisplayRGBD;
+    bool mDisplayTarget;
+    bool mDisplayTrails;
+    bool mDisplayWaypoints;
+    int mModelIndex;
+    QVector<QString> mModelNames;
+};
+
+typedef QSharedPointer<SystemViewParams> SystemViewParamsPtr;
+
+#endif // SYSTEMVIEWPARAMS

src/ui/map3D/ViewParamWidget.cc

+#include "ViewParamWidget.h"
+
+#include <osg/LineWidth>
+#include <QCheckBox>
+#include <QFormLayout>
+#include <QLabel>
+#include <QPushButton>
+
+#include "UASInterface.h"
+
+ViewParamWidget::ViewParamWidget(GlobalViewParamsPtr& globalViewParams,
+                                 QMap<int, SystemViewParamsPtr>& systemViewParamMap,
+                                 QWidget* parent, QWidget* mainWindow)
+ : QDockWidget(tr("View Parameters"), mainWindow)
+ , mGlobalViewParams(globalViewParams)
+ , mSystemViewParamMap(systemViewParamMap)
+ , mParent(parent)
+ , mFollowCameraComboBox(new QComboBox(this))
+ , mTabWidget(new QTabWidget(this))
+{
+    QVBoxLayout* layout = new QVBoxLayout;
+    QWidget* widget = new QWidget;
+    widget->setLayout(layout);
+
+    setWidget(widget);
+
+    buildLayout(layout);
+
+    mTabWidget->setFocusPolicy(Qt::NoFocus);
+
+    connect(parent, SIGNAL(systemCreatedSignal(UASInterface*)),
+            this, SLOT(systemCreated(UASInterface*)));
+}
+
+void
+ViewParamWidget::setFollowCameraId(int id)
+{
+    for (int i = 0; i < mFollowCameraComboBox->count(); ++i)
+    {
+        if (mFollowCameraComboBox->itemText(i).endsWith(QString::number(id)))
+        {
+            mFollowCameraComboBox->setCurrentIndex(i);
+            return;
+        }
+    }
+
+    mFollowCameraComboBox->setCurrentIndex(0);
+}
+
+void
+ViewParamWidget::systemCreated(UASInterface *uas)
+{
+    addTab(uas->getUASID());
+
+    QString text("MAV ");
+    text += QString::number(uas->getUASID());
+    mFollowCameraComboBox->addItem(text);
+}
+
+void
+ViewParamWidget::buildLayout(QVBoxLayout* layout)
+{
+    mFollowCameraComboBox->addItem("None");
+
+    QComboBox* frameComboBox = new QComboBox(this);
+    frameComboBox->addItem("Local");
+    frameComboBox->addItem("Global");
+
+    QComboBox* imageryComboBox = new QComboBox(this);
+    imageryComboBox->addItem("None");
+    imageryComboBox->addItem("Map (Google)");
+    imageryComboBox->addItem("Satellite (Google)");
+
+    QCheckBox* worldGridCheckBox = new QCheckBox(this);
+    worldGridCheckBox->setChecked(mGlobalViewParams->displayWorldGrid());
+
+    QMapIterator<int, SystemViewParamsPtr> it(mSystemViewParamMap);
+    while (it.hasNext())
+    {
+        QString text("MAV ");
+        text += QString::number(it.key());
+        mFollowCameraComboBox->addItem(text);
+
+        addTab(it.key());
+    }
+
+    QFormLayout* formLayout = new QFormLayout;
+    formLayout->addRow(tr("Follow Camera"), mFollowCameraComboBox);
+    formLayout->addRow(tr("Frame"), frameComboBox);
+    formLayout->addRow(tr("Imagery"), imageryComboBox);
+    formLayout->addRow(tr("World Grid"), worldGridCheckBox);
+
+    layout->addLayout(formLayout);
+    layout->addWidget(mTabWidget);
+
+    // connect signals/slots
+    connect(mFollowCameraComboBox, SIGNAL(currentIndexChanged(const QString&)),
+            mGlobalViewParams.data(), SLOT(followCameraChanged(const QString&)));
+    connect(frameComboBox, SIGNAL(currentIndexChanged(const QString&)),
+            mGlobalViewParams.data(), SLOT(frameChanged(const QString&)));
+    connect(imageryComboBox, SIGNAL(currentIndexChanged(int)),
+            mGlobalViewParams.data(), SLOT(imageryTypeChanged(int)));
+    connect(worldGridCheckBox, SIGNAL(stateChanged(int)),
+            mGlobalViewParams.data(), SLOT(toggleWorldGrid(int)));
+}
+
+void
+ViewParamWidget::addTab(int systemId)
+{
+    // add widgets that configure system-specific parameters
+    SystemViewParamsPtr systemViewParams = mSystemViewParamMap[systemId];
+
+    QWidget* page = new QWidget;
+
+    QCheckBox* colorPointCloudCheckBox = new QCheckBox(this);
+    colorPointCloudCheckBox->setChecked(systemViewParams->colorPointCloudByDistance());
+
+    QCheckBox* localGridCheckBox = new QCheckBox(this);
+    localGridCheckBox->setChecked(systemViewParams->displayLocalGrid());
+
+    QComboBox* modelComboBox = new QComboBox(this);
+    for (int i = 0; i < systemViewParams->modelNames().size(); ++i)
+    {
+        modelComboBox->addItem(systemViewParams->modelNames().at(i));
+    }
+
+    QCheckBox* obstacleListCheckBox = new QCheckBox(this);
+    obstacleListCheckBox->setChecked(systemViewParams->displayObstacleList());
+
+    QCheckBox* plannedPathCheckBox = new QCheckBox(this);
+    plannedPathCheckBox->setChecked(systemViewParams->displayPlannedPath());
+
+    QCheckBox* pointCloudCheckBox = new QCheckBox(this);
+    pointCloudCheckBox->setChecked(systemViewParams->displayPointCloud());
+
+    QCheckBox* rgbdCheckBox = new QCheckBox(this);
+    rgbdCheckBox->setChecked(systemViewParams->displayRGBD());
+
+    QCheckBox* targetCheckBox = new QCheckBox(this);
+    targetCheckBox->setChecked(systemViewParams->displayTarget());
+
+    QCheckBox* trailsCheckBox = new QCheckBox(this);
+    trailsCheckBox->setChecked(systemViewParams->displayTrails());
+
+    QCheckBox* waypointsCheckBox = new QCheckBox(this);
+    waypointsCheckBox->setChecked(systemViewParams->displayWaypoints());
+
+    QFormLayout* formLayout = new QFormLayout;
+    page->setLayout(formLayout);
+
+    formLayout->addRow(tr("Color Point Cloud"), colorPointCloudCheckBox);
+    formLayout->addRow(tr("Local Grid"), localGridCheckBox);
+    formLayout->addRow(tr("Model"), modelComboBox);
+    formLayout->addRow(tr("Obstacles"), obstacleListCheckBox);
+    formLayout->addRow(tr("Planned Path"), plannedPathCheckBox);
+    formLayout->addRow(tr("Point Cloud"), pointCloudCheckBox);
+    formLayout->addRow(tr("RGBD"), rgbdCheckBox);
+    formLayout->addRow(tr("Target"), targetCheckBox);
+    formLayout->addRow(tr("Trails"), trailsCheckBox);
+    formLayout->addRow(tr("Waypoints"), waypointsCheckBox);
+
+    QString label("MAV ");
+    label += QString::number(systemId);
+
+    mTabWidget->addTab(page, label);
+
+    // connect signals / slots
+    connect(colorPointCloudCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleColorPointCloud(int)));
+    connect(localGridCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleLocalGrid(int)));
+    connect(modelComboBox, SIGNAL(currentIndexChanged(int)),
+            systemViewParams.data(), SLOT(modelChanged(int)));
+    connect(obstacleListCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleObstacleList(int)));
+    connect(plannedPathCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(togglePlannedPath(int)));
+    connect(pointCloudCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(togglePointCloud(int)));
+    connect(rgbdCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleRGBD(int)));
+    connect(targetCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleTarget(int)));
+    connect(trailsCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleTrails(int)));
+    connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleWaypoints(int)));
+}

src/ui/map3D/ViewParamWidget.h

+#ifndef VIEWPARAMWIDGET_H
+#define VIEWPARAMWIDGET_H
+
+#include <QComboBox>
+#include <QDockWidget>
+#include <QTabWidget>
+#include <QVBoxLayout>
+
+#include "GlobalViewParams.h"
+#include "SystemViewParams.h"
+
+class UASInterface;
+
+class ViewParamWidget : public QDockWidget
+{
+    Q_OBJECT
+
+public:
+    ViewParamWidget(GlobalViewParamsPtr& globalViewParams,
+                    QMap<int, SystemViewParamsPtr>& systemViewParamMap,
+                    QWidget* parent = 0, QWidget* mainWindow = 0);
+
+    void setFollowCameraId(int id);
+
+signals:
+
+private slots:
+    void systemCreated(UASInterface* uas);
+
+private:
+    void buildLayout(QVBoxLayout* layout);
+    void addTab(int systemId);
+
+    // view parameters
+    GlobalViewParamsPtr mGlobalViewParams;
+    QMap<int, SystemViewParamsPtr>& mSystemViewParamMap;
+
+    // parent widget
+    QWidget* mParent;
+
+    // child widgets
+    QComboBox* mFollowCameraComboBox;
+    QTabWidget* mTabWidget;
+};
+
+#endif // VIEWPARAMWIDGET_H

src/ui/map3D/WaypointGroupNode.cc

@@ -37,7 +37,8 @@ This file is part of the QGROUNDCONTROL project
 
 #include "Imagery.h"
 
-WaypointGroupNode::WaypointGroupNode()
+WaypointGroupNode::WaypointGroupNode(const QColor& color)
+ : mColor(color)
 {
 
 }
@@ -49,7 +50,7 @@ WaypointGroupNode::init(void)
 }
 
 void
-WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+WaypointGroupNode::update(UASInterface* uas, MAV_FRAME frame)
 {
     if (!uas)
     {
@@ -105,11 +106,12 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
 
         if (wp->getCurrent())
         {
-            sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
+            sd->setColor(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f));
         }
         else
         {
-            sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+            sd->setColor(osg::Vec4(mColor.redF(), mColor.greenF(),
+                                   mColor.blueF(), 0.5f));
         }
 
         osg::ref_ptr<osg::Geode> geode = new osg::Geode;
@@ -133,14 +135,8 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
         sd->setShape(cylinder);
         sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
 
-        if (wp->getCurrent())
-        {
-            sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
-        }
-        else
-        {
-            sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
-        }
+        sd->setColor(osg::Vec4(mColor.redF(), mColor.greenF(),
+                               mColor.blueF(), 0.5f));
 
         geode = new osg::Geode;
         geode->addDrawable(sd);
@@ -164,7 +160,8 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             geometry->setVertexArray(vertices);
 
             osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
-            colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+            colors->push_back(osg::Vec4(mColor.redF(), mColor.greenF(),
+                                        mColor.blueF(), 0.5f));
             geometry->setColorArray(colors);
             geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
 

src/ui/map3D/WaypointGroupNode.h

@@ -39,13 +39,15 @@ This file is part of the QGROUNDCONTROL project
 class WaypointGroupNode : public osg::Group
 {
 public:
-    WaypointGroupNode();
+    WaypointGroupNode(const QColor& color);
 
     void init(void);
-    void update(MAV_FRAME frame, UASInterface* uas);
+    void update(UASInterface* uas, MAV_FRAME frame);
 
 private:
     void getPosition(Waypoint* wp, double& x, double& y, double& z);
+
+    QColor mColor;
 };
 
 #endif // WAYPOINTGROUPNODE_H