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