QGCGoogleEarthView.cc 25.1 KB
Newer Older
1 2 3 4
#include <QApplication>
#include <QDir>
#include <QShowEvent>
#include <QSettings>
5
#include <QInputDialog>
6 7 8 9 10 11 12 13 14

#include <QDebug>
#include <QFile>
#include <QTextStream>
#include "UASManager.h"

#ifdef Q_OS_MAC
#include <QWebFrame>
#include <QWebPage>
15
#include <QWebElement>
16 17 18
#include "QGCWebPage.h"
#endif

19 20 21
#ifdef _MSC_VER
#include <QAxObject>
#include <QUuid>
22
#include <mshtml.h>
23 24
#include <comdef.h>
#include <qaxtypes.h>
25 26 27
#endif

#include "QGC.h"
28 29
#include "ui_QGCGoogleEarthView.h"
#include "QGCGoogleEarthView.h"
pixhawk's avatar
pixhawk committed
30
#include "UASWaypointManager.h"
31 32 33 34

#define QGCGOOGLEEARTHVIEWSETTINGS QString("GoogleEarthViewSettings_")

QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) :
35 36 37 38 39 40 41 42 43 44
    QWidget(parent),
    updateTimer(new QTimer(this)),
    refreshRateMs(100),
    mav(NULL),
    followCamera(true),
    trailEnabled(true),
    webViewInitialized(false),
    jScriptInitialized(false),
    gEarthInitialized(false),
    currentViewMode(QGCGoogleEarthView::VIEW_MODE_SIDE),
45
#if (defined Q_OS_MAC)
46
    webViewMac(new QWebView(this)),
47 48
#endif
#ifdef _MSC_VER
49 50
    webViewWin(new QGCWebAxWidget(this)),
    documentWin(NULL),
51
#endif
52
    ui(new Ui::QGCGoogleEarthView)
53 54 55 56
{
#ifdef _MSC_VER
    // Create layout and attach webViewWin

57 58 59 60 61 62 63 64 65
    QFile file("doc.html");
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
        qDebug() << __FILE__ << __LINE__ << "Could not open log file";

    QTextStream out(&file);
    out << webViewWin->generateDocumentation();
    out.flush();
    file.flush();
    file.close();
66 67 68 69 70 71 72 73 74 75 76 77 78


#else
#endif

    // Load settings
    QSettings settings;
    followCamera = settings.value(QGCGOOGLEEARTHVIEWSETTINGS + "follow", followCamera).toBool();
    trailEnabled = settings.value(QGCGOOGLEEARTHVIEWSETTINGS + "trail", trailEnabled).toBool();

    ui->setupUi(this);
#if (defined Q_OS_MAC)
    ui->webViewLayout->addWidget(webViewMac);
pixhawk's avatar
pixhawk committed
79
    //connect(webViewMac, SIGNAL(loadFinished(bool)), this, SLOT(initializeGoogleEarth(bool)));
80 81 82 83 84 85 86
#endif

#ifdef _MSC_VER
    ui->webViewLayout->addWidget(webViewWin);
#endif

    connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateState()));
pixhawk's avatar
pixhawk committed
87
    connect(ui->resetButton, SIGNAL(clicked()), this, SLOT(reloadHTML()));
88
    connect(ui->changeViewButton, SIGNAL(clicked()), this, SLOT(toggleViewMode()));
89
    connect(ui->clearTrailsButton, SIGNAL(clicked()), this, SLOT(clearTrails()));
90 91
    connect(ui->atmosphereCheckBox, SIGNAL(clicked(bool)), this, SLOT(enableAtmosphere(bool)));
    connect(ui->daylightCheckBox, SIGNAL(clicked(bool)), this, SLOT(enableDaylight(bool)));
92 93

    connect(UASManager::instance(), SIGNAL(homePositionChanged(double,double,double)), this, SLOT(setHome(double,double,double)));
94 95 96 97 98 99 100 101
}

QGCGoogleEarthView::~QGCGoogleEarthView()
{
    QSettings settings;
    settings.setValue(QGCGOOGLEEARTHVIEWSETTINGS + "follow", followCamera);
    settings.setValue(QGCGOOGLEEARTHVIEWSETTINGS + "trail", trailEnabled);
    settings.sync();
102
#if (defined Q_OS_MAC)
103
    delete webViewMac;
104 105
#endif
#ifdef _MSC_VER
106
    delete webViewWin;
107
#endif
108 109 110
    delete ui;
}

111 112 113 114 115 116 117 118
/**
 * @param range in centimeters
 */
void QGCGoogleEarthView::setViewRangeScaledInt(int range)
{
    setViewRange(range/100.0f);
}

pixhawk's avatar
pixhawk committed
119 120 121 122 123 124 125 126 127
void QGCGoogleEarthView::reloadHTML()
{
    hide();
    webViewInitialized = false;
    jScriptInitialized = false;
    gEarthInitialized = false;
    show();
}

128 129 130 131 132
void QGCGoogleEarthView::enableEditMode(bool mode)
{
    javaScript(QString("setDraggingAllowed(%1);").arg(mode));
}

133 134 135 136 137 138 139 140 141 142
void QGCGoogleEarthView::enableDaylight(bool enable)
{
    javaScript(QString("enableDaylight(%1);").arg(enable));
}

void QGCGoogleEarthView::enableAtmosphere(bool enable)
{
    javaScript(QString("enableAtmosphere(%1);").arg(enable));
}

143 144 145 146 147 148 149 150
/**
 * @param range in meters (SI-units)
 */
void QGCGoogleEarthView::setViewRange(float range)
{
    javaScript(QString("setViewRange(%1);").arg(range, 0, 'f', 5));
}

151 152 153 154 155
void QGCGoogleEarthView::setDistanceMode(int mode)
{
    javaScript(QString("setDistanceMode(%1);").arg(mode));
}

156 157
void QGCGoogleEarthView::toggleViewMode()
{
158
    switch (currentViewMode) {
159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175
    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)
{
176
    switch (mode) {
177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
    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));
}

194 195
void QGCGoogleEarthView::addUAS(UASInterface* uas)
{
196 197 198 199 200 201 202 203 204 205 206 207
    // uasid, type, color (in #rrbbgg format)
    QString uasColor = uas->getColor().name().remove(0, 1);
    // Convert to bbggrr format
    QString rChannel = uasColor.mid(0, 2);
    uasColor.remove(0, 2);
    uasColor.prepend(rChannel);
    // Set alpha value to 0x66, append JavaScript quotes ('')
    uasColor.prepend("'66").append("'");
    javaScript(QString("createAircraft(%1, %2, %3);").arg(uas->getUASID()).arg(uas->getSystemType()).arg(uasColor));

    if (trailEnabled) javaScript(QString("showTrail(%1);").arg(uas->getUASID()));

208 209
    // Automatically receive further position updates
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
pixhawk's avatar
pixhawk committed
210 211 212 213 214 215
    // Receive waypoint updates
    // Connect the waypoint manager / data storage to the UI
    connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
    connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
    //connect(this, SIGNAL(waypointCreated(Waypoint*)), uas->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
    // TODO Update waypoint list on UI changes here
216 217 218 219
}

void QGCGoogleEarthView::setActiveUAS(UASInterface* uas)
{
220
    if (uas) {
221
        mav = uas;
222
        javaScript(QString("setCurrAircraft(%1);").arg(uas->getUASID()));
223
        updateWaypointList(uas->getUASID());
224 225 226
    }
}

pixhawk's avatar
pixhawk committed
227 228 229 230 231 232 233
/**
 * This function is called if a a single waypoint is updated and
 * also if the whole list changes.
 */
void QGCGoogleEarthView::updateWaypoint(int uas, Waypoint* wp)
{
    // Only accept waypoints in global coordinate frame
234
    if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->isNavigationType()) {
pixhawk's avatar
pixhawk committed
235 236 237
        // We're good, this is a global waypoint

        // Get the index of this waypoint
lm's avatar
lm committed
238
        // note the call to getGlobalFrameAndNavTypeIndexOf()
pixhawk's avatar
pixhawk committed
239
        // as we're only handling global waypoints
lm's avatar
lm committed
240
        int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeIndexOf(wp);
pixhawk's avatar
pixhawk committed
241
        // If not found, return (this should never happen, but helps safety)
242
        if (wpindex == -1) {
pixhawk's avatar
pixhawk committed
243
            return;
244
        } else {
lm's avatar
lm committed
245
            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()));
246
            //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());
pixhawk's avatar
pixhawk committed
247 248 249 250 251 252 253 254 255 256 257 258 259
        }
    }
}

/**
 * Update the whole list of waypoints. This is e.g. necessary if the list order changed.
 * The UAS manager will emit the appropriate signal whenever updating the list
 * is necessary.
 */
void QGCGoogleEarthView::updateWaypointList(int uas)
{
    // Get already existing waypoints
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
260
    if (uasInstance) {
pixhawk's avatar
pixhawk committed
261
        // Get all waypoints, including non-global waypoints
lm's avatar
lm committed
262
        QVector<Waypoint*> wpList = uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList();
pixhawk's avatar
pixhawk committed
263 264 265 266

        // Trim internal list to number of global waypoints in the waypoint manager list
        javaScript(QString("updateWaypointListLength(%1,%2);").arg(uas).arg(wpList.count()));

lm's avatar
lm committed
267 268
        qDebug() << QString("updateWaypointListLength(%1,%2);").arg(uas).arg(wpList.count());

pixhawk's avatar
pixhawk committed
269
        // Load all existing waypoints into map view
270
        foreach (Waypoint* wp, wpList) {
pixhawk's avatar
pixhawk committed
271 272 273 274 275
            updateWaypoint(uas, wp);
        }
    }
}

276
void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec)
277 278
{
    Q_UNUSED(usec);
lm's avatar
lm committed
279
    javaScript(QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 18).arg(lon, 0, 'f', 18).arg(alt, 0, 'f', 15));
280

lm's avatar
lm committed
281
    //qDebug() << QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15);
282 283
}

284 285 286
void QGCGoogleEarthView::clearTrails()
{
    QList<UASInterface*> mavs = UASManager::instance()->getUASList();
287
    foreach (UASInterface* currMav, mavs) {
288 289 290 291
        javaScript(QString("clearTrail(%1);").arg(currMav->getUASID()));
    }
}

292 293
void QGCGoogleEarthView::showTrail(bool state)
{
294
    // Check if the current trail has to be hidden
295
    if (trailEnabled && !state) {
296
        QList<UASInterface*> mavs = UASManager::instance()->getUASList();
297
        foreach (UASInterface* currMav, mavs) {
298
            javaScript(QString("hideTrail(%1);").arg(currMav->getUASID()));
299 300 301 302
        }
    }

    // Check if the current trail has to be shown
303
    if (!trailEnabled && state) {
304
        QList<UASInterface*> mavs = UASManager::instance()->getUASList();
305
        foreach (UASInterface* currMav, mavs) {
306 307 308 309
            javaScript(QString("showTrail(%1);").arg(currMav->getUASID()));
        }
    }
    trailEnabled = state;
310 311 312 313 314
    ui->trailCheckbox->setChecked(state);
}

void QGCGoogleEarthView::showWaypoints(bool state)
{
pixhawk's avatar
pixhawk committed
315
    waypointsEnabled = state;
316 317 318 319 320
}

void QGCGoogleEarthView::follow(bool follow)
{
    ui->followAirplaneCheckbox->setChecked(follow);
321 322
    if (follow != followCamera) {
        if (follow) {
323
            setViewMode(VIEW_MODE_CHASE_LOCKED);
324
        } else {
325 326 327
            setViewMode(VIEW_MODE_SIDE);
        }
    }
328
    followCamera = follow;
329
    javaScript(QString("setFollowEnabled(%1)").arg(follow));
330 331 332 333 334 335 336 337
}

void QGCGoogleEarthView::goHome()
{
    // Disable follow and update
    follow(false);
    updateState();
    // Go to home location
338
    javaScript("goHome();");
339 340 341 342
}

void QGCGoogleEarthView::setHome(double lat, double lon, double alt)
{
343
    qDebug() << "SETTING GCS HOME IN GOOGLE MAPS" << lat << lon << alt;
344
    javaScript(QString("setGCSHome(%1,%2,%3);").arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15));
345 346
}

347 348 349 350 351 352 353 354 355 356 357 358 359 360
void QGCGoogleEarthView::setHome()
{
    javaScript(QString("enableSetHomeMode();"));
}

void QGCGoogleEarthView::moveToPosition()
{
    bool ok;
    javaScript("sampleCurrentPosition();");
    double latitude = documentElement("currentCameraLatitude").toDouble();
    double longitude = documentElement("currentCameraLongitude").toDouble();
    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
                                         QString("%1,%2").arg(latitude).arg(longitude), &ok);
361
    if (ok && !text.isEmpty()) {
362
        QStringList split = text.split(",");
363
        if (split.length() == 2) {
364 365 366 367 368 369
            bool convert;
            double latitude = split.first().toDouble(&convert);
            ok &= convert;
            double longitude = split.last().toDouble(&convert);
            ok &= convert;

370
            if (ok) {
371 372 373 374 375 376
                javaScript(QString("setLookAtLatLon(%1,%2);").arg(latitude, 0, 'f', 15).arg(longitude, 0, 'f', 15));
            }
        }
    }
}

377 378
void QGCGoogleEarthView::hideEvent(QHideEvent* event)
{
379 380
    Q_UNUSED(event);
    updateTimer->stop();
381 382
}

383 384 385 386
void QGCGoogleEarthView::showEvent(QShowEvent* event)
{
    // React only to internal (pre-display)
    // events
387
    Q_UNUSED(event)
388
    // Enable widget, initialize on first run
389

390
    if (!webViewInitialized) {
391
#if (defined Q_OS_MAC)
392 393 394
        webViewMac->setPage(new QGCWebPage(webViewMac));
        webViewMac->settings()->setAttribute(QWebSettings::PluginsEnabled, true);
        webViewMac->load(QUrl(QCoreApplication::applicationDirPath()+"/earth.html"));
395 396 397
#endif

#ifdef _MSC_VER
398 399
        //webViewWin->dynamicCall("GoHome()");
        webViewWin->dynamicCall("Navigate(const QString&)", QApplication::applicationDirPath() + "/earth.html");
400 401
#endif

402 403 404
        webViewInitialized = true;
        // Reloading the webpage, this resets Google Earth
        gEarthInitialized = false;
405

406 407 408 409
        QTimer::singleShot(3000, this, SLOT(initializeGoogleEarth()));
    } else {
        updateTimer->start(refreshRateMs);
    }
410 411
}

412 413
void QGCGoogleEarthView::printWinException(int no, QString str1, QString str2, QString str3)
{
414
    qDebug() << no << str1 << str2 << str3;
415 416 417 418 419
}

QVariant QGCGoogleEarthView::javaScript(QString javaScript)
{
#ifdef Q_OS_MAC
420
    if (!jScriptInitialized) {
421
        return QVariant(false);
422
    } else {
423 424
        return webViewMac->page()->currentFrame()->evaluateJavaScript(javaScript);
    }
425 426
#endif
#ifdef _MSC_VER
427
    if(!jScriptInitialized) {
428 429
        qDebug() << "TOO EARLY JAVASCRIPT CALL, ABORTING";
        return QVariant(false);
430
    } else {
431 432 433
        QVariantList params;
        params.append(javaScript);
        params.append("JScript");
434 435
        QVariant result = jScriptWin->dynamicCall("execScript(QString, QString)", params);
        return result;
436
    }
437 438 439
#endif
}

440 441 442 443 444 445 446 447
QVariant QGCGoogleEarthView::documentElement(QString name)
{
#ifdef Q_OS_MAC
    QString javaScript("getGlobal(%1)");
    QVariant result = webViewMac->page()->currentFrame()->evaluateJavaScript(javaScript.arg(name));
    return result;
#endif
#ifdef _MSC_VER
448
    if(!jScriptInitialized) {
449
        qDebug() << "TOO EARLY JAVASCRIPT CALL, ABORTING";
450 451
    } else {
        if (documentWin) {
452 453 454 455 456 457 458 459 460 461 462 463
            QString resultString;

            // Get HTMLElement object
            QVariantList params;
            IHTMLDocument3* doc;
            documentWin->queryInterface( IID_IHTMLDocument3, (void**)&doc);
            params.append(name);
            IHTMLElement* element = NULL;
            // Append alias
            name.prepend("JScript_");
            HRESULT res = doc->getElementById(QStringToBSTR(name), &element);
            //BSTR elemString;
464
            if (element) {
465 466 467 468
                //element->get_innerHTML(&elemString);
                VARIANT var;
                var.vt = VT_BSTR;
                HRESULT res = element->getAttribute(L"value", 0, &var);
469
                if (SUCCEEDED(res) && (var.vt != VT_NULL)) {
470 471 472
                    QByteArray typeName;
                    QVariant qtValue = VARIANTToQVariant(var,typeName);
                    return qtValue;
473
                } else {
474 475
                    qDebug() << __FILE__ << __LINE__ << "JAVASCRIPT ATTRIBUTE" << name << "NOT FOUND";
                }
476
            } else {
477 478 479
                qDebug() << __FILE__ << __LINE__ << "DID NOT GET HTML ELEMENT" << name;
            }
        }
480
    }
481
    return QVariant(0);
482 483 484
#endif
}

485 486
void QGCGoogleEarthView::initializeGoogleEarth()
{
487
    if (!jScriptInitialized) {
488 489
#ifdef Q_OS_MAC
        jScriptInitialized = true;
490
#endif
491 492
#ifdef _MSC_VER
        QAxObject* doc = webViewWin->querySubObject("Document()");
493
        //IDispatch* Disp;
494
        IDispatch* winDoc = NULL;
495
        IHTMLDocument2* document = NULL;
496 497 498 499

        //332C4425-26CB-11D0-B483-00C04FD90119 IHTMLDocument2
        //25336920-03F9-11CF-8FD0-00AA00686F13 HTMLDocument
        doc->queryInterface(QUuid("{332C4425-26CB-11D0-B483-00C04FD90119}"), (void**)(&winDoc));
500
        if (winDoc) {
501
            document = NULL;
502 503 504
            winDoc->QueryInterface( IID_IHTMLDocument2, (void**)&document );
            IHTMLWindow2 *window = NULL;
            document->get_parentWindow( &window );
505
            documentWin = new QAxObject(document, webViewWin);
506 507 508
            jScriptWin = new QAxObject(window, webViewWin);
            connect(jScriptWin, SIGNAL(exception(int,QString,QString,QString)), this, SLOT(printWinException(int,QString,QString,QString)));
            jScriptInitialized = true;
509
        } else {
510 511
            qDebug() << "COULD NOT GET DOCUMENT OBJECT! Aborting";
        }
512
#endif
513
        QTimer::singleShot(1500, this, SLOT(initializeGoogleEarth()));
514 515
        return;
    }
516

517 518
    if (!gEarthInitialized) {
        if (!documentElement("initialized").toBool()) {
519
            QTimer::singleShot(300, this, SLOT(initializeGoogleEarth()));
520
            qDebug() << "NOT INITIALIZED, WAITING";
521
        } else {
522 523
            gEarthInitialized = true;

524
            // Set home location
525
            setHome(UASManager::instance()->getHomeLatitude(), UASManager::instance()->getHomeLongitude(), UASManager::instance()->getHomeAltitude());
526 527 528

            // Add all MAVs
            QList<UASInterface*> mavs = UASManager::instance()->getUASList();
529
            foreach (UASInterface* currMav, mavs) {
530
                addUAS(currMav);
531 532
            }

533 534 535
            // Set current UAS
            setActiveUAS(UASManager::instance()->getActiveUAS());

536
            // Add any further MAV automatically
537 538
            connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)), Qt::UniqueConnection);
            connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)), Qt::UniqueConnection);
539 540 541 542 543

            // Connect UI signals/slots

            // Follow checkbox
            ui->followAirplaneCheckbox->setChecked(followCamera);
544
            connect(ui->followAirplaneCheckbox, SIGNAL(toggled(bool)), this, SLOT(follow(bool)), Qt::UniqueConnection);
545 546 547

            // Trail checkbox
            ui->trailCheckbox->setChecked(trailEnabled);
548
            connect(ui->trailCheckbox, SIGNAL(toggled(bool)), this, SLOT(showTrail(bool)), Qt::UniqueConnection);
549 550 551

            // Go home
            connect(ui->goHomeButton, SIGNAL(clicked()), this, SLOT(goHome()));
552 553 554 555 556 557 558 559 560
            // Set home
            connect(ui->setHomeButton, SIGNAL(clicked()), this, SLOT(setHome()));

            // Visibility of set home button
            connect(ui->editButton, SIGNAL(clicked(bool)), ui->setHomeButton, SLOT(setVisible(bool)));
            ui->setHomeButton->setVisible(ui->editButton->isChecked());

            // To Lat/Lon button
            connect(ui->toLatLonButton, SIGNAL(clicked()), this, SLOT(moveToPosition()));
561 562

            // Cam distance slider
563
            connect(ui->camDistanceSlider, SIGNAL(valueChanged(int)), this, SLOT(setViewRangeScaledInt(int)), Qt::UniqueConnection);
564
            setViewRangeScaledInt(ui->camDistanceSlider->value());
565

566 567 568 569 570
            // Distance combo box
            connect(ui->camDistanceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setDistanceMode(int)), Qt::UniqueConnection);
            // Edit mode button
            connect(ui->editButton, SIGNAL(clicked(bool)), this, SLOT(enableEditMode(bool)), Qt::UniqueConnection);

571 572 573
            // Update waypoint list
            if (mav) updateWaypointList(mav->getUASID());

574 575 576
            // Start update timer
            updateTimer->start(refreshRateMs);

577 578
            // Set current view mode
            setViewMode(currentViewMode);
579 580
            setDistanceMode(ui->camDistanceComboBox->currentIndex());
            enableEditMode(ui->editButton->isChecked());
581 582
            enableAtmosphere(ui->atmosphereCheckBox->isChecked());
            enableDaylight(ui->daylightCheckBox->isChecked());
pixhawk's avatar
pixhawk committed
583
            follow(this->followCamera);
584 585 586

            // Move to home location
            goHome();
587 588 589 590 591 592
        }
    }
}

void QGCGoogleEarthView::updateState()
{
593 594 595
#if (QGC_EVENTLOOP_DEBUG)
    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
596
    if (gEarthInitialized) {
597 598 599 600 601 602 603 604 605 606 607
        int uasId = 0;
        double lat = 47.3769;
        double lon = 8.549444;
        double alt = 470.0;

        float roll = 0.0f;
        float pitch = 0.0f;
        float yaw = 0.0f;

        // Update all MAVs
        QList<UASInterface*> mavs = UASManager::instance()->getUASList();
608
        foreach (UASInterface* currMav, mavs) {
609 610 611 612 613 614 615
            uasId = currMav->getUASID();
            lat = currMav->getLatitude();
            lon = currMav->getLongitude();
            alt = currMav->getAltitude();
            roll = currMav->getRoll();
            pitch = currMav->getPitch();
            yaw = currMav->getYaw();
616

617 618
            //qDebug() << "SETTING POSITION FOR" << uasId << lat << lon << alt << roll << pitch << yaw;

619
            javaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);")
620
                       .arg(uasId)
621 622 623 624 625 626
                       .arg(lat, 0, 'f', 15)
                       .arg(lon, 0, 'f', 15)
                       .arg(alt, 0, 'f', 15)
                       .arg(roll, 0, 'f', 9)
                       .arg(pitch, 0, 'f', 9)
                       .arg(yaw, 0, 'f', 9));
627
        }
628 629 630 631 632 633 634 635 636


        // Read out new waypoint positions and waypoint create events
        // this is polling (bad) but forced because of the crappy
        // Microsoft API available in Qt - improvements wanted

        // First check if a new WP should be created
//        bool newWaypointPending = .to
        bool newWaypointPending = documentElement("newWaypointPending").toBool();
637
        if (newWaypointPending) {
638 639 640 641 642 643 644 645
            bool coordsOk = true;
            bool ok;
            double latitude = documentElement("newWaypointLatitude").toDouble(&ok);
            coordsOk &= ok;
            double longitude = documentElement("newWaypointLongitude").toDouble(&ok);
            coordsOk &= ok;
            double altitude = documentElement("newWaypointAltitude").toDouble(&ok);
            coordsOk &= ok;
646
            if (coordsOk) {
647
                // Add new waypoint
648
                if (mav) {
649 650 651 652 653 654 655 656 657 658 659 660 661 662 663
                    int nextIndex = mav->getWaypointManager()->getWaypointList().count();
                    Waypoint* wp = new Waypoint(nextIndex, latitude, longitude, altitude, true);
                    wp->setFrame(MAV_FRAME_GLOBAL);
//                    wp.setLatitude(latitude);
//                    wp.setLongitude(longitude);
//                    wp.setAltitude(altitude);
                    mav->getWaypointManager()->addWaypoint(wp);
                }
            }
            javaScript("setNewWaypointPending(false);");
        }

        // Check if a waypoint should be moved
        bool dragWaypointPending = documentElement("dragWaypointPending").toBool();

664
        if (dragWaypointPending) {
665 666 667 668 669 670 671 672
            bool coordsOk = true;
            bool ok;
            double latitude = documentElement("dragWaypointLatitude").toDouble(&ok);
            coordsOk &= ok;
            double longitude = documentElement("dragWaypointLongitude").toDouble(&ok);
            coordsOk &= ok;
            double altitude = documentElement("dragWaypointAltitude").toDouble(&ok);
            coordsOk &= ok;
673 674

            // UPDATE WAYPOINTS, HOME LOCATION AND OTHER LOCATIONS
675
            if (coordsOk) {
676
                QString idText = documentElement("dragWaypointIndex").toString();
677
                if (idText == "HOME") {
678 679 680
                    qDebug() << "HOME UPDATED!";
                    UASManager::instance()->setHomePosition(latitude, longitude, altitude);
                    ui->setHomeButton->setChecked(false);
681
                } else {
682
                    // Update waypoint or symbol
683
                    if (mav) {
lm's avatar
lm committed
684
                        QVector<Waypoint*> wps = mav->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList();
685 686 687 688

                        bool ok;
                        int index = idText.toInt(&ok);

689
                        if (ok && index >= 0 && index < wps.count()) {
690 691 692 693 694 695
                            Waypoint* wp = wps.at(index);
                            wp->setLatitude(latitude);
                            wp->setLongitude(longitude);
                            wp->setAltitude(altitude);
                            mav->getWaypointManager()->notifyOfChange(wp);
                        }
696 697
                    }
                }
698
            } else {
699 700 701 702 703 704
                // If coords were not ok, move the view in google earth back
                // to last acceptable location
                updateWaypointList(mav->getUASID());
            }
            javaScript("setDragWaypointPending(false);");
        }
705 706 707 708 709 710 711 712 713 714 715 716 717 718 719
    }
}


void QGCGoogleEarthView::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
    switch (e->type()) {
    case QEvent::LanguageChange:
        ui->retranslateUi(this);
        break;
    default:
        break;
    }
}