diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 56337c48a7f64a48a16750e311597d68ad23ea8f..6d6ecf93579c3a6044ddc80ea6b8787850df0033 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/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(QGC::groundTimeUsecs()) * 0.08f)); y = y*0.93f + 0.07f*(y+sin(static_cast(QGC::groundTimeUsecs()) * 0.5f)); z = z*0.93f + 0.07f*(z+sin(static_cast(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); diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index d29cf5dde715baf1380173dfc12899f58b452de3..cd6643287efe6c52e09349ee576bf128a3ebae6b 100644 --- a/src/input/Freenect.cc +++ b/src/input/Freenect.cc @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class Freenect. + * + * @author Lionel Heng + * + */ + #include "Freenect.h" #include diff --git a/src/input/Freenect.h b/src/input/Freenect.h index 216b4d49c8c4830f5e01d8558e73b0d5f7ed6854..4ff4aa8571a56658dec53d7523ace3b9c36c0aeb 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class Freenect. + * + * @author Lionel Heng + * + */ + #ifndef FREENECT_H #define FREENECT_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3ca6c54ac8b2a7afd6e73e2d973519b3f90922f6..ab84b09eff5c7659f3bfe1ea59fc05a9a5fe67fe 100644 --- a/src/uas/UAS.cc +++ b/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); diff --git a/src/uas/UAS.h b/src/uas/UAS.h index da8c44c0e8a4645a19e1e6f84139f5676a5fa64b..6cbffc87612ea2c4a295a189012ef2b618dc7aa4 100644 --- a/src/uas/UAS.h +++ b/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; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index b4b348d9591c9ebb69cea986b8ce3694d4948ec4..32825bc5b49f1504ed07b3a8f34de460634e35bb 100644 --- a/src/uas/UASInterface.h +++ b/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; diff --git a/src/ui/map3D/Imagery.cc b/src/ui/map3D/Imagery.cc index 8a0c22bc3876765247ef5fcbaa4014efd079b9a6..e0ff4752c1b7e824b9a1fabe7932cf2064763176 100644 --- a/src/ui/map3D/Imagery.cc +++ b/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. diff --git a/src/ui/map3D/Imagery.h b/src/ui/map3D/Imagery.h index 224ef258c169d72a8f65605fa3a2846f9aef4647..b20f846ab28de03049b3492e999a3a42801e3514 100644 --- a/src/ui/map3D/Imagery.h +++ b/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; diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index df0f641f3832b93fd0c9d5bd597b2b906c9b32df..322add368712ba24415ab1084ec89aa24f5d5b27 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/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 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); diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index b7e251a857a40d350275959347e79417cc096c94..c1b5fdad89c764901f339511487885735e5b8029 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/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 diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index 6c9de7149913fb6e5d7b1201def5fdd7ffd1f419..797ff6120aea882c6c40e8bdca8b165ef5230d79 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/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 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 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);