Merge branch 'dev' of pixhawk.ethz.ch:qgroundcontrol into dev

qgroundcontrol.pro

@@ -238,7 +238,6 @@ HEADERS += src/MG.h \
     src/ui/QGCWebView.h \
     src/ui/map3D/QGCGoogleEarthView.h \
     src/ui/map3D/QGCWebPage.h
-
 contains(DEPENDENCIES_PRESENT, osg) { 
     message("Including headers for OpenSceneGraph")
     
@@ -255,7 +254,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
         src/ui/map3D/TextureCache.h \
         src/ui/map3D/Texture.h \
         src/ui/map3D/Imagery.h \
-        src/ui/map3D/HUDScaleGeode.h
+        src/ui/map3D/HUDScaleGeode.h \
+        src/ui/map3D/WaypointGroupNode.h
     contains(DEPENDENCIES_PRESENT, osgearth) { 
         message("Including headers for OSGEARTH")
         
@@ -357,7 +357,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
         src/ui/map3D/TextureCache.cc \
         src/ui/map3D/Texture.cc \
         src/ui/map3D/Imagery.cc \
-        src/ui/map3D/HUDScaleGeode.cc
+        src/ui/map3D/HUDScaleGeode.cc \
+        src/ui/map3D/WaypointGroupNode.cc
     contains(DEPENDENCIES_PRESENT, osgearth) { 
         message("Including sources for osgEarth")
         

src/input/Freenect.cc

@@ -106,7 +106,7 @@ Freenect::~Freenect()
     if (device != NULL)
     {
         freenect_stop_depth(device);
-        freenect_stop_rgb(device);
+        freenect_stop_video(device);
     }
 
     freenect_close_device(device);
@@ -136,8 +136,8 @@ Freenect::init(int userDeviceNumber)
 
     freenect_set_user(device, this);
 
-    memset(rgb, 0, FREENECT_RGB_SIZE);
-    memset(depth, 0, FREENECT_DEPTH_SIZE);
+    memset(rgb, 0, FREENECT_VIDEO_RGB_SIZE);
+    memset(depth, 0, FREENECT_DEPTH_11BIT_SIZE);
 
     // set Kinect parameters
     if (freenect_set_tilt_degs(device, tiltAngle) != 0)
@@ -148,22 +148,22 @@ Freenect::init(int userDeviceNumber)
     {
         return false;
     }
-    if (freenect_set_rgb_format(device, FREENECT_FORMAT_RGB) != 0)
+    if (freenect_set_video_format(device, FREENECT_VIDEO_RGB) != 0)
     {
         return false;
     }
-    if (freenect_set_depth_format(device, FREENECT_FORMAT_11_BIT) != 0)
+    if (freenect_set_depth_format(device, FREENECT_DEPTH_11BIT) != 0)
     {
         return false;
     }
-    freenect_set_rgb_callback(device, rgbCallback);
+    freenect_set_video_callback(device, videoCallback);
     freenect_set_depth_callback(device, depthCallback);
 
-    if (freenect_start_rgb(device) != 0)
+    if (freenect_start_depth(device) != 0)
     {
         return false;
     }
-    if (freenect_start_depth(device) != 0)
+    if (freenect_start_video(device) != 0)
     {
         return false;
     }
@@ -182,14 +182,10 @@ Freenect::process(void)
         return false;
     }
 
-    //libfreenect changed some access functions in one of the new revisions
-    freenect_raw_device_state state;
-    freenect_get_mks_accel(&state, &ax, &ay, &az);
-    //tiltAngle = freenect_get_tilt_degs(&state);
-
-    //these are the old access functions
-    //freenect_get_raw_accel(device, &ax, &ay, &az);
-    //freenect_get_mks_accel(device, &dx, &dy, &dz);
+    freenect_raw_tilt_state* state;
+    freenect_update_tilt_state(device);
+    state = freenect_get_tilt_state(device);
+    freenect_get_mks_accel(state, &ax, &ay, &az);
 
     return true;
 }
@@ -199,7 +195,7 @@ Freenect::getRgbData(void)
 {
     QMutexLocker locker(&rgbMutex);
     return QSharedPointer<QByteArray>(
-            new QByteArray(rgb, FREENECT_RGB_SIZE));
+            new QByteArray(rgb, FREENECT_VIDEO_RGB_SIZE));
 }
 
 QSharedPointer<QByteArray>
@@ -207,7 +203,7 @@ Freenect::getRawDepthData(void)
 {
     QMutexLocker locker(&depthMutex);
     return QSharedPointer<QByteArray>(
-            new QByteArray(depth, FREENECT_DEPTH_SIZE));
+            new QByteArray(depth, FREENECT_DEPTH_11BIT_SIZE));
 }
 
 QSharedPointer<QByteArray>
@@ -215,7 +211,7 @@ Freenect::getColoredDepthData(void)
 {
     QMutexLocker locker(&coloredDepthMutex);
     return QSharedPointer<QByteArray>(
-            new QByteArray(coloredDepth, FREENECT_RGB_SIZE));
+            new QByteArray(coloredDepth, FREENECT_VIDEO_RGB_SIZE));
 }
 
 QVector<QVector3D>
@@ -386,22 +382,22 @@ Freenect::projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
 }
 
 void
-Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp)
+Freenect::videoCallback(freenect_device* device, void* video, uint32_t timestamp)
 {
     Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
 
     QMutexLocker locker(&freenect->rgbMutex);
-    memcpy(freenect->rgb, rgb, FREENECT_RGB_SIZE);
+    memcpy(freenect->rgb, video, FREENECT_VIDEO_RGB_SIZE);
 }
 
 void
 Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp)
 {
     Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
-    freenect_depth* data = reinterpret_cast<freenect_depth *>(depth);
+    uint16_t* data = reinterpret_cast<uint16_t *>(depth);
 
     QMutexLocker depthLocker(&freenect->depthMutex);
-    memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE);
+    memcpy(freenect->depth, data, FREENECT_DEPTH_11BIT_SIZE);
 
     QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
     unsigned short* src = reinterpret_cast<unsigned short *>(data);

src/input/Freenect.h

@@ -95,7 +95,7 @@ private:
     void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
                              const IntrinsicCameraParameters& params);
 
-    static void rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp);
+    static void videoCallback(freenect_device* device, void* video, uint32_t timestamp);
     static void depthCallback(freenect_device* device, void* depth, uint32_t timestamp);
 
     freenect_context* context;
@@ -122,13 +122,13 @@ private:
     int tiltAngle;
 
     // rgbd data
-    char rgb[FREENECT_RGB_SIZE];
+    char rgb[FREENECT_VIDEO_RGB_SIZE];
     QMutex rgbMutex;
 
-    char depth[FREENECT_DEPTH_SIZE];
+    char depth[FREENECT_DEPTH_11BIT_SIZE];
     QMutex depthMutex;
 
-    char coloredDepth[FREENECT_RGB_SIZE];
+    char coloredDepth[FREENECT_VIDEO_RGB_SIZE];
     QMutex coloredDepthMutex;
 
     // accelerometer data

src/ui/WaypointView.cc

@@ -49,7 +49,6 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
     m_ui->setupUi(this);
 
     this->wp = wp;
-    wp->setFrame(MAV_FRAME_LOCAL);
 
     // add actions
     m_ui->comboBox_action->addItem("Navigate",MAV_ACTION_NAVIGATE);
@@ -63,7 +62,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
 
     // defaults
     changedAction(0);
-    changedFrame(0);
+    changedFrame(wp->getFrame());
 
     // Read values and set user interface
     updateValues();

src/ui/map3D/Pixhawk3DWidget.h

@@ -37,6 +37,7 @@
 #include "HUDScaleGeode.h"
 #include "Imagery.h"
 #include "ImageWindowGeode.h"
+#include "WaypointGroupNode.h"
 
 #ifdef QGC_LIBFREENECT_ENABLED
 #include "Freenect.h"
@@ -61,6 +62,7 @@ public slots:
     void setActiveUAS(UASInterface* uas);
 
 private slots:
+    void selectFrame(QString text);
     void showGrid(int state);
     void showTrail(int state);
     void showWaypoints(int state);
@@ -86,29 +88,34 @@ protected:
     UASInterface* uas;
 
 private:
+    void getPose(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);
+
     osg::ref_ptr<osg::Geode> createGrid(void);
     osg::ref_ptr<osg::Geode> createTrail(void);
     osg::ref_ptr<Imagery> createMap(void);
-    osg::ref_ptr<osg::Node> createTarget(void);
-    osg::ref_ptr<osg::Group> createWaypoints(void);
     osg::ref_ptr<osg::Geode> createRGBD3D(void);
 
     void setupHUD(void);
     void resizeHUD(void);
 
     void updateHUD(double robotX, double robotY, double robotZ,
-                   double robotRoll, double robotPitch, double robotYaw);
+                   double robotRoll, double robotPitch, double robotYaw,
+                   const QString& utmZone);
     void updateTrail(double robotX, double robotY, double robotZ);
     void updateImagery(double originX, double originY, double originZ,
                        const QString& zone);
-    void updateTarget(void);
     void updateWaypoints(void);
 #ifdef QGC_LIBFREENECT_ENABLED
     void updateRGBD(void);
 #endif
 
-    void markTarget(void);
-
     int findWaypoint(int mouseX, int mouseY);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
@@ -123,7 +130,6 @@ private:
     bool displayGrid;
     bool displayTrail;
     bool displayImagery;
-    bool displayTarget;
     bool displayWaypoints;
     bool displayRGBD2D;
     bool displayRGBD3D;
@@ -147,9 +153,7 @@ private:
     osg::ref_ptr<osg::Geometry> trailGeometry;
     osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
     osg::ref_ptr<Imagery> mapNode;
-    osg::ref_ptr<osg::Geode> targetNode;
-    osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
-    osg::ref_ptr<osg::Group> waypointsNode;
+    osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
 #ifdef QGC_LIBFREENECT_ENABLED
     QScopedPointer<Freenect> freenect;
@@ -161,8 +165,7 @@ private:
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
-    QPushButton* targetButton;
-
+    MAV_FRAME frame;
     double lastRobotX, lastRobotY, lastRobotZ;
 };
 

src/ui/map3D/Q3DWidget.cc

@@ -168,6 +168,7 @@ Q3DWidget::createHUD(void)
     hudGroup->setStateSet(hudStateSet);
     hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
     hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    hudStateSet->setMode(GL_BLEND, osg::StateAttribute::ON);
     hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
     hudStateSet->setRenderBinDetails(11, "RenderBin");
 

src/ui/map3D/WaypointGroupNode.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 WaypointGroupNode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "WaypointGroupNode.h"
+
+#include <osg/LineWidth>
+#include <osg/PositionAttitudeTransform>
+#include <osg/ShapeDrawable>
+
+#include "Imagery.h"
+
+WaypointGroupNode::WaypointGroupNode()
+{
+
+}
+
+void
+WaypointGroupNode::init(void)
+{
+
+}
+
+void
+WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+{
+    if (uas)
+    {
+        double robotX, robotY, robotZ;
+        if (frame == MAV_FRAME_GLOBAL)
+        {
+            double latitude = uas->getLatitude();
+            double longitude = uas->getLongitude();
+            double altitude = uas->getAltitude();
+
+            QString utmZone;
+            Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
+            robotZ = -altitude;
+        }
+        else if (frame == MAV_FRAME_LOCAL)
+        {
+            robotX = uas->getLocalX();
+            robotY = uas->getLocalY();
+            robotZ = uas->getLocalZ();
+        }
+
+        if (getNumChildren() > 0)
+        {
+            removeChild(0, getNumChildren());
+        }
+
+        const QVector<Waypoint *>& list = uas->getWaypointManager().getWaypointList();
+
+        for (int i = 0; i < list.size(); i++)
+        {
+            Waypoint* wp = list.at(i);
+
+            double wpX, wpY, wpZ;
+            getPosition(wp, wpX, wpY, wpZ);
+
+            osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+            osg::ref_ptr<osg::Cylinder> cylinder =
+                    new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
+                                      wp->getOrbit(),
+                                      fabs(wpZ));
+
+            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));
+            }
+
+            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)
+            {
+                double nextWpX, nextWpY, nextWpZ;
+                getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
+
+                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, -wpZ));
+                vertices->push_back(osg::Vec3d(nextWpY - wpY,
+                                               nextWpX - wpX,
+                                               -nextWpZ));
+                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);
+                geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+
+                geode->addDrawable(geometry);
+            }
+
+            osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+                    new osg::PositionAttitudeTransform;
+
+            pat->setPosition(osg::Vec3d(wpY - robotY,
+                                        wpX - robotX,
+                                        robotZ));
+
+            addChild(pat);
+            pat->addChild(geode);
+        }
+    }
+}
+
+void
+WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
+{
+    if (wp->getFrame() == MAV_FRAME_GLOBAL)
+    {
+        double latitude = wp->getY();
+        double longitude = wp->getX();
+        double altitude = wp->getZ();
+
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+        z = -altitude;
+    }
+    else if (wp->getFrame() == MAV_FRAME_LOCAL)
+    {
+        x = wp->getX();
+        y = wp->getY();
+        z = wp->getZ();
+    }
+}

src/ui/map3D/WaypointGroupNode.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 WaypointGroupNode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef WAYPOINTGROUPNODE_H
+#define WAYPOINTGROUPNODE_H
+
+#include <osg/Group>
+
+#include "UASInterface.h"
+
+class WaypointGroupNode : public osg::Group
+{
+public:
+    WaypointGroupNode();
+
+    void init(void);
+    void update(MAV_FRAME frame, UASInterface* uas);
+
+private:
+    void getPosition(Waypoint* wp, double& x, double& y, double& z);
+};
+
+#endif // WAYPOINTGROUPNODE_H