Merge remote-tracking branch 'remotes/upstream/master'

qgroundcontrol.pri

@@ -246,11 +246,11 @@ message("Compiling for linux 32")
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-            #-losgQt \
-
     DEFINES += QGC_OSG_ENABLED
+    DEFINES += QGC_OSG_QT_ENABLED
     }
 
     exists(/usr/local/include/google/protobuf) {
@@ -330,11 +330,11 @@ linux-g++-64 {
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-#            -losgQt \
-
     DEFINES += QGC_OSG_ENABLED
+    DEFINES += QGC_OSG_QT_ENABLED
     }
 
     exists(/usr/local/include/google/protobuf) {

qgroundcontrol.pro

@@ -387,7 +387,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
     message("Including headers for Protocol Buffers")
 
     # Enable only if protobuf is available
-    HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h
+    HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h \
+               src/ui/map3D/ObstacleGroupNode.h
 }
 contains(DEPENDENCIES_PRESENT, libfreenect) { 
     message("Including headers for libfreenect")
@@ -527,7 +528,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
     message("Including sources for Protocol Buffers")
 
     # Enable only if protobuf is available
-    SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc
+    SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc \
+               src/ui/map3D/ObstacleGroupNode.cc
 }
 contains(DEPENDENCIES_PRESENT, libfreenect) { 
     message("Including sources for libfreenect")

src/uas/UAS.cc

@@ -993,6 +993,11 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
         rgbdImage.CopyFrom(*message);
         emit rgbdImageChanged(this);
     }
+    else if (message->GetTypeName() == obstacleList.GetTypeName())
+    {
+        obstacleList.CopyFrom(*message);
+        emit obstacleListChanged(this);
+    }
 }
 
 #endif
@@ -1906,25 +1911,6 @@ void UAS::executeCommand(MAV_CMD command)
     sendMessage(msg);
 }
 
-void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component)
-{
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-    cmd.command = (uint8_t)command;
-    cmd.confirmation = confirmation;
-    cmd.param1 = param1;
-    cmd.param2 = param2;
-    cmd.param3 = param3;
-    cmd.param4 = param4;
-    cmd.param5 = 0.0f;
-    cmd.param6 = 0.0f;
-    cmd.param7 = 0.0f;
-    cmd.target_system = uasId;
-    cmd.target_component = component;
-    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
-    sendMessage(msg);
-}
-
 void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
 {
     mavlink_message_t msg;
@@ -2191,7 +2177,7 @@ void UAS::shutdown()
 void UAS::setTargetPosition(float x, float y, float z, float yaw)
 {
     mavlink_message_t msg;
-    mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 2, 3, 0, yaw, x, y, z);
     sendMessage(msg);
 }
 

src/uas/UAS.h

@@ -142,6 +142,10 @@ public:
     px::RGBDImage getRGBDImage() const {
         return rgbdImage;
     }
+
+    px::ObstacleList getObstacleList() const {
+        return obstacleList;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -230,6 +234,7 @@ protected: //COMMENTS FOR TEST UNIT
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
     px::RGBDImage rgbdImage;
+    px::ObstacleList obstacleList;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
@@ -406,8 +411,6 @@ public slots:
     void setUASName(const QString& name);
     /** @brief Executes a command **/
     void executeCommand(MAV_CMD command);
-    /** @brief Executes a command **/
-    void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component);
     /** @brief Executes a command with 7 params */
     void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component);
     /** @brief Set the current battery type and voltages */
@@ -563,10 +566,14 @@ signals:
     void imageStarted(quint64 timestamp);
     /** @brief A new camera image has arrived */
     void imageReady(UASInterface* uas);
+#ifdef QGC_PROTOBUF_ENABLED
     /** @brief Point cloud data has been changed */
     void pointCloudChanged(UASInterface* uas);
     /** @brief RGBD image data has been changed */
     void rgbdImageChanged(UASInterface* uas);
+    /** @brief Obstacle list data has been changed */
+    void obstacleListChanged(UASInterface* uas);
+#endif
     /** @brief HIL controls have changed */
     void hilControlsChanged(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode);
 

src/uas/UASInterface.h

@@ -46,7 +46,7 @@ This file is part of the QGROUNDCONTROL project
 
 #ifdef QGC_PROTOBUF_ENABLED
 #include <tr1/memory>
-#include <pixhawk.pb.h>
+#include <pixhawk/pixhawk.pb.h>
 #endif
 
 /**
@@ -97,6 +97,7 @@ public:
 #ifdef QGC_PROTOBUF_ENABLED
     virtual px::PointCloudXYZRGB getPointCloud() const = 0;
     virtual px::RGBDImage getRGBDImage() const = 0;
+    virtual px::ObstacleList getObstacleList() const = 0;
 #endif
 
     virtual bool isArmed() const = 0;
@@ -214,7 +215,7 @@ public slots:
     /** @brief Execute command immediately **/
     virtual void executeCommand(MAV_CMD command) = 0;
     /** @brief Executes a command **/
-    virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component) = 0;
+    virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0;
 
     /** @brief Selects the airframe */
     virtual void setAirframe(int airframe) = 0;

src/ui/HUD.cc

@@ -1431,10 +1431,13 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
 
 void HUD::copyImage()
 {
+    if (isVisible())
+    {
         qDebug() << "HUD::copyImage()";
         UAS* u = dynamic_cast<UAS*>(this->uas);
         if (u)
         {
             this->glImage = QGLWidget::convertToGLFormat(u->getImage());
         }
+    }
 }

src/ui/MAVLinkDecoder.cc

@@ -27,8 +27,9 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
     messageFilter.insert(MAVLINK_MSG_ID_MISSION_ITEM, false);
     messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false);
     messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false);
-    #ifdef MAVLINK_ENABLED_PIXHAWK
     messageFilter.insert(MAVLINK_MSG_ID_DATA_STREAM, false);
+    #ifdef MAVLINK_ENABLED_PIXHAWK
+    messageFilter.insert(MAVLINK_MSG_ID_ENCAPSULATED_DATA, false);
     messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
     #endif
     messageFilter.insert(MAVLINK_MSG_ID_EXTENDED_MESSAGE, false);

src/ui/QGCFirmwareUpdate.h

@@ -17,9 +17,23 @@ public:
 
 protected:
     void changeEvent(QEvent *e);
+    void showEvent(QShowEvent* event)
+    {
+        QWidget::showEvent(event);
+        emit visibilityChanged(true);
+    }
+
+    void hideEvent(QHideEvent* event)
+    {
+        QWidget::hideEvent(event);
+        emit visibilityChanged(false);
+    }
 
 private:
     Ui::QGCFirmwareUpdate *ui;
+
+signals:
+    void visibilityChanged(bool visible);
 };
 
 #endif // QGCFIRMWAREUPDATE_H

src/ui/QGCToolBar.cc

@@ -392,6 +392,6 @@ void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QS
 
 QGCToolBar::~QGCToolBar()
 {
-    delete toggleLoggingAction;
-    delete logReplayAction;
+    if (toggleLoggingAction) toggleLoggingAction->deleteLater();
+    if (logReplayAction) logReplayAction->deleteLater();
 }

src/ui/designer/QGCCommandButton.cc

@@ -79,7 +79,6 @@ QGCCommandButton::~QGCCommandButton()
 void QGCCommandButton::sendCommand()
 {
     if (QGCToolWidgetItem::uas) {
-        // FIXME
         int index = ui->editCommandComboBox->itemData(ui->editCommandComboBox->currentIndex()).toInt();
         MAV_CMD command = static_cast<MAV_CMD>(index);
         int confirm = (ui->editConfirmationCheckBox->isChecked()) ? 1 : 0;
@@ -87,9 +86,12 @@ void QGCCommandButton::sendCommand()
         float param2 = ui->editParam2SpinBox->value();
         float param3 = ui->editParam3SpinBox->value();
         float param4 = ui->editParam4SpinBox->value();
+        float param5 = ui->editParam5SpinBox->value();
+        float param6 = ui->editParam6SpinBox->value();
+        float param7 = ui->editParam7SpinBox->value();
         int component = ui->editComponentSpinBox->value();
 
-        QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component);
+        QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
         qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
     } else {
         qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";

src/ui/map/QGCMapWidget.cc

@@ -284,6 +284,68 @@ void QGCMapWidget::updateGlobalPosition()
     }
 }
 
+void QGCMapWidget::updateLocalPosition()
+{
+    QList<UASInterface*> systems = UASManager::instance()->getUASList();
+    foreach (UASInterface* system, systems)
+    {
+        // Get reference to graphic UAV item
+        mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
+        // Check if reference is valid, else create a new one
+        if (uav == NULL)
+        {
+            MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
+            AddUAV(system->getUASID(), newUAV);
+            uav = newUAV;
+            uav->SetTrailTime(1);
+            uav->SetTrailDistance(5);
+            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
+        }
+
+        // Set new lat/lon position of UAV icon
+        double latitude = UASManager::instance()->getHomeLatitude();
+        double longitude = UASManager::instance()->getHomeLongitude();
+        double altitude = UASManager::instance()->getHomeAltitude();
+        internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
+        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
+        // Follow status
+        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
+        // Convert from radians to degrees and apply
+        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
+    }
+}
+
+void QGCMapWidget::updateLocalPositionEstimates()
+{
+    QList<UASInterface*> systems = UASManager::instance()->getUASList();
+    foreach (UASInterface* system, systems)
+    {
+        // Get reference to graphic UAV item
+        mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
+        // Check if reference is valid, else create a new one
+        if (uav == NULL)
+        {
+            MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
+            AddUAV(system->getUASID(), newUAV);
+            uav = newUAV;
+            uav->SetTrailTime(1);
+            uav->SetTrailDistance(5);
+            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
+        }
+
+        // Set new lat/lon position of UAV icon
+        double latitude = UASManager::instance()->getHomeLatitude();
+        double longitude = UASManager::instance()->getHomeLongitude();
+        double altitude = UASManager::instance()->getHomeAltitude();
+        internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
+        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
+        // Follow status
+        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
+        // Convert from radians to degrees and apply
+        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
+    }
+}
+
 
 void QGCMapWidget::updateSystemSpecs(int uas)
 {

src/ui/map/QGCMapWidget.h

@@ -46,6 +46,10 @@ public slots:
     void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);
     /** @brief Update the global position of all systems */
     void updateGlobalPosition();
+    /** @brief Update the local position and draw it converted to GPS reference */
+    void updateLocalPosition();
+    /** @brief Update the local position estimates (individual sensors) and draw it converted to GPS reference */
+    void updateLocalPositionEstimates();
     /** @brief Update the type, size, etc. of this system */
     void updateSystemSpecs(int uas);
     /** @brief Change current system in focus / editing */

src/ui/map3D/HUDScaleGeode.cc

@@ -40,7 +40,7 @@ HUDScaleGeode::HUDScaleGeode()
 }
 
 void
-HUDScaleGeode::init(void)
+HUDScaleGeode::init(osg::ref_ptr<osgText::Font>& font)
 {
     osg::ref_ptr<osg::Vec2Array> outlineVertices(new osg::Vec2Array);
     outlineVertices->push_back(osg::Vec2(20.0f, 50.0f));
@@ -96,7 +96,7 @@ HUDScaleGeode::init(void)
 
     text = new osgText::Text;
     text->setCharacterSize(11);
-    text->setFont("images/Vera.ttf");
+    text->setFont(font);
     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));

src/ui/map3D/HUDScaleGeode.h

@@ -41,7 +41,7 @@ class HUDScaleGeode : public osg::Geode
 public:
     HUDScaleGeode();
 
-    void init(void);
+    void init(osg::ref_ptr<osgText::Font>& font);
     void update(int windowHeight, float cameraFov, float cameraDistance,
                 bool darkBackground);
 

src/ui/map3D/ImageWindowGeode.cc

@@ -31,11 +31,17 @@
 
 #include "ImageWindowGeode.h"
 
-ImageWindowGeode::ImageWindowGeode(const QString& caption,
-                                   const osg::Vec4& backgroundColor,
-                                   osg::ref_ptr<osg::Image>& image)
+ImageWindowGeode::ImageWindowGeode()
     : border(5)
 {
+
+}
+
+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);
@@ -82,7 +88,7 @@ ImageWindowGeode::ImageWindowGeode(const QString& caption,
     text = new osgText::Text;
     text->setText(caption.toStdString().c_str());
     text->setCharacterSize(11);
-    text->setFont("images/Vera.ttf");
+    text->setFont(font);
     text->setAxisAlignment(osgText::Text::SCREEN);
     text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
 

src/ui/map3D/ImageWindowGeode.h

@@ -40,8 +40,10 @@
 class ImageWindowGeode : public osg::Geode
 {
 public:
-    ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor,
-                     osg::ref_ptr<osg::Image>& image);
+    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);
 

src/ui/map3D/ObstacleGroupNode.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 ObstacleGroupNode.
+ *
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
+ *
+ */
+
+#include "ObstacleGroupNode.h"
+
+#include <osg/PositionAttitudeTransform>
+#include <osg/ShapeDrawable>
+
+#include "Imagery.h"
+
+ObstacleGroupNode::ObstacleGroupNode()
+{
+
+}
+
+void
+ObstacleGroupNode::init(void)
+{
+
+}
+
+void
+ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+{
+    if (!uas || frame == MAV_FRAME_GLOBAL)
+    {
+        return;
+    }
+
+    double robotX = uas->getLocalX();
+    double robotY = uas->getLocalY();
+    double robotZ = uas->getLocalZ();
+
+    if (getNumChildren() > 0)
+    {
+        removeChild(0, getNumChildren());
+    }
+
+    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+
+    px::ObstacleList obstacleList = uas->getObstacleList();
+
+    for (int i = 0; i < obstacleList.obstacles_size(); ++i)
+    {
+        const px::Obstacle& obs = obstacleList.obstacles(i);
+
+        osg::Vec3d obsPos(obs.y() - robotY, obs.x() - robotX, robotZ - obs.z());
+
+        osg::ref_ptr<osg::Box> box =
+            new osg::Box(obsPos, obs.width(), obs.width(), obs.height());
+        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(box);
+
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        sd->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
+
+        geode->addDrawable(sd);
+    }
+
+    addChild(geode);
+}
+

src/ui/map3D/ObstacleGroupNode.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 ObstacleGroupNode.
+ *
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
+ *
+ */
+
+#ifndef OBSTACLEGROUPNODE_H
+#define OBSTACLEGROUPNODE_H
+
+#include <osg/Group>
+
+#include "UASInterface.h"
+
+class ObstacleGroupNode : public osg::Group
+{
+public:
+    ObstacleGroupNode();
+
+    void init(void);
+    void update(MAV_FRAME frame, UASInterface* uas);
+};
+
+#endif // OBSTACLEGROUPNODE_H

src/ui/map3D/Pixhawk3DWidget.h

@@ -25,7 +25,7 @@
  * @file
  *   @brief Definition of the class Pixhawk3DWidget.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 
@@ -38,6 +38,9 @@
 #include "Imagery.h"
 #include "ImageWindowGeode.h"
 #include "WaypointGroupNode.h"
+#ifdef QGC_PROTOBUF_ENABLED
+    #include "ObstacleGroupNode.h"
+#endif
 
 #include "Q3DWidget.h"
 
@@ -67,10 +70,12 @@ private slots:
     void recenter(void);
     void toggleFollowCamera(int state);
 
+    void selectTargetHeading(void);
     void selectTarget(void);
+    void setTarget(void);
     void insertWaypoint(void);
-    void moveWaypoint(void);
-    void setWaypoint(void);
+    void moveWaypointPosition(void);
+    void moveWaypointHeading(void);
     void deleteWaypoint(void);
     void setWaypointAltitude(void);
     void clearAllWaypoints(void);
@@ -78,12 +83,28 @@ private slots:
 protected:
     QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
     void buildLayout(void);
+    virtual void resizeGL(int width, int height);
     virtual void display(void);
     virtual void keyPressEvent(QKeyEvent* event);
     virtual void mousePressEvent(QMouseEvent* event);
+    void showEvent(QShowEvent* event)
+    {
+        QWidget::showEvent(event);
+        emit visibilityChanged(true);
+    }
+
+    void hideEvent(QHideEvent* event)
+    {
+        QWidget::hideEvent(event);
+        emit visibilityChanged(false);
+    }
+    virtual void mouseMoveEvent(QMouseEvent* event);
 
     UASInterface* uas;
 
+signals:
+    void visibilityChanged(bool visible);
+
 private:
     void getPose(double& x, double& y, double& z,
                  double& roll, double& pitch, double& yaw,
@@ -113,16 +134,19 @@ private:
     void updateTarget(double robotX, double robotY);
 #ifdef QGC_PROTOBUF_ENABLED
     void updateRGBD(double robotX, double robotY, double robotZ);
+    void updateObstacles(void);
 #endif
 
-    int findWaypoint(int mouseX, int mouseY);
+    int findWaypoint(const QPoint& mousePos);
     bool findTarget(int mouseX, int mouseY);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
 
     enum Mode {
         DEFAULT_MODE,
-        MOVE_WAYPOINT_MODE
+        MOVE_WAYPOINT_POSITION_MODE,
+        MOVE_WAYPOINT_HEADING_MODE,
+        SELECT_TARGET_HEADING_MODE
     };
     Mode mode;
     int selectedWpIndex;
@@ -133,6 +157,7 @@ private:
     bool displayWaypoints;
     bool displayRGBD2D;
     bool displayRGBD3D;
+    bool displayObstacleList;
     bool enableRGBDColor;
     bool enableTarget;
 
@@ -157,11 +182,15 @@ private:
     osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Node> targetNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
+#ifdef QGC_PROTOBUF_ENABLED
+    osg::ref_ptr<ObstacleGroupNode> obstacleGroupNode;
+#endif
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
     MAV_FRAME frame;
-    osg::Vec2d target;
+    osg::Vec3d target;
+    QPoint cachedMousePos;
     double lastRobotX, lastRobotY, lastRobotZ;
 };
 

src/ui/map3D/Q3DWidget.cc

@@ -35,6 +35,9 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/Geometry>
 #include <osg/LineWidth>
 #include <osg/MatrixTransform>
+#ifdef QGC_OSG_QT_ENABLED
+#include <osgQt/QFontImplementation>
+#endif
 #ifdef Q_OS_MACX
 #include <Carbon/Carbon.h>
 #endif
@@ -56,12 +59,21 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     cameraParams.cameraFov = 30.0f;
     cameraParams.minClipRange = 1.0f;
     cameraParams.maxClipRange = 10000.0f;
+#ifdef QGC_OSG_QT_ENABLED
+    osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
+    fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
+#else
+    osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
+    fontImpl = 0;//new osgText::Font::Font("images/Vera.ttf");
+#endif
+    font = new osgText::Font(fontImpl);
 
     osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
 
     setThreadingModel(osgViewer::Viewer::SingleThreaded);
 
     setFocusPolicy(Qt::StrongFocus);
+    setMouseTracking(true);
 }
 
 Q3DWidget::~Q3DWidget()

src/ui/map3D/Q3DWidget.h

@@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/LineSegment>
 #include <osg/PositionAttitudeTransform>
 #include <osgGA/TrackballManipulator>
+#include <osgText/Font>
 #include <osgViewer/Viewer>
 
 #include "GCManipulator.h"
@@ -259,6 +260,8 @@ protected:
 
     CameraParams cameraParams; /**< Struct representing camera parameters. */
     float fps;
+
+    osg::ref_ptr<osgText::Font> font;
 };
 
 #endif // Q3DWIDGET_H

src/ui/map3D/WaypointGroupNode.cc

@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
  * @file
  *   @brief Definition of the class WaypointGroupNode.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 
@@ -51,11 +51,16 @@ WaypointGroupNode::init(void)
 void
 WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
 {
-    if (uas) {
+    if (!uas)
+    {
+        return;
+    }
+
     double robotX = 0.0;
     double robotY = 0.0;
     double robotZ = 0.0;
-        if (frame == MAV_FRAME_GLOBAL) {
+    if (frame == MAV_FRAME_GLOBAL)
+    {
         double latitude = uas->getLatitude();
         double longitude = uas->getLongitude();
         double altitude = uas->getAltitude();
@@ -63,25 +68,63 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
         robotZ = -altitude;
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
         robotX = uas->getLocalX();
         robotY = uas->getLocalY();
         robotZ = uas->getLocalZ();
     }
 
-        if (getNumChildren() > 0) {
+    if (getNumChildren() > 0)
+    {
         removeChild(0, getNumChildren());
     }
 
     const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
 
-        for (int i = 0; i < list.size(); i++) {
+    for (int i = 0; i < list.size(); i++)
+    {
         Waypoint* wp = list.at(i);
 
         double wpX, wpY, wpZ;
         getPosition(wp, wpX, wpY, wpZ);
+        double wpYaw = osg::DegreesToRadians(wp->getYaw());
+
+        osg::ref_ptr<osg::Group> group = new osg::Group;
 
+        // cone indicates waypoint orientation
         osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+        double coneRadius = wp->getAcceptanceRadius() / 2.0;
+        osg::ref_ptr<osg::Cone> cone =
+            new osg::Cone(osg::Vec3d(wpZ, 0.0, 0.0),
+                          coneRadius, wp->getAcceptanceRadius() * 2.0);
+
+        sd->setShape(cone);
+        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));
+        }
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+        pat->addChild(geode);
+        pat->setAttitude(osg::Quat(wpYaw - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                                   M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+                                   0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+        group->addChild(pat);
+
+        // cylinder indicates waypoint position
+        sd = new osg::ShapeDrawable;
         osg::ref_ptr<osg::Cylinder> cylinder =
             new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
                               wp->getAcceptanceRadius(),
@@ -90,20 +133,25 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
         sd->setShape(cylinder);
         sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
 
-            if (wp->getCurrent()) {
+        if (wp->getCurrent())
+        {
             sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
-            } else {
+        }
+        else
+        {
             sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
         }
 
-            osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode = new osg::Geode;
         geode->addDrawable(sd);
+        group->addChild(geode);
 
         char wpLabel[10];
         sprintf(wpLabel, "wp%d", i);
-            geode->setName(wpLabel);
+        group->setName(wpLabel);
 
-            if (i < list.size() - 1) {
+        if (i < list.size() - 1)
+        {
             double nextWpX, nextWpY, nextWpZ;
             getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
 
@@ -130,23 +178,21 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             geode->addDrawable(geometry);
         }
 
-            osg::ref_ptr<osg::PositionAttitudeTransform> pat =
-                new osg::PositionAttitudeTransform;
-
+        pat = new osg::PositionAttitudeTransform;
         pat->setPosition(osg::Vec3d(wpY - robotY,
                                     wpX - robotX,
                                     robotZ));
 
         addChild(pat);
-            pat->addChild(geode);
-        }
+        pat->addChild(group);
     }
 }
 
 void
 WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
 {
-    if (wp->getFrame() == MAV_FRAME_GLOBAL) {
+    if (wp->getFrame() == MAV_FRAME_GLOBAL)
+    {
         double latitude = wp->getY();
         double longitude = wp->getX();
         double altitude = wp->getZ();
@@ -154,7 +200,9 @@ WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
         z = -altitude;
-    } else if (wp->getFrame() == MAV_FRAME_LOCAL_NED) {
+    }
+    else if (wp->getFrame() == MAV_FRAME_LOCAL_NED)
+    {
         x = wp->getX();
         y = wp->getY();
         z = wp->getZ();

src/ui/map3D/WaypointGroupNode.h

@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
  * @file
  *   @brief Definition of the class WaypointGroupNode.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 

thirdParty/mavlink/include/mavlink_protobuf_manager.hpp

@@ -42,6 +42,18 @@ public:
 			registerType(msg);
 		}
 
+		// register ObstacleList
+		{
+			std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
+			registerType(msg);
+		}
+
+		// register ObstacleMap
+		{
+			std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
+			registerType(msg);
+		}
+
 		srand(time(NULL));
 		mStreamID = rand() + 1;
 	}
@@ -186,6 +198,11 @@ public:
 				if (offset == 0)
 				{
 					queue.push_back(msg);
+
+					if ((flags & 0x1) != 0x1)
+					{
+						reassemble = true;
+					}
 				}
 				else
 				{

thirdParty/mavlink/include/mavlink_types.h

@@ -15,10 +15,21 @@
 
 #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
 
+
+#if (defined __GNUC__) || (defined _MSC_VER) || (defined __MINGW32__) || (defined __WATCOMC__) || (defined __CMB__) || (defined __BORLANDC__) || (defined __clang__)
+#define MAVLINK_EXTENDED_MESSAGES_ENABLED
+#endif
+
+// EXTENDED message definition - the extended message set is compatible to standard MAVLink message passing
+// but does NOT have to be supported by the platform. The extended message set will NOT consume
+// any memory if the messages are not explicitely used
+
+#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
 #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
 #define MAVLINK_EXTENDED_HEADER_LEN 14
 #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
 #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
+#endif
 
 typedef struct param_union {
 	union {
@@ -51,12 +62,13 @@ typedef struct __mavlink_message {
 	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
 } mavlink_message_t;
 
-
+#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
 typedef struct __mavlink_extended_message {
        mavlink_message_t base_msg;
        int32_t extended_payload_len;   ///< Length of extended payload if any
        uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
 } mavlink_extended_message_t;
+#endif
 
 
 typedef enum {