Fixed imagery rendering issues. Robot position in Pixhawk3DWidget is now...

Fixed imagery rendering issues. Robot position in Pixhawk3DWidget is now derived from the global pose (lat/lon/alt) instead of the local pose.

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,12 +73,15 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
         sendDropRate(0),
         lowBattAlarm(false),
         positionLock(false),
-        localX(0),
-        localY(0),
-        localZ(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();
@@ -342,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

@@ -83,6 +83,10 @@ public:
     double getLocalY() const { return localY; };
     double getLocalZ() const { return localZ; };
 
+    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; };
     double getYaw() const { return yaw; };
@@ -139,6 +143,9 @@ protected:
     double localX;
     double localY;
     double localZ;
+    double latitude;
+    double longitude;
+    double altitude;
     double roll;
     double pitch;
     double yaw;

src/uas/UASInterface.h

@@ -70,6 +70,10 @@ public:
     virtual double getLocalY() const = 0;
     virtual double getLocalZ() 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;
     virtual double getYaw() 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,
                 const QString& utmZone)
 {
     if (getNumDrawables() > 0)
@@ -171,10 +172,10 @@ 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,
                                     true));
             }
         }
@@ -213,6 +214,7 @@ Imagery::prefetch3D(double radius, double tileResolution,
 void
 Imagery::draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
+                double xOffset, double yOffset,
                 const QString& utmZone)
 {
     if (getNumDrawables() > 0)
@@ -246,10 +248,10 @@ 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,
                                     true));
             }
         }
@@ -397,7 +399,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 +434,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 +515,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,
                 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,
                 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

@@ -78,7 +78,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 
     // generate map model
     mapNode = createMap();
-    allocentricMap->addChild(mapNode);
+    rollingMap->addChild(mapNode);
 
     // generate target model
     allocentricMap->addChild(createTarget());
@@ -203,9 +203,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);
@@ -358,9 +362,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 +408,7 @@ Pixhawk3DWidget::display(void)
 
     if (displayImagery)
     {
-        updateImagery();
+        updateImagery(robotX, robotY, utmZone);
     }
 
     if (displayTarget)
@@ -422,7 +433,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)
@@ -779,15 +790,14 @@ Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
 }
 
 void
-Pixhawk3DWidget::updateImagery(void)
+Pixhawk3DWidget::updateImagery(double originX, double originY,
+                               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 +838,8 @@ Pixhawk3DWidget::updateImagery(void)
                     resolution,
                     cameraManipulator->getCenter().y(),
                     cameraManipulator->getCenter().x(),
+                    originX,
+                    originY,
                     zone);
 
     // prefetch map tiles
@@ -894,6 +906,13 @@ 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);
+
         if (waypointsNode->getNumChildren() > 0)
         {
             waypointsNode->removeChild(0, waypointsNode->getNumChildren());
@@ -923,8 +942,8 @@ Pixhawk3DWidget::updateWaypoints(void)
             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(),
+            pat->setPosition(osg::Vec3d(list.at(i)->getY() - robotY,
+                                        list.at(i)->getX() - robotX,
                                         0.0));
 
             waypointsNode->addChild(pat);

src/ui/map3D/Pixhawk3DWidget.h

@@ -91,7 +91,7 @@ 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, const QString& zone);
     void updateTarget(void);
     void updateWaypoints(void);
 #ifdef QGC_LIBFREENECT_ENABLED

src/ui/map3D/Q3DWidget.cc

@@ -71,6 +71,9 @@ void
 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);
 
@@ -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);