Merged

src/comm/MAVLinkSimulationLink.cc

@@ -99,12 +99,6 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     // Open packet log
     mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
     mavlinkLogFile->open(QIODevice::ReadOnly);
-
-    // position at Pixhawk lab @ ETHZ
-    x = 5247273.0f;
-    y = 465955.0f;
-    z = -0.2f;
-    yaw = 0;
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()
@@ -383,15 +377,14 @@ void MAVLinkSimulationLink::mainloop()
         x = x*0.93f + 0.07f*(x+sin(static_cast<float>(QGC::groundTimeUsecs()) * 0.08f));
         y = y*0.93f + 0.07f*(y+sin(static_cast<float>(QGC::groundTimeUsecs()) * 0.5f));
         z = z*0.93f + 0.07f*(z+sin(static_cast<float>(QGC::groundTimeUsecs()*0.001f)) * 0.1f);
-        x = 5247273.0f;
-        y = 465955.0f;
-//        x = (x > 5.0f) ? 5.0f : x;
-//        y = (y > 5.0f) ? 5.0f : y;
-//        z = (z > 3.0f) ? 3.0f : z;
-//
-//        x = (x < -5.0f) ? -5.0f : x;
-//        y = (y < -5.0f) ? -5.0f : y;
-//        z = (z < -3.0f) ? -3.0f : z;
+
+        x = (x > 5.0f) ? 5.0f : x;
+        y = (y > 5.0f) ? 5.0f : y;
+        z = (z > 3.0f) ? 3.0f : z;
+
+        x = (x < -5.0f) ? -5.0f : x;
+        y = (y < -5.0f) ? -5.0f : y;
+        z = (z < -3.0f) ? -3.0f : z;
 
         // Send back new setpoint
         mavlink_message_t ret;
@@ -409,14 +402,14 @@ void MAVLinkSimulationLink::mainloop()
         streampointer += bufferlength;
 
         // GPS RAW
-        mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.001), 8.548103+(y*0.001), z, 0, 0, 2.5f, 0.1f);
+        mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f);
         bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
         //add data into datastream
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
         // GLOBAL POSITION
-        mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.001), 8.548103+(y*0.001), z, 0, 0);
+        mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 47.378028137103+(x*0.00001), 8.54899892510421+(y*0.00001), z, 0, 0, 0);
         bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
         //add data into datastream
         memcpy(stream+streampointer,buffer, bufferlength);

src/input/Freenect.cc

+/*=====================================================================
+
+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 Freenect.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #include "Freenect.h"
 
 #include <cmath>

src/input/Freenect.h

+/*=====================================================================
+
+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 Freenect.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #ifndef FREENECT_H
 #define FREENECT_H
 

src/uas/UAS.cc

@@ -73,15 +73,15 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
         sendDropRate(0),
         lowBattAlarm(false),
         positionLock(false),
-        localX(0),
-        localY(0),
-        localZ(0),
-        latitude(0),
-        longitude(0),
-        altitude(0),
-        roll(0),
-        pitch(0),
-        yaw(0),
+        localX(0.0),
+        localY(0.0),
+        localZ(0.0),
+        latitude(0.0),
+        longitude(0.0),
+        altitude(0.0),
+        roll(0.0),
+        pitch(0.0),
+        yaw(0.0),
         statusTimeout(new QTimer(this))
 {
     color = UASInterface::getNextColor();
@@ -345,6 +345,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_global_position_t pos;
                 mavlink_msg_global_position_decode(&message, &pos);
                 quint64 time = getUnixTime(pos.usec);
+                latitude = pos.lat;
+                longitude = pos.lon;
+                altitude = pos.alt;
                 emit valueChanged(uasId, "lat", pos.lat, time);
                 emit valueChanged(uasId, "lon", pos.lon, time);
                 emit valueChanged(uasId, "alt", pos.alt, time);

src/uas/UAS.h

@@ -82,9 +82,9 @@ public:
     double getLocalX() const { return localX; };
     double getLocalY() const { return localY; };
     double getLocalZ() const { return localZ; };
-    double getLat() const { return latitude; };
-    double getLon() const { return longitude; };
-    double getAlt() const { return altitude; };
+    double getLatitude() const { return latitude; };
+    double getLongitude() const { return longitude; };
+    double getAltitude() const { return altitude; };
 
     double getRoll() const { return roll; };
     double getPitch() const { return pitch; };

src/uas/UASInterface.h

@@ -70,9 +70,9 @@ public:
     virtual double getLocalY() const = 0;
     virtual double getLocalZ() const = 0;
 
-    virtual double getLat() const = 0;
-    virtual double getLon() const = 0;
-    virtual double getAlt() const = 0;
+    virtual double getLatitude() const = 0;
+    virtual double getLongitude() const = 0;
+    virtual double getAltitude() const = 0;
 
     virtual double getRoll() const = 0;
     virtual double getPitch() const = 0;

src/ui/map3D/Imagery.cc

@@ -118,6 +118,7 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
 void
 Imagery::draw2D(double windowWidth, double windowHeight,
                 double zoom, double xOrigin, double yOrigin,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone)
 {
     if (getNumDrawables() > 0)
@@ -171,10 +172,11 @@ Imagery::draw2D(double windowWidth, double windowHeight,
             TexturePtr t = textureCache->get(tileURL);
             if (!t.isNull())
             {
-                addDrawable(t->draw(y1, x1,
-                                    y2, x2,
-                                    y3, x3,
-                                    y4, x4,
+                addDrawable(t->draw(y1 - yOffset, x1 - xOffset,
+                                    y2 - yOffset, x2 - xOffset,
+                                    y3 - yOffset, x3 - xOffset,
+                                    y4 - yOffset, x4 - xOffset,
+                                    zOffset,
                                     true));
             }
         }
@@ -213,6 +215,7 @@ Imagery::prefetch3D(double radius, double tileResolution,
 void
 Imagery::draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone)
 {
     if (getNumDrawables() > 0)
@@ -246,10 +249,11 @@ Imagery::draw3D(double radius, double tileResolution,
 
             if (!t.isNull())
             {
-                addDrawable(t->draw(y1, x1,
-                                    y2, x2,
-                                    y3, x3,
-                                    y4, x4,
+                addDrawable(t->draw(y1 - yOffset, x1 - xOffset,
+                                    y2 - yOffset, x2 - xOffset,
+                                    y3 - yOffset, x3 - xOffset,
+                                    y4 - yOffset, x4 - xOffset,
+                                    zOffset,
                                     true));
             }
         }
@@ -397,7 +401,7 @@ Imagery::UTMtoTile(double northing, double easting, const QString& utmZone,
 }
 
 QChar
-Imagery::UTMLetterDesignator(double latitude) const
+Imagery::UTMLetterDesignator(double latitude)
 {
     // This routine determines the correct UTM letter designator for the given latitude
     // returns 'Z' if latitude is outside the UTM limits of 84N to 80S
@@ -432,7 +436,7 @@ Imagery::UTMLetterDesignator(double latitude) const
 void
 Imagery::LLtoUTM(double latitude, double longitude,
                  double& utmNorthing, double& utmEasting,
-                 QString& utmZone) const
+                 QString& utmZone)
 {
     // converts lat/long to UTM coords.  Equations from USGS Bulletin 1532
     // East Longitudes are positive, West longitudes are negative.
@@ -513,7 +517,7 @@ Imagery::LLtoUTM(double latitude, double longitude,
 
 void
 Imagery::UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
-                 double& latitude, double& longitude) const
+                 double& latitude, double& longitude)
 {
     // converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
     // East Longitudes are positive, West longitudes are negative.

src/ui/map3D/Imagery.h

@@ -60,6 +60,7 @@ public:
                     const QString& utmZone);
     void draw2D(double windowWidth, double windowHeight,
                 double zoom, double xOrigin, double yOrigin,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone);
 
     void prefetch3D(double radius, double tileResolution,
@@ -67,10 +68,17 @@ public:
                     const QString& utmZone);
     void draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone);
 
     bool update(void);
 
+    static void LLtoUTM(double latitude, double longitude,
+                        double& utmNorthing, double& utmEasting,
+                        QString& utmZone);
+    static void UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
+                        double& latitude, double& longitude);
+
 private:
     void imageBounds(int tileX, int tileY, double tileResolution,
                      double& x1, double& y1, double& x2, double& y2,
@@ -90,13 +98,7 @@ private:
     void UTMtoTile(double northing, double easting, const QString& utmZone,
                    double tileResolution, int& tileX, int& tileY,
                    int& zoomLevel) const;
-    QChar UTMLetterDesignator(double latitude) const;
-
-    void LLtoUTM(double latitude, double longitude,
-                 double& utmNorthing, double& utmEasting,
-                 QString& utmZone) const;
-    void UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
-                 double& latitude, double& longitude) const;
+    static QChar UTMLetterDesignator(double latitude);
 
     QString getTileLocation(int tileX, int tileY, int zoomLevel,
                             double tileResolution) const;

src/ui/map3D/Pixhawk3DWidget.cc

@@ -47,6 +47,8 @@
 Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
      : Q3DWidget(parent)
      , uas(NULL)
+     , mode(DEFAULT_MODE)
+     , selectedWpIndex(-1)
      , displayGrid(true)
      , displayTrail(false)
      , displayImagery(true)
@@ -78,7 +80,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 
     // generate map model
     mapNode = createMap();
-    allocentricMap->addChild(mapNode);
+    rollingMap->addChild(mapNode);
 
     // generate target model
     allocentricMap->addChild(createTarget());
@@ -203,9 +205,13 @@ Pixhawk3DWidget::recenter(void)
     double robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
     if (uas != NULL)
     {
-        robotX = uas->getLocalX();
-        robotY = uas->getLocalY();
-        robotZ = uas->getLocalZ();
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
+        robotZ = -altitude;
     }
 
     recenterCamera(robotY, robotX, -robotZ);
@@ -224,6 +230,88 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
     }
 }
 
+void
+Pixhawk3DWidget::insertWaypoint(void)
+{
+    if (uas)
+    {
+        double altitude = uas->getAltitude();
+
+        std::pair<double,double> cursorWorldCoords =
+                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+        Waypoint* wp = new Waypoint(0,
+                                    cursorWorldCoords.first,
+                                    cursorWorldCoords.second,
+                                    -altitude);
+        uas->getWaypointManager().addWaypoint(wp);
+    }
+}
+
+void
+Pixhawk3DWidget::moveWaypoint(void)
+{
+    mode = MOVE_WAYPOINT_MODE;
+}
+
+void
+Pixhawk3DWidget::setWaypoint(void)
+{
+    if (uas)
+    {
+        double altitude = uas->getAltitude();
+
+        std::pair<double,double> cursorWorldCoords =
+                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+        const QVector<Waypoint *> waypoints =
+                uas->getWaypointManager().getWaypointList();
+        Waypoint* waypoint = waypoints.at(selectedWpIndex);
+        waypoint->setX(cursorWorldCoords.first);
+        waypoint->setY(cursorWorldCoords.second);
+        waypoint->setZ(-altitude);
+    }
+}
+
+void
+Pixhawk3DWidget::deleteWaypoint(void)
+{
+    if (uas)
+    {
+        uas->getWaypointManager().removeWaypoint(selectedWpIndex);
+    }
+}
+
+void
+Pixhawk3DWidget::setWaypointAltitude(void)
+{
+    if (uas)
+    {
+        const QVector<Waypoint *> waypoints =
+                uas->getWaypointManager().getWaypointList();
+//        waypoints.at(selectedWpIndex)->setZ(0.0);
+    }
+}
+
+void
+Pixhawk3DWidget::clearAllWaypoints(void)
+{
+    if (uas)
+    {
+        double altitude = uas->getAltitude();
+
+        std::pair<double,double> cursorWorldCoords =
+                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+        const QVector<Waypoint *> waypoints =
+                uas->getWaypointManager().getWaypointList();
+        for (int i = waypoints.size() - 1; i >= 0; --i)
+        {
+            uas->getWaypointManager().removeWaypoint(i);
+        }
+    }
+}
+
 QVector< osg::ref_ptr<osg::Node> >
 Pixhawk3DWidget::findVehicleModels(void)
 {
@@ -297,18 +385,13 @@ Pixhawk3DWidget::buildLayout(void)
     mapComboBox->addItem("Map (Google)");
     mapComboBox->addItem("Satellite (Google)");
 
-    QLabel* modelLabel = new QLabel("Vehicle Model", this);
+    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());
     }
 
-    targetButton = new QPushButton(this);
-    targetButton->setCheckable(true);
-    targetButton->setChecked(false);
-    targetButton->setIcon(QIcon(QString::fromUtf8(":/images/status/weather-clear.svg")));
-
     QPushButton* recenterButton = new QPushButton(this);
     recenterButton->setText("Recenter Camera");
 
@@ -327,10 +410,9 @@ Pixhawk3DWidget::buildLayout(void)
     layout->addWidget(mapComboBox, 1, 5);
     layout->addWidget(modelLabel, 1, 6);
     layout->addWidget(modelComboBox, 1, 7);
-    layout->addWidget(targetButton, 1, 8);
-    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 9);
-    layout->addWidget(recenterButton, 1, 10);
-    layout->addWidget(followCameraCheckBox, 1, 11);
+    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 8);
+    layout->addWidget(recenterButton, 1, 9);
+    layout->addWidget(followCameraCheckBox, 1, 10);
     layout->setRowStretch(0, 100);
     layout->setRowStretch(1, 1);
     setLayout(layout);
@@ -358,9 +440,16 @@ Pixhawk3DWidget::display(void)
         return;
     }
 
-    double robotX = uas->getLocalX();
-    double robotY = uas->getLocalY();
-    double robotZ = uas->getLocalZ();
+    double latitude = uas->getLatitude();
+    double longitude = uas->getLongitude();
+    double altitude = uas->getAltitude();
+
+    double robotX;
+    double robotY;
+    QString utmZone;
+    Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
+    double robotZ = -altitude;
+
     double robotRoll = uas->getRoll();
     double robotPitch = uas->getPitch();
     double robotYaw = uas->getYaw();
@@ -397,7 +486,7 @@ Pixhawk3DWidget::display(void)
 
     if (displayImagery)
     {
-        updateImagery();
+        updateImagery(robotX, robotY, robotZ, utmZone);
     }
 
     if (displayTarget)
@@ -422,7 +511,7 @@ Pixhawk3DWidget::display(void)
 
     rollingMap->setChildValue(gridNode, displayGrid);
     rollingMap->setChildValue(trailNode, displayTrail);
-    allocentricMap->setChildValue(mapNode, displayImagery);
+    rollingMap->setChildValue(mapNode, displayImagery);
     rollingMap->setChildValue(targetNode, displayTarget);
     rollingMap->setChildValue(waypointsNode, displayWaypoints);
     if (enableFreenect)
@@ -462,9 +551,30 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
 void
 Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
-    if (event->button() == Qt::LeftButton && targetButton->isChecked())
+    if (event->button() == Qt::LeftButton)
+    {
+        if (mode == MOVE_WAYPOINT_MODE)
+        {
+            setWaypoint();
+            mode = DEFAULT_MODE;
+
+            return;
+        }
+
+        if (event->modifiers() == Qt::ShiftModifier)
+        {
+            selectedWpIndex = findWaypoint(event->x(), event->y());
+            if (selectedWpIndex == -1)
+            {
+                showInsertWaypointMenu(event->globalPos());
+            }
+            else
             {
-        markTarget();
+                showEditWaypointMenu(event->globalPos());
+            }
+
+            return;
+        }
     }
 
     Q3DWidget::mousePressEvent(event);
@@ -779,15 +889,14 @@ Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
 }
 
 void
-Pixhawk3DWidget::updateImagery(void)
+Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
+                               const QString& zone)
 {
     if (mapNode->getImageryType() == Imagery::BLANK_MAP)
     {
         return;
     }
 
-    char zone[5] = "32T";
-
     double viewingRadius = cameraManipulator->getDistance() * 10.0;
     if (viewingRadius < 100.0)
     {
@@ -828,6 +937,9 @@ Pixhawk3DWidget::updateImagery(void)
                     resolution,
                     cameraManipulator->getCenter().y(),
                     cameraManipulator->getCenter().x(),
+                    originX,
+                    originY,
+                    originZ,
                     zone);
 
     // prefetch map tiles
@@ -894,6 +1006,14 @@ Pixhawk3DWidget::updateWaypoints(void)
 {
     if (uas)
     {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+
+        double robotX, robotY;
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
+        double robotZ = -uas->getAltitude();
+
         if (waypointsNode->getNumChildren() > 0)
         {
             waypointsNode->removeChild(0, waypointsNode->getNumChildren());
@@ -903,29 +1023,63 @@ Pixhawk3DWidget::updateWaypoints(void)
 
         for (int i = 0; i < list.size(); i++)
         {
+            Waypoint* wp = list.at(i);
+
             osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
-            osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere;
-            sphere->setRadius(0.2);
-            sd->setShape(sphere);
+            osg::ref_ptr<osg::Cylinder> cylinder =
+                    new osg::Cylinder(osg::Vec3d(0.0, 0.0, - wp->getZ() / 2.0),
+                                      wp->getOrbit(),
+                                      fabs(wp->getZ()));
+
+            sd->setShape(cylinder);
+            sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
 
-            if (list.at(i)->getCurrent())
+            if (wp->getCurrent())
             {
-                sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 1.0f));
+                sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
             }
             else
             {
-                sd->setColor(osg::Vec4(0.0f, 1.0f, 1.0f, 1.0f));
+                sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
             }
 
             osg::ref_ptr<osg::Geode> geode = new osg::Geode;
             geode->addDrawable(sd);
 
+            char wpLabel[10];
+            sprintf(wpLabel, "wp%d", i);
+            geode->setName(wpLabel);
+
+            if (i < list.size() - 1)
+            {
+                osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
+                osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
+                vertices->push_back(osg::Vec3d(0.0, 0.0, -wp->getZ()));
+                vertices->push_back(osg::Vec3d(list.at(i+1)->getY() - wp->getY(),
+                                               list.at(i+1)->getX() - wp->getX(),
+                                               -list.at(i+1)->getZ()));
+                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));
+                geometry->setColorArray(colors);
+                geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+                geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
+
+                osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
+                linewidth->setWidth(2.0f);
+                geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
+
+                geode->addDrawable(geometry);
+            }
+
             osg::ref_ptr<osg::PositionAttitudeTransform> pat =
                     new osg::PositionAttitudeTransform;
 
-            pat->setPosition(osg::Vec3d(list.at(i)->getY() - uas->getLocalY(),
-                                        list.at(i)->getX() - uas->getLocalX(),
-                                        0.0));
+            pat->setPosition(osg::Vec3d(wp->getY() - robotY,
+                                        wp->getX() - robotX,
+                                        robotZ));
 
             waypointsNode->addChild(pat);
             pat->addChild(geode);
@@ -1142,3 +1296,59 @@ Pixhawk3DWidget::markTarget(void)
 
     targetButton->setChecked(false);
 }
+
+int
+Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
+{
+    if (getSceneData() != NULL)
+    {
+        osgUtil::LineSegmentIntersector::Intersections intersections;
+
+        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        {
+            for (osgUtil::LineSegmentIntersector::Intersections::iterator
+                 it = intersections.begin(); it != intersections.end(); it++)
+            {
+                for (uint i = 0 ; i < it->nodePath.size(); ++i)
+                {
+                    std::string nodeName = it->nodePath[i]->getName();
+                    if (nodeName.substr(0, 2).compare("wp") == 0)
+                    {
+                        qDebug() << nodeName.c_str() << "Got!!";
+                        return atoi(nodeName.substr(2).c_str());
+                    }
+                }
+            }
+        }
+    }
+
+    return -1;
+}
+
+void
+Pixhawk3DWidget::showInsertWaypointMenu(const QPoint &cursorPos)
+{
+    QMenu menu;
+    menu.addAction("Insert new waypoint", this, SLOT(insertWaypoint()));
+    menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
+    menu.exec(cursorPos);
+}
+
+void
+Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
+{
+    QMenu menu;
+
+    QString text;
+    text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
+    menu.addAction(text, this, SLOT(moveWaypoint()));
+
+    text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
+    menu.addAction(text, this, SLOT(setWaypointAltitude()));
+
+    text = QString("Delete waypoint %1").arg(QString::number(selectedWpIndex));
+    menu.addAction(text, this, SLOT(deleteWaypoint()));
+
+    menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
+    menu.exec(cursorPos);
+}

src/ui/map3D/Pixhawk3DWidget.h

@@ -68,6 +68,13 @@ private slots:
     void recenter(void);
     void toggleFollowCamera(int state);
 
+    void insertWaypoint(void);
+    void moveWaypoint(void);
+    void setWaypoint(void);
+    void deleteWaypoint(void);
+    void setWaypointAltitude(void);
+    void clearAllWaypoints(void);
+
 protected:
     QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
     void buildLayout(void);
@@ -91,7 +98,8 @@ private:
     void updateHUD(double robotX, double robotY, double robotZ,
                    double robotRoll, double robotPitch, double robotYaw);
     void updateTrail(double robotX, double robotY, double robotZ);
-    void updateImagery(void);
+    void updateImagery(double originX, double originY, double originZ,
+                       const QString& zone);
     void updateTarget(void);
     void updateWaypoints(void);
 #ifdef QGC_LIBFREENECT_ENABLED
@@ -100,6 +108,17 @@ private:
 
     void markTarget(void);
 
+    int findWaypoint(int mouseX, int mouseY);
+    void showInsertWaypointMenu(const QPoint& cursorPos);
+    void showEditWaypointMenu(const QPoint& cursorPos);
+
+    enum Mode {
+        DEFAULT_MODE,
+        MOVE_WAYPOINT_MODE
+    };
+    Mode mode;
+    int selectedWpIndex;
+
     bool displayGrid;
     bool displayTrail;
     bool displayImagery;

src/ui/map3D/Q3DWidget.cc

@@ -72,6 +72,9 @@ Q3DWidget::init(float fps)
 {
     getCamera()->setGraphicsContext(osgGW);
 
+    // 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
@@ -149,8 +152,9 @@ Q3DWidget::createRobot(void)
 osg::ref_ptr<osg::Node>
 Q3DWidget::createHUD(void)
 {
-    hudProjectionMatrix->setMatrix(osg::Matrix::ortho2D(0, width(),
-                                                        0, height()));
+    hudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width(),
+                                                      0.0, height(),
+                                                      -10.0, 10.0));
 
     osg::ref_ptr<osg::MatrixTransform> hudModelViewMatrix(
             new osg::MatrixTransform);
@@ -256,8 +260,9 @@ Q3DWidget::getMouseY(void)
 void
 Q3DWidget::resizeGL(int width, int height)
 {
-    hudProjectionMatrix->setMatrix(osg::Matrix::ortho2D(0, width,
-                                                        0, height));
+    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);

src/ui/map3D/QGCGoogleEarthView.cc

@@ -76,24 +76,25 @@ void QGCGoogleEarthView::updateState()
         if (mav)
         {
             uasId = mav->getUASID();
-            lat = mav->getLat();
-            lon = mav->getLon();
-            alt = mav->getAlt();
+            lat = mav->getLatitude();
+            lon = mav->getLongitude();
+            alt = mav->getAltitude();
             roll = mav->getRoll();
             pitch = mav->getPitch();
             yaw = mav->getYaw();
         }
-//        ui->webView->page()->currentFrame()->evaluateJavaScript(QString("setAircraftPosition(%1, %2, %3, %4);")
-//                                                                .arg(uasId)
-//                                                                .arg(lat)
-//                                                                .arg(lon)
-//                                                                .arg(alt));
-//        //ui->webView->page()->currentFrame()->evaluateJavaScript(QString("drawAndCenter(%1, %2, %3, %4, '%5', %6, %7, %8, %9, %10, %11);").arg(lat).arg(lon).arg(alt).arg("true").arg("ff0000ff").arg("1").arg("true").arg("true").arg(yaw).arg(pitch).arg(roll));
+        ui->webView->page()->currentFrame()->evaluateJavaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);")
+                                                                .arg(uasId)
+                                                                .arg(lat)
+                                                                .arg(lon)
+                                                                .arg(alt+500)
+                                                                .arg(roll)
+                                                                .arg(pitch)
+                                                                .arg(yaw));
 
         if (followCamera)
         {
              ui->webView->page()->currentFrame()->evaluateJavaScript(QString("updateFollowAircraft()"));
-            //ui->webView->page()->currentFrame()->evaluateJavaScript(QString("followAircraft(%1);").arg(mav->getUASID()));
         }
     }
 }

src/ui/map3D/Texture.cc

@@ -113,22 +113,24 @@ Texture::sync(const WebImagePtr& image)
 
 osg::ref_ptr<osg::Geometry>
 Texture::draw(double x1, double y1, double x2, double y2,
+              double z,
               bool smoothInterpolation) const
 {
-    return draw(x1, y1, x2, y1, x2, y2, x1, y2, smoothInterpolation);
+    return draw(x1, y1, x2, y1, x2, y2, x1, y2, z, smoothInterpolation);
 }
 
 osg::ref_ptr<osg::Geometry>
 Texture::draw(double x1, double y1, double x2, double y2,
               double x3, double y3, double x4, double y4,
+              double z,
               bool smoothInterpolation) const
 {
     osg::Vec3dArray* vertices =
             static_cast<osg::Vec3dArray*>(geometry->getVertexArray());
-    (*vertices)[0].set(x1, y1, -0.1);
-    (*vertices)[1].set(x2, y2, -0.1);
-    (*vertices)[2].set(x3, y3, -0.1);
-    (*vertices)[3].set(x4, y4, -0.1);
+    (*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);
 
     osg::DrawArrays* drawarrays =
             static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));

src/ui/map3D/Texture.h

@@ -52,9 +52,11 @@ public:
     void sync(const WebImagePtr& image);
 
     osg::ref_ptr<osg::Geometry> draw(double x1, double y1, double x2, double y2,
+                                     double z,
                                      bool smoothInterpolation) const;
     osg::ref_ptr<osg::Geometry> draw(double x1, double y1, double x2, double y2,
                                      double x3, double y3, double x4, double y4,
+                                     double z,
                                      bool smoothInterpolation) const;
 
 private: