From 6c5ff745652de858d6349b2d9ed579cda944fd22 Mon Sep 17 00:00:00 2001 From: lm Date: Thu, 17 Feb 2011 15:15:32 +0100 Subject: [PATCH] Fully working drag and drop for Google Earth --- images/earth.html | 32 ++++++++++++++++++++++++-- src/uas/UAS.cc | 2 +- src/ui/QGCParamWidget.cc | 2 +- src/ui/WaypointView.ui | 2 +- src/ui/map3D/QGCGoogleEarthView.cc | 47 +++++++++++++++++++++++++++++++++++++- src/ui/map3D/QGCGoogleEarthView.h | 13 +++++++++++ src/ui/map3D/QGCGoogleEarthView.ui | 13 ++++++++--- 7 files changed, 102 insertions(+), 9 deletions(-) diff --git a/images/earth.html b/images/earth.html index 721003b..bed5775 100644 --- a/images/earth.html +++ b/images/earth.html @@ -37,6 +37,11 @@ var currTilt = 40.0; ///<< The tilt angle (in degrees) var currFollowTilt = 40.0; var currView = null; +var viewMode = 0; +var lastTilt = 0; +var lastRoll = 0; +var lastHeading = 0; + var M_PI = 3.14159265; @@ -486,11 +491,34 @@ function setCurrentAircraft(id) /** @brief Set the current view mode * - * @param mode 0: map view, 1: chase cam, fixed, 2: chase cam, free + * @param mode 0: side map view, 1: top/north map view, 2: fixed chase cam, 3: free chase cam */ function setViewMode(mode) { - + viewMode = mode; + + var currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE); + + if (mode == 0) + { + currView.setTilt(0); + currView.setHeading(0); + } + if (mode == 1) + { + lastTilt = currView.getTilt(); + lastHeading = currView.getHeading(); + var lastLat2 = currView.getLatitude(); + var lastLon2 = currView.getLongitude(); + var lastAlt2 = currView.getAltitude(); + currView.setTilt(0); + currView.setHeading(0); + currView.setLatitude(lastLat2); + currView.setLongitude(lastLon2); + currView.setAltitude(lastAlt2); + } + + ge.getView().setAbstractView(currView); } function updateFollowAircraft() diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3ee6738..9d0eb3e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -661,7 +661,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) alt = 0; emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE"); } - emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E7, time); + emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); // Smaller than threshold and not NaN if (pos.v < 1000000 && pos.v == pos.v) { diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 1cfcd08..d289338 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -258,7 +258,7 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa bool justWritten = false; bool writeMismatch = false; - bool lastWritten = false; + //bool lastWritten = false; // Mark this parameter as received in write ACK list QMap* map = transmissionMissingWriteAckPackets.value(component); if (map && map->contains(parameterName)) diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui index 6a3b949..4239009 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointView.ui @@ -161,7 +161,7 @@ QProgressBar::chunk#thrustBar { - + 2 diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index ae26b3c..5e4c9d3 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -38,6 +38,7 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : webViewInitialized(false), jScriptInitialized(false), gEarthInitialized(false), + currentViewMode(QGCGoogleEarthView::VIEW_MODE_SIDE), #if (defined Q_OS_MAC) webViewMac(new QWebView(this)), #endif @@ -85,6 +86,7 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateState())); connect(ui->resetButton, SIGNAL(clicked()), this, SLOT(reloadHTML())); + connect(ui->changeViewButton, SIGNAL(clicked()), this, SLOT(toggleViewMode())); } QGCGoogleEarthView::~QGCGoogleEarthView() @@ -127,6 +129,46 @@ void QGCGoogleEarthView::setViewRange(float range) javaScript(QString("setViewRange(%1);").arg(range, 0, 'f', 5)); } +void QGCGoogleEarthView::toggleViewMode() +{ + switch (currentViewMode) + { + case VIEW_MODE_MAP: + setViewMode(VIEW_MODE_SIDE); + break; + case VIEW_MODE_SIDE: + setViewMode(VIEW_MODE_MAP); + break; + case VIEW_MODE_CHASE_LOCKED: + setViewMode(VIEW_MODE_CHASE_FREE); + break; + case VIEW_MODE_CHASE_FREE: + setViewMode(VIEW_MODE_CHASE_LOCKED); + break; + } +} + +void QGCGoogleEarthView::setViewMode(QGCGoogleEarthView::VIEW_MODE mode) +{ + switch (mode) + { + case VIEW_MODE_MAP: + ui->changeViewButton->setText("Free View"); + break; + case VIEW_MODE_SIDE: + ui->changeViewButton->setText("Map View"); + break; + case VIEW_MODE_CHASE_LOCKED: + ui->changeViewButton->setText("Free Chase"); + break; + case VIEW_MODE_CHASE_FREE: + ui->changeViewButton->setText("Fixed Chase"); + break; + } + currentViewMode = mode; + javaScript(QString("setViewMode(%1);").arg(mode)); +} + void QGCGoogleEarthView::addUAS(UASInterface* uas) { // uasid, type, color (in #rrbbgg format) @@ -183,7 +225,7 @@ void QGCGoogleEarthView::updateWaypoint(int uas, Waypoint* wp) else { javaScript(QString("updateWaypoint(%1,%2,%3,%4,%5,%6);").arg(uas).arg(wpindex).arg(wp->getLatitude(), 0, 'f', 18).arg(wp->getLongitude(), 0, 'f', 18).arg(wp->getAltitude(), 0, 'f', 18).arg(wp->getAction())); - qDebug() << QString("updateWaypoint(%1,%2,%3,%4,%5,%6);").arg(uas).arg(wpindex).arg(wp->getLatitude(), 0, 'f', 18).arg(wp->getLongitude(), 0, 'f', 18).arg(wp->getAltitude(), 0, 'f', 18).arg(wp->getAction()); + //qDebug() << QString("updateWaypoint(%1,%2,%3,%4,%5,%6);").arg(uas).arg(wpindex).arg(wp->getLatitude(), 0, 'f', 18).arg(wp->getLongitude(), 0, 'f', 18).arg(wp->getAltitude(), 0, 'f', 18).arg(wp->getAction()); } } } @@ -453,6 +495,9 @@ void QGCGoogleEarthView::initializeGoogleEarth() // Start update timer updateTimer->start(refreshRateMs); + // Set current view mode + setViewMode(currentViewMode); + follow(this->followCamera); gEarthInitialized = true; diff --git a/src/ui/map3D/QGCGoogleEarthView.h b/src/ui/map3D/QGCGoogleEarthView.h index f4e29a3..994cca3 100644 --- a/src/ui/map3D/QGCGoogleEarthView.h +++ b/src/ui/map3D/QGCGoogleEarthView.h @@ -71,6 +71,14 @@ public: explicit QGCGoogleEarthView(QWidget *parent = 0); ~QGCGoogleEarthView(); + enum VIEW_MODE + { + VIEW_MODE_SIDE, ///< View from above, orientation is free + VIEW_MODE_MAP, ///< View from above, orientation is north (map view) + VIEW_MODE_CHASE_LOCKED, ///< Locked chase camera + VIEW_MODE_CHASE_FREE, ///< Position is chasing object, but view can rotate around object + }; + public slots: /** @brief Update the internal state. Does not trigger a redraw */ void updateState(); @@ -96,6 +104,10 @@ public slots: void setHome(double lat, double lon, double alt); /** @brief Set camera view range to aircraft in meters */ void setViewRange(float range); + /** @brief Set the view mode */ + void setViewMode(VIEW_MODE mode); + /** @brief Toggle through the different view modes */ + void toggleViewMode(); /** @brief Set camera view range to aircraft in centimeters */ void setViewRangeScaledInt(int range); /** @brief Reset Google Earth View */ @@ -123,6 +135,7 @@ protected: bool webViewInitialized; bool jScriptInitialized; bool gEarthInitialized; + VIEW_MODE currentViewMode; #ifdef _MSC_VER QGCWebAxWidget* webViewWin; QAxObject* jScriptWin; diff --git a/src/ui/map3D/QGCGoogleEarthView.ui b/src/ui/map3D/QGCGoogleEarthView.ui index b98f097..ec6e4ba 100644 --- a/src/ui/map3D/QGCGoogleEarthView.ui +++ b/src/ui/map3D/QGCGoogleEarthView.ui @@ -23,7 +23,7 @@ 2 - + @@ -58,7 +58,7 @@ - + Qt::Horizontal @@ -180,13 +180,20 @@ - + Reset + + + + Overhead + + +