Added feature to 3D view to display current scale.

qgroundcontrol.pro

@@ -235,8 +235,6 @@ HEADERS += src/MG.h \
     src/comm/QGCMAVLink.h \
     src/ui/QGCWebView.h \
     src/ui/map3D/QGCGoogleEarthView.h
-
-
 contains(DEPENDENCIES_PRESENT, osg) { 
     message("Including headers for OpenSceneGraph")
     
@@ -252,7 +250,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
         src/ui/map3D/WebImage.h \
         src/ui/map3D/TextureCache.h \
         src/ui/map3D/Texture.h \
-        src/ui/map3D/Imagery.h
+        src/ui/map3D/Imagery.h \
+        src/ui/map3D/HUDScaleGeode.h
     contains(DEPENDENCIES_PRESENT, osgearth) { 
         message("Including headers for OSGEARTH")
         
@@ -352,7 +351,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
         src/ui/map3D/WebImage.cc \
         src/ui/map3D/TextureCache.cc \
         src/ui/map3D/Texture.cc \
-        src/ui/map3D/Imagery.cc
+        src/ui/map3D/Imagery.cc \
+        src/ui/map3D/HUDScaleGeode.cc
     contains(DEPENDENCIES_PRESENT, osgearth) { 
         message("Including sources for osgEarth")
         

src/ui/map3D/HUDScaleGeode.cc

@@ -0,0 +1,125 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class HUDScaleGeode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "HUDScaleGeode.h"
+
+#include <osg/Geometry>
+#include <osg/LineWidth>
+
+HUDScaleGeode::HUDScaleGeode()
+{
+
+}
+
+void
+HUDScaleGeode::init(void)
+{
+    osg::ref_ptr<osg::Vec2Array> outlineVertices(new osg::Vec2Array);
+    outlineVertices->push_back(osg::Vec2(20.0f, 50.0f));
+    outlineVertices->push_back(osg::Vec2(20.0f, 70.0f));
+    outlineVertices->push_back(osg::Vec2(20.0f, 60.0f));
+    outlineVertices->push_back(osg::Vec2(100.0f, 60.0f));
+    outlineVertices->push_back(osg::Vec2(100.0f, 50.0f));
+    outlineVertices->push_back(osg::Vec2(100.0f, 70.0f));
+
+    osg::ref_ptr<osg::Geometry> outlineGeometry(new osg::Geometry);
+    outlineGeometry->setVertexArray(outlineVertices);
+
+    osg::ref_ptr<osg::Vec4Array> outlineColor(new osg::Vec4Array);
+    outlineColor->push_back(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+    outlineGeometry->setColorArray(outlineColor);
+    outlineGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+    outlineGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
+                                                         0, 6));
+
+    osg::ref_ptr<osg::LineWidth> outlineWidth(new osg::LineWidth());
+    outlineWidth->setWidth(4.0f);
+    outlineGeometry->getOrCreateStateSet()->
+            setAttributeAndModes(outlineWidth, osg::StateAttribute::ON);
+
+    addDrawable(outlineGeometry);
+
+    osg::ref_ptr<osg::Vec2Array> markerVertices(new osg::Vec2Array);
+    markerVertices->push_back(osg::Vec2(20.0f, 50.0f));
+    markerVertices->push_back(osg::Vec2(20.0f, 70.0f));
+    markerVertices->push_back(osg::Vec2(20.0f, 60.0f));
+    markerVertices->push_back(osg::Vec2(100.0f, 60.0f));
+    markerVertices->push_back(osg::Vec2(100.0f, 50.0f));
+    markerVertices->push_back(osg::Vec2(100.0f, 70.0f));
+
+    osg::ref_ptr<osg::Geometry> markerGeometry(new osg::Geometry);
+    markerGeometry->setVertexArray(markerVertices);
+
+    osg::ref_ptr<osg::Vec4Array> markerColor(new osg::Vec4Array);
+    markerColor->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 1.0f));
+    markerGeometry->setColorArray(markerColor);
+    markerGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+    markerGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
+                                                        0, 6));
+
+    osg::ref_ptr<osg::LineWidth> markerWidth(new osg::LineWidth());
+    markerWidth->setWidth(1.5f);
+    markerGeometry->getOrCreateStateSet()->
+            setAttributeAndModes(markerWidth, osg::StateAttribute::ON);
+
+    addDrawable(markerGeometry);
+
+    text = new osgText::Text;
+    text->setCharacterSize(11);
+    text->setFont("images/Vera.ttf");
+    text->setAxisAlignment(osgText::Text::SCREEN);
+    text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+    text->setPosition(osg::Vec3(40.0f, 45.0f, -1.5f));
+
+    addDrawable(text);
+}
+
+void
+HUDScaleGeode::update(int windowHeight, float cameraFov, float cameraDistance,
+                      bool darkBackground)
+{
+    float f = static_cast<float>(windowHeight) / 2.0f
+              / tanf(cameraFov / 180.0f * M_PI / 2.0f);
+    float dist = cameraDistance / f * 80.0f;
+
+    if (darkBackground)
+    {
+        text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+    }
+    else
+    {
+        text->setColor(osg::Vec4(0.0f, 0.0f, 0.0f, 1.0f));
+    }
+
+    text->setText(QString("%1 m").arg(dist, 0, 'f', 2).toStdString());
+}

src/ui/map3D/HUDScaleGeode.h

@@ -0,0 +1,52 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class HUDScaleGeode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef HUDSCALEGEODE_H
+#define HUDSCALEGEODE_H
+
+#include <osg/Geode>
+#include <osgText/Text>
+#include <QString>
+
+class HUDScaleGeode : public osg::Geode
+{
+public:
+    HUDScaleGeode();
+
+    void init(void);
+    void update(int windowHeight, float cameraFov, float cameraDistance,
+                bool darkBackground);
+
+private:
+    osg::ref_ptr<osgText::Text> text;
+};
+
+#endif // HUDSCALEGEODE_H

src/ui/map3D/Pixhawk3DWidget.cc

@@ -287,9 +287,18 @@ Pixhawk3DWidget::setWaypointAltitude(void)
 {
     if (uas)
     {
+        bool ok;
         const QVector<Waypoint *> waypoints =
                 uas->getWaypointManager().getWaypointList();
-//        waypoints.at(selectedWpIndex)->setZ(0.0);
+        Waypoint* waypoint = waypoints.at(selectedWpIndex);
+
+        double newAltitude =
+                QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
+                                        tr("Altitude (m):"), -waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
+        if (ok)
+        {
+            waypoint->setZ(-newAltitude);
+        }
     }
 }
 
@@ -760,6 +769,10 @@ Pixhawk3DWidget::setupHUD(void)
                                         osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
                                         depthImage);
     hudGroup->addChild(depth2DGeode);
+
+    scaleGeode = new HUDScaleGeode;
+    scaleGeode->init();
+    hudGroup->addChild(scaleGeode);
 }
 
 void
@@ -821,6 +834,15 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
             " " << cursorPosition.second << "]";
     statusText->setText(oss.str());
 
+    bool darkBackground = true;
+    if (mapNode->getImageryType() == Imagery::GOOGLE_MAP)
+    {
+        darkBackground = false;
+    }
+
+    scaleGeode->update(height(), cameraParams.cameraFov,
+                       cameraManipulator->getDistance(), darkBackground);
+
     if (!rgb.isNull())
     {
         rgbImage->setImage(640, 480, 1,

src/ui/map3D/Pixhawk3DWidget.h

@@ -34,6 +34,7 @@
 
 #include <osgText/Text>
 
+#include "HUDScaleGeode.h"
 #include "Imagery.h"
 #include "ImageWindowGeode.h"
 
@@ -136,6 +137,7 @@ private:
     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;

src/ui/map3D/Texture.cc

@@ -127,10 +127,10 @@ Texture::draw(double x1, double y1, double x2, double y2,
 {
     osg::Vec3dArray* vertices =
             static_cast<osg::Vec3dArray*>(geometry->getVertexArray());
-    (*vertices)[0].set(x1, y1, z - 0.1);
-    (*vertices)[1].set(x2, y2, z - 0.1);
-    (*vertices)[2].set(x3, y3, z - 0.1);
-    (*vertices)[3].set(x4, y4, z - 0.1);
+    (*vertices)[0].set(x1, y1, z);
+    (*vertices)[1].set(x2, y2, z);
+    (*vertices)[2].set(x3, y3, z);
+    (*vertices)[3].set(x4, y4, z);
 
     osg::DrawArrays* drawarrays =
             static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));