diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 43e8dc6f1dbc754d7da6b97b50c13669469648a2..334773c5fb12fa6287e826fdc571d37a1fbf664a 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -389,6 +389,10 @@ void MAVLinkSimulationLink::mainloop() y = (y < -5.0f) ? -5.0f : y; z = (z < -3.0f) ? -3.0f : z; + // position at Pixhawk lab @ ETHZ + x += 5247273.0f; + y += 465955.0f; + // Send back new setpoint mavlink_message_t ret; mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw); diff --git a/src/ui/map3D/Imagery.cc b/src/ui/map3D/Imagery.cc index 14a1112241d1f325f25de366711916dc03fd16e5..001506b00362faa7aeb1282a9c283fce17e973fe 100644 --- a/src/ui/map3D/Imagery.cc +++ b/src/ui/map3D/Imagery.cc @@ -214,6 +214,7 @@ Imagery::draw3D(double radius, double imageResolution, centerTileX, centerTileY, zoomLevel); UTMtoTile(maxX, maxY, utmZone, imageResolution, maxTileX, minTileY, zoomLevel); + if (maxTileX - minTileX + 1 > 14) { minTileX = centerTileX - 7; @@ -235,6 +236,7 @@ Imagery::draw3D(double radius, double imageResolution, imageBounds(c, r, imageResolution, x1, y1, x2, y2, x3, y3, x4, y4); TexturePtr t = textureCache->get(tileURL); + if (!t.isNull()) { t->draw(x1 - xOrigin, y1 - yOrigin, @@ -249,6 +251,8 @@ Imagery::draw3D(double radius, double imageResolution, bool Imagery::update(void) { + textureCache->sync(); + return true; } @@ -257,7 +261,7 @@ Imagery::imageBounds(int32_t x, int32_t y, double imageResolution, double& x1, double& y1, double& x2, double& y2, double& x3, double& y3, double& x4, double& y4) { - int32_t zoomLevel = 17 - static_cast(rint(log2(imageResolution))); + int32_t zoomLevel = 19 - static_cast(rint(log2(imageResolution))); int32_t numTiles = static_cast(exp2(static_cast(zoomLevel))); double lon1 = 360.0 * (static_cast(x) @@ -273,10 +277,10 @@ Imagery::imageBounds(int32_t x, int32_t y, double imageResolution, * 2.0 * M_PI - M_PI); QString utmZone; - LLtoUTM(lat1, lon1, y1, x1, utmZone); - LLtoUTM(lat1, lon2, y2, x2, utmZone); - LLtoUTM(lat2, lon2, y3, x3, utmZone); - LLtoUTM(lat2, lon1, y4, x4, utmZone); + LLtoUTM(lat2, lon1, x1, y1, utmZone); + LLtoUTM(lat2, lon2, x2, y2, utmZone); + LLtoUTM(lat1, lon2, x3, y3, utmZone); + LLtoUTM(lat1, lon1, x4, y4, utmZone); } double @@ -302,7 +306,7 @@ Imagery::UTMtoTile(double northing, double easting, const QString& utmZone, UTMtoLL(northing, easting, utmZone, latitude, longitude); - zoomLevel = 17 - static_cast(rint(log2(imageResolution))); + zoomLevel = 19 - static_cast(rint(log2(imageResolution))); int32_t numTiles = static_cast(exp2(static_cast(zoomLevel))); double x = longitude / 180.0; diff --git a/src/ui/map3D/QMap3DWidget.cc b/src/ui/map3D/QMap3DWidget.cc index 2aaad841dabfdda323164e173a81407239194fff..b926b5e32f801ae638af3fd50c85557de0500769 100644 --- a/src/ui/map3D/QMap3DWidget.cc +++ b/src/ui/map3D/QMap3DWidget.cc @@ -51,7 +51,7 @@ QMap3DWidget::QMap3DWidget(QWidget* parent) , updateLastUnlockedPose(true) , displayTarget(false) , displayWaypoints(true) - , imagery(new Imagery) + , imagery(0) { setFocusPolicy(Qt::StrongFocus); @@ -223,7 +223,7 @@ QMap3DWidget::displayHandler(void) if (displayImagery) { - drawImagery(robotX, robotY, "32N", true); + drawImagery(robotX, robotY, "32T", true); } glPopMatrix(); @@ -389,9 +389,15 @@ QMap3DWidget::timer(void* clientData) void QMap3DWidget::timerHandler(void) { + if (imagery.isNull()) + { + imagery.reset(new Imagery); + } + double timeLapsed = getTime() - lastRedrawTime; if (timeLapsed > 0.1) { + imagery->update(); forceRedraw(); lastRedrawTime = getTime(); } @@ -598,6 +604,8 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, glPushMatrix(); glEnable(GL_BLEND); + glTranslatef(0, 0, 0.1); + CameraPose camPose = getCameraPose(); double viewingRadius = camPose.distance / 4000.0 * 3000.0; if (viewingRadius < 100.0) @@ -605,8 +613,8 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, viewingRadius = 100.0; } - double minResolution = 0.25; - double centerResolution = camPose.distance / 160.0 * 0.25; + double minResolution = 0.125; + double centerResolution = camPose.distance / 160.0; double maxResolution = 2.0; double resolution = minResolution; @@ -619,7 +627,7 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, resolution = maxResolution; } - imagery->draw3D(viewingRadius, resolution, originX, originY, camPose.yOffset, camPose.xOffset, zone); + imagery->draw3D(viewingRadius, resolution, originX, originY, camPose.xOffset, camPose.yOffset, zone); if (prefetch) { @@ -627,13 +635,13 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, { imagery->prefetch3D(viewingRadius / 2.0, resolution / 2.0, originX, originY, - camPose.yOffset, camPose.xOffset, zone); + camPose.xOffset, camPose.yOffset, zone); } if (resolution * 2.0 <= maxResolution) { imagery->prefetch3D(viewingRadius * 2.0, resolution * 2.0, originX, originY, - camPose.yOffset, camPose.xOffset, zone); + camPose.xOffset, camPose.yOffset, zone); } } diff --git a/src/ui/map3D/Texture.cc b/src/ui/map3D/Texture.cc index 0a073227718173dc20aa2e4d98034737883d50e2..62f59d0b5176f7ea2c0ebb83e3855951dab6ec7d 100644 --- a/src/ui/map3D/Texture.cc +++ b/src/ui/map3D/Texture.cc @@ -55,13 +55,12 @@ Texture::sync(const WebImagePtr& image) glBindTexture(GL_TEXTURE_2D, id); glTexImage2D(GL_TEXTURE_2D, 0, 3, textureWidth, textureHeight, - 0, GL_RGB, GL_UNSIGNED_BYTE, NULL); + 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL); } glBindTexture(GL_TEXTURE_2D, id); glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, imageWidth, imageHeight, - GL_RGB, GL_UNSIGNED_BYTE, image->getData()); - + GL_RGBA, GL_UNSIGNED_BYTE, image->getData()); } } diff --git a/src/ui/map3D/TextureCache.cc b/src/ui/map3D/TextureCache.cc index 34b2311bff860109f7045bf92461c9bc20664ca6..5fb21038319d47b38bda90f5e5f863185560cb11 100644 --- a/src/ui/map3D/TextureCache.cc +++ b/src/ui/map3D/TextureCache.cc @@ -7,6 +7,7 @@ TextureCache::TextureCache(uint32_t _cacheSize) for (uint32_t i = 0; i < cacheSize; ++i) { TexturePtr t(new Texture); + GLuint id; glGenTextures(1, &id); t->setID(id); diff --git a/src/ui/map3D/WebImage.cc b/src/ui/map3D/WebImage.cc index 107579df6c929667e79d24efb61dbfcf03bf79b4..e915c154f3fa4f8d4dc7e62738fb15988765d2bc 100644 --- a/src/ui/map3D/WebImage.cc +++ b/src/ui/map3D/WebImage.cc @@ -1,5 +1,8 @@ #include "WebImage.h" +#include +#include + WebImage::WebImage() : state(WebImage::UNINITIALIZED) , sourceURL("") @@ -46,13 +49,25 @@ WebImage::setSourceURL(const QString& url) const uint8_t* WebImage::getData(void) const { - return image->bits(); + return image->scanLine(0); } void WebImage::setData(const QByteArray& data) { - image->loadFromData(data); + QImage tempImage; + if (tempImage.loadFromData(data)) + { + if (image.isNull()) + { + image = QSharedPointer(new QImage); + } + *image = QGLWidget::convertToGLFormat(tempImage); + } + else + { + qDebug() << "# WARNING: cannot load image data for" << sourceURL; + } } int32_t @@ -67,6 +82,12 @@ WebImage::getHeight(void) const return image->height(); } +int32_t +WebImage::getByteCount(void) const +{ + return image->byteCount(); +} + uint64_t WebImage::getLastReference(void) const { diff --git a/src/ui/map3D/WebImage.h b/src/ui/map3D/WebImage.h index ec489a9a3d97a5c139c72e2545d4aa93de44b75a..15fec65ca2ed57c11e6d7c012d83fcae4ee25edc 100644 --- a/src/ui/map3D/WebImage.h +++ b/src/ui/map3D/WebImage.h @@ -3,6 +3,7 @@ #include #include +#include #include class WebImage @@ -30,6 +31,7 @@ public: int32_t getWidth(void) const; int32_t getHeight(void) const; + int32_t getByteCount(void) const; uint64_t getLastReference(void) const; void setLastReference(uint64_t value);