Added parameter dialog to modify terrain offset parameters.

qgroundcontrol.pro

@@ -358,7 +358,8 @@ HEADERS += src/MG.h \
     src/ui/mavlink/QGCMAVLinkMessageSender.h \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h \
     src/ui/QGCPluginHost.h \
-    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h
+    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h \
+    src/ui/map3D/TerrainParamDialog.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
@@ -498,7 +499,8 @@ SOURCES += src/main.cc \
     src/ui/mavlink/QGCMAVLinkMessageSender.cc \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc \
     src/ui/QGCPluginHost.cc \
-    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc
+    src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc \
+    src/ui/map3D/TerrainParamDialog.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/map3D/GlobalViewParams.cc

@@ -72,6 +72,18 @@ GlobalViewParams::frame(void) const
     return mFrame;
 }
 
+QVector4D&
+GlobalViewParams::terrainOffset(void)
+{
+    return mTerrainOffset;
+}
+
+QVector4D
+GlobalViewParams::terrainOffset(void) const
+{
+    return mTerrainOffset;
+}
+
 void
 GlobalViewParams::followCameraChanged(const QString& text)
 {

src/ui/map3D/GlobalViewParams.h

@@ -3,7 +3,7 @@
 
 #include <QObject>
 #include <QString>
-#include <QVector>
+#include <QVector4D>
 
 #include "QGCMAVLink.h"
 #include "Imagery.h"
@@ -30,6 +30,9 @@ public:
     MAV_FRAME& frame(void);
     MAV_FRAME frame(void) const;
 
+    QVector4D& terrainOffset(void);
+    QVector4D terrainOffset(void) const;
+
 public slots:
     void followCameraChanged(const QString& text);
     void frameChanged(const QString &text);
@@ -46,6 +49,7 @@ private:
     Imagery::Type mImageryType;
     int mFollowCameraId;
     MAV_FRAME mFrame;
+    QVector4D mTerrainOffset;
 };
 
 typedef QSharedPointer<GlobalViewParams> GlobalViewParamsPtr;

src/ui/map3D/Pixhawk3DWidget.cc

@@ -41,6 +41,7 @@
 
 #include "../MainWindow.h"
 #include "PixhawkCheetahGeode.h"
+#include "TerrainParamDialog.h"
 #include "UASManager.h"
 
 #include "QGC.h"
@@ -73,6 +74,9 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     mWorldGridNode = createWorldGrid();
     m3DWidget->worldMap()->addChild(mWorldGridNode, false);
 
+    mTerrainPAT = new osg::PositionAttitudeTransform;
+    m3DWidget->worldMap()->addChild(mTerrainPAT);
+
     // generate map model
     mImageryNode = createImagery();
     m3DWidget->worldMap()->addChild(mImageryNode, false);
@@ -403,6 +407,19 @@ Pixhawk3DWidget::clearData(void)
     }
 }
 
+void
+Pixhawk3DWidget::showTerrainParamWindow(void)
+{
+    TerrainParamDialog::getTerrainParams(mGlobalViewParams);
+
+    const QVector4D& terrainOffset = mGlobalViewParams->terrainOffset();
+
+    mTerrainPAT->setPosition(osg::Vec3d(terrainOffset.y(), terrainOffset.x(), -terrainOffset.z()));
+    mTerrainPAT->setAttitude(osg::Quat(M_PI_2 - terrainOffset.w(), osg::Vec3d(0.0f, 0.0f, 1.0f),
+                                       0.0, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                                       0.0, osg::Vec3d(0.0f, 1.0f, 0.0f)));
+}
+
 void
 Pixhawk3DWidget::showViewParamWindow(void)
 {
@@ -507,10 +524,18 @@ Pixhawk3DWidget::loadTerrainModel(void)
     {
         if (mTerrainNode.get())
         {
-            m3DWidget->worldMap()->removeChild(mTerrainNode);
+            mTerrainPAT->removeChild(mTerrainNode);
         }
         mTerrainNode = node;
-        m3DWidget->worldMap()->addChild(mTerrainNode);
+        mTerrainNode->setName("terrain");
+        mTerrainPAT->addChild(mTerrainNode);
+
+        mGlobalViewParams->terrainOffset() = QVector4D();
+
+        mTerrainPAT->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
+        mTerrainPAT->setAttitude(osg::Quat(M_PI_2, 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)));
     }
     else
     {
@@ -835,7 +860,7 @@ Pixhawk3DWidget::update(void)
     MAV_FRAME frame = mGlobalViewParams->frame();
 
     // set node visibility
-    m3DWidget->worldMap()->setChildValue(mTerrainNode,
+    m3DWidget->worldMap()->setChildValue(mTerrainPAT,
                                          mGlobalViewParams->displayTerrain());
     m3DWidget->worldMap()->setChildValue(mWorldGridNode,
                                          mGlobalViewParams->displayWorldGrid());
@@ -1169,6 +1194,16 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
             return;
         }
     }
+    else if (event->button() == Qt::RightButton)
+    {
+        if (findTerrain(event->pos()))
+        {
+            showTerrainMenu(event->globalPos());
+
+            event->accept();
+        }
+    }
+
 
     m3DWidget->handleMousePressEvent(event);
 }
@@ -2261,6 +2296,7 @@ Pixhawk3DWidget::findWaypoint(const QPoint& mousePos)
     return -1;
 }
 
+
 bool
 Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
 {
@@ -2288,6 +2324,41 @@ Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
     return false;
 }
 
+bool
+Pixhawk3DWidget::findTerrain(const QPoint& mousePos)
+{
+    if (!m3DWidget->getSceneData() || !mActiveUAS)
+    {
+        return -1;
+    }
+
+    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 (uint i = 0 ; i < it->nodePath.size(); ++i)
+            {
+                osg::Node* node = it->nodePath[i];
+                std::string nodeName = node->getName();
+
+                if (nodeName.compare("terrain") == 0)
+                {
+                    return true;
+                }
+            }
+        }
+    }
+
+    return false;
+}
+
 void
 Pixhawk3DWidget::showInsertWaypointMenu(const QPoint &cursorPos)
 {
@@ -2319,3 +2390,11 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
     menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
     menu.exec(cursorPos);
 }
+
+void
+Pixhawk3DWidget::showTerrainMenu(const QPoint &cursorPos)
+{
+    QMenu menu;
+    menu.addAction("Edit terrain parameters", this, SLOT(showTerrainParamWindow()));
+    menu.exec(cursorPos);
+}

src/ui/map3D/Pixhawk3DWidget.h

@@ -67,6 +67,7 @@ signals:
 
 private slots:
     void clearData(void);
+    void showTerrainParamWindow(void);
     void showViewParamWindow(void);
     void followCameraChanged(int systemId);
     void recenterActiveCamera(void);
@@ -168,8 +169,10 @@ private:
 
     int findWaypoint(const QPoint& mousePos);
     bool findTarget(int mouseX, int mouseY);
+    bool findTerrain(const QPoint& mousePos);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
+    void showTerrainMenu(const QPoint& cursorPos);
 
     const qreal kMessageTimeout; // message timeout in seconds
 
@@ -198,6 +201,7 @@ private:
     osg::ref_ptr<HUDScaleGeode> mScaleGeode;
     osg::ref_ptr<osgText::Text> mStatusText;
     osg::ref_ptr<osg::Node> mTerrainNode;
+    osg::ref_ptr<osg::PositionAttitudeTransform> mTerrainPAT;
     osg::ref_ptr<osg::Geode> mWorldGridNode;
 
     QPoint mCachedMousePos;

src/ui/map3D/TerrainParamDialog.cc

+#include "TerrainParamDialog.h"
+
+#include <QFormLayout>
+#include <QGroupBox>
+#include <QPushButton>
+
+TerrainParamDialog::TerrainParamDialog(QWidget* parent)
+ : QDialog(parent)
+{
+    QVBoxLayout* layout = new QVBoxLayout;
+    setLayout(layout);
+
+    setWindowTitle(tr("Terrain Parameters"));
+    setModal(true);
+
+    buildLayout(layout);
+}
+
+void
+TerrainParamDialog::getTerrainParams(GlobalViewParamsPtr &globalViewParams)
+{
+    QVector4D& terrainOffset = globalViewParams->terrainOffset();
+
+    TerrainParamDialog dialog;
+    dialog.mXOffsetSpinBox->setValue(terrainOffset.x());
+    dialog.mYOffsetSpinBox->setValue(terrainOffset.y());
+    dialog.mZOffsetSpinBox->setValue(terrainOffset.z());
+    dialog.mYawOffsetSpinBox->setValue(osg::RadiansToDegrees(terrainOffset.w()));
+
+    if (dialog.exec() == QDialog::Accepted)
+    {
+        terrainOffset.setX(dialog.mXOffsetSpinBox->value());
+        terrainOffset.setY(dialog.mYOffsetSpinBox->value());
+        terrainOffset.setZ(dialog.mZOffsetSpinBox->value());
+        terrainOffset.setW(osg::DegreesToRadians(dialog.mYawOffsetSpinBox->value()));
+    }
+}
+
+void
+TerrainParamDialog::closeWithSaving(void)
+{
+    done(QDialog::Accepted);
+}
+
+void
+TerrainParamDialog::closeWithoutSaving(void)
+{
+    done(QDialog::Rejected);
+}
+
+void
+TerrainParamDialog::buildLayout(QVBoxLayout* layout)
+{
+    QGroupBox* offsetGroupBox = new QGroupBox(tr("Offset"), this);
+
+    mXOffsetSpinBox = new QDoubleSpinBox(this);
+    mXOffsetSpinBox->setDecimals(1);
+    mXOffsetSpinBox->setRange(-100.0, 100.0);
+    mXOffsetSpinBox->setValue(0.0);
+
+    mYOffsetSpinBox = new QDoubleSpinBox(this);
+    mYOffsetSpinBox->setDecimals(1);
+    mYOffsetSpinBox->setRange(-100.0, 100.0);
+    mYOffsetSpinBox->setValue(0.0);
+
+    mZOffsetSpinBox = new QDoubleSpinBox(this);
+    mZOffsetSpinBox->setDecimals(1);
+    mZOffsetSpinBox->setRange(-100.0, 100.0);
+    mZOffsetSpinBox->setValue(0.0);
+
+    mYawOffsetSpinBox = new QDoubleSpinBox(this);
+    mYawOffsetSpinBox->setDecimals(0);
+    mYawOffsetSpinBox->setRange(-180.0, 180.0);
+    mYawOffsetSpinBox->setValue(0.0);
+
+    QFormLayout* formLayout = new QFormLayout;
+    formLayout->addRow(tr("x (m)"), mXOffsetSpinBox);
+    formLayout->addRow(tr("y (m)"), mYOffsetSpinBox);
+    formLayout->addRow(tr("z (m)"), mZOffsetSpinBox);
+    formLayout->addRow(tr("Yaw (deg)"), mYawOffsetSpinBox);
+
+    offsetGroupBox->setLayout(formLayout);
+
+    layout->addWidget(offsetGroupBox);
+    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding));
+
+    QPushButton* cancelButton = new QPushButton(this);
+    cancelButton->setText("Cancel");
+
+    QPushButton* saveButton = new QPushButton(this);
+    saveButton->setText("Save");
+
+    QHBoxLayout* buttonLayout = new QHBoxLayout;
+    buttonLayout->addWidget(cancelButton);
+    buttonLayout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding));
+    buttonLayout->addWidget(saveButton);
+
+    layout->addLayout(buttonLayout);
+
+    connect(cancelButton, SIGNAL(clicked()), this, SLOT(closeWithoutSaving()));
+    connect(saveButton, SIGNAL(clicked()), this, SLOT(closeWithSaving()));
+}

src/ui/map3D/TerrainParamDialog.h

+#ifndef TERRAINPARAMDIALOG_H
+#define TERRAINPARAMDIALOG_H
+
+#include <QDoubleSpinBox>
+#include <QDialog>
+#include <QVBoxLayout>
+
+#include "GlobalViewParams.h"
+
+class TerrainParamDialog : public QDialog
+{
+    Q_OBJECT
+
+public:
+    TerrainParamDialog(QWidget* parent = 0);
+
+    static void getTerrainParams(GlobalViewParamsPtr& globalViewParams);
+
+private slots:
+    void closeWithSaving(void);
+    void closeWithoutSaving(void);
+
+private:
+    void buildLayout(QVBoxLayout* layout);
+
+    QDoubleSpinBox* mXOffsetSpinBox;
+    QDoubleSpinBox* mYOffsetSpinBox;
+    QDoubleSpinBox* mZOffsetSpinBox;
+    QDoubleSpinBox* mYawOffsetSpinBox;
+};
+
+#endif // TERRAINPARAMDIALOG_H