QGCMapWidget.cc 31.6 KB
Newer Older
lm's avatar
lm committed
1
#include "QGCMapWidget.h"
LM's avatar
LM committed
2
#include "QGCMapToolBar.h"
lm's avatar
lm committed
3
#include "UASInterface.h"
lm's avatar
lm committed
4
#include "UASManager.h"
5
#include "MAV2DIcon.h"
6
#include "Waypoint2DIcon.h"
7
#include "UASWaypointManager.h"
8
#include "ArduPilotMegaMAV.h"
lm's avatar
lm committed
9 10

QGCMapWidget::QGCMapWidget(QWidget *parent) :
11 12
    mapcontrol::OPMapWidget(parent),
    firingWaypointChange(NULL),
13
    maxUpdateInterval(2.1f), // 2 seconds
14 15 16
    followUAVEnabled(false),
    trailType(mapcontrol::UAVTrailType::ByTimeElapsed),
    trailInterval(2.0f),
17
    followUAVID(0),
18
    mapInitialized(false),
19
    homeAltitude(0),
20
    uas(NULL)
lm's avatar
lm committed
21
{
22
    currWPManager = UASManager::instance()->getActiveUASWaypointManager();
23
    waypointLines.insert(0, new QGraphicsItemGroup(map));
24 25 26 27 28
    connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
    connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
    connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
    connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
    offlineMode = true;
29
    // Widget is inactive until shown
30
    defaultGuidedAlt = -1;
31
    loadSettings(false);
32

33 34 35 36
    //handy for debugging:
    //this->SetShowTileGridLines(true);

    //default appears to be Google Hybrid, and is broken currently
37
#if defined MAP_DEFAULT_TYPE_BING
38
    this->SetMapType(MapType::BingHybrid);
39 40 41 42 43
#elif defined MAP_DEFAULT_TYPE_GOOGLE
    this->SetMapType(MapType::GoogleHybrid);
#else
    this->SetMapType(MapType::OpenStreetMap);
#endif
44

45 46
    this->setContextMenuPolicy(Qt::ActionsContextMenu);

47
    // Go to options
48 49 50 51 52 53 54 55
    QAction *guidedaction = new QAction(this);
    guidedaction->setText("Go To Here (Guided Mode)");
    connect(guidedaction,SIGNAL(triggered()),this,SLOT(guidedActionTriggered()));
    this->addAction(guidedaction);
    guidedaction = new QAction(this);
    guidedaction->setText("Go To Here Alt (Guided Mode)");
    connect(guidedaction,SIGNAL(triggered()),this,SLOT(guidedAltActionTriggered()));
    this->addAction(guidedaction);
56
    // Point camera option
57 58 59 60
    QAction *cameraaction = new QAction(this);
    cameraaction->setText("Point Camera Here");
    connect(cameraaction,SIGNAL(triggered()),this,SLOT(cameraActionTriggered()));
    this->addAction(cameraaction);
61 62 63 64 65
    // Set home location option
    QAction *sethomeaction = new QAction(this);
    sethomeaction->setText("Set Home Location Here");
    connect(sethomeaction,SIGNAL(triggered()),this,SLOT(setHomeActionTriggered()));
    this->addAction(sethomeaction);
66 67 68
}
void QGCMapWidget::guidedActionTriggered()
{
69 70 71 72 73
    if (!uas)
    {
        QMessageBox::information(0,"Error","Please connect first");
        return;
    }
74 75 76 77 78 79 80 81 82 83
    if (currWPManager)
    {
        if (defaultGuidedAlt == -1)
        {
            if (!guidedAltActionTriggered())
            {
                return;
            }
        }
        // Create new waypoint and send it to the WPManager to send out.
84
        internals::PointLatLng pos = map->FromLocalToLatLng(contextMousePressPos.x(), contextMousePressPos.y());
85 86 87 88 89 90 91 92 93 94
        qDebug() << "Guided action requested. Lat:" << pos.Lat() << "Lon:" << pos.Lng();
        Waypoint wp;
        wp.setLatitude(pos.Lat());
        wp.setLongitude(pos.Lng());
        wp.setAltitude(defaultGuidedAlt);
        currWPManager->goToWaypoint(&wp);
    }
}
bool QGCMapWidget::guidedAltActionTriggered()
{
95 96 97 98 99
    if (!uas)
    {
        QMessageBox::information(0,"Error","Please connect first");
        return false;
    }
100 101 102 103 104 105 106 107 108 109 110
    bool ok = false;
    int tmpalt = QInputDialog::getInt(this,"Altitude","Enter default altitude (in meters) of destination point for guided mode",100,0,30000,1,&ok);
    if (!ok)
    {
        //Use has chosen cancel. Do not send the waypoint
        return false;
    }
    defaultGuidedAlt = tmpalt;
    guidedActionTriggered();
    return true;
}
111 112
void QGCMapWidget::cameraActionTriggered()
{
113 114 115 116 117
    if (!uas)
    {
        QMessageBox::information(0,"Error","Please connect first");
        return;
    }
118 119 120 121
    ArduPilotMegaMAV *newmav = qobject_cast<ArduPilotMegaMAV*>(this->uas);
    if (newmav)
    {
        newmav->setMountConfigure(4,true,true,true);
122
        internals::PointLatLng pos = map->FromLocalToLatLng(contextMousePressPos.x(), contextMousePressPos.y());
123 124 125
        newmav->setMountControl(pos.Lat(),pos.Lng(),100,true);
    }
}
126

127 128 129 130 131 132 133 134 135 136 137 138 139 140 141
/**
 * @brief QGCMapWidget::setHomeActionTriggered
 */
bool QGCMapWidget::setHomeActionTriggered()
{
    if (!uas)
    {
        QMessageBox::information(0,"Error","Please connect first");
        return false;
    }
    UASManager *uasManager = UASManager::instance();
    if (!uasManager) { return false; }

    // Enter an altitude
    bool ok = false;
142
    double alt = QInputDialog::getDouble(this,"Home Altitude","Enter altitude (in meters) of new home location",0.0,0.0,30000.0,2,&ok);
143 144 145
    if (!ok) return false; //Use has chosen cancel. Do not send the waypoint

    // Create new waypoint and send it to the WPManager to send out.
146 147
    internals::PointLatLng pos = map->FromLocalToLatLng(contextMousePressPos.x(), contextMousePressPos.y());
    qDebug("Set home location sent. Lat: %f, Lon: %f, Alt: %f.", pos.Lat(), pos.Lng(), alt);
148 149 150 151 152 153 154 155

    bool success = uasManager->setHomePositionAndNotify(pos.Lat(),pos.Lng(), alt);

    qDebug() << ((success)? "Set new home location." : "Failed to set new home location.");

    return success;
}

156 157
void QGCMapWidget::mousePressEvent(QMouseEvent *event)
{
158 159 160 161 162 163 164

    // Store right-click event presses separate for context menu
    // TODO add check if click was on map, or popup box.
    if (event->button() == Qt::RightButton) {
        contextMousePressPos = event->pos();
    }

165 166 167 168 169 170 171
    mapcontrol::OPMapWidget::mousePressEvent(event);
}

void QGCMapWidget::mouseReleaseEvent(QMouseEvent *event)
{
    mousePressPos = event->pos();
    mapcontrol::OPMapWidget::mouseReleaseEvent(event);
172 173 174 175 176 177
}

QGCMapWidget::~QGCMapWidget()
{
    SetShowHome(false);	// doing this appears to stop the map lib crashing on exit
    SetShowUAV(false);	//   "          "
178
    storeSettings();
179 180 181 182
}

void QGCMapWidget::showEvent(QShowEvent* event)
{
183
    // Disable OP's standard UAV, we have more than one
LM's avatar
LM committed
184 185
    SetShowUAV(false);

186 187 188
    // Pass on to parent widget
    OPMapWidget::showEvent(event);

189 190
    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)), Qt::UniqueConnection);
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)), Qt::UniqueConnection);
191 192
    connect(UASManager::instance(), SIGNAL(homePositionChanged(double,double,double)), this, SLOT(updateHomePosition(double,double,double)));

lm's avatar
lm committed
193 194 195 196
    foreach (UASInterface* uas, UASManager::instance()->getUASList())
    {
        addUAS(uas);
    }
lm's avatar
lm committed
197 198


199 200 201
    if (!mapInitialized)
    {
        internals::PointLatLng pos_lat_lon = internals::PointLatLng(0, 0);
lm's avatar
lm committed
202

203 204
        SetMouseWheelZoomType(internals::MouseWheelZoomType::MousePositionWithoutCenter);	    // set how the mouse wheel zoom functions
        SetFollowMouse(true);				    // we want a contiuous mouse position reading
lm's avatar
lm committed
205

206
        SetShowHome(true);					    // display the HOME position on the map
207 208
        Home->SetSafeArea(0);                         // set radius (meters)
        Home->SetShowSafeArea(false);                                         // show the safe area
209
        Home->SetCoord(pos_lat_lon);             // set the HOME position
lm's avatar
lm committed
210

211 212
        setFrameStyle(QFrame::NoFrame);      // no border frame
        setBackgroundBrush(QBrush(Qt::black)); // tile background
lm's avatar
lm committed
213

214 215
        // Set current home position
        updateHomePosition(UASManager::instance()->getHomeLatitude(), UASManager::instance()->getHomeLongitude(), UASManager::instance()->getHomeAltitude());
216

217 218
        // Set currently selected system
        activeUASSet(UASManager::instance()->getActiveUAS());
219

220 221
        // Connect map updates to the adapter slots
        connect(this, SIGNAL(WPValuesChanged(WayPointItem*)), this, SLOT(handleMapWaypointEdit(WayPointItem*)));
222

223 224
        SetCurrentPosition(pos_lat_lon);         // set the map position
        setFocus();
225

226 227 228
        // Start timer
        connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateGlobalPosition()));
        mapInitialized = true;
229
        //QTimer::singleShot(800, this, SLOT(loadSettings()));
230
    }
231
    updateTimer.start(maxUpdateInterval*1000);
232
    // Update all UAV positions
233
    updateGlobalPosition();
lm's avatar
lm committed
234 235
}

236
void QGCMapWidget::hideEvent(QHideEvent* event)
lm's avatar
lm committed
237
{
238
    updateTimer.stop();
239 240
    storeSettings();
    OPMapWidget::hideEvent(event);
lm's avatar
lm committed
241
}
lm's avatar
lm committed
242

243 244 245 246
/**
 * @param changePosition Load also the last position from settings and update the map position.
 */
void QGCMapWidget::loadSettings(bool changePosition)
247 248 249 250 251 252 253 254
{
    // Atlantic Ocean near Africa, coordinate origin
    double lastZoom = 1;
    double lastLat = 0;
    double lastLon = 0;

    QSettings settings;
    settings.beginGroup("QGC_MAPWIDGET");
255 256 257 258 259 260 261 262
    if (changePosition)
    {
        lastLat = settings.value("LAST_LATITUDE", lastLat).toDouble();
        lastLon = settings.value("LAST_LONGITUDE", lastLon).toDouble();
        lastZoom = settings.value("LAST_ZOOM", lastZoom).toDouble();
    }
    trailType = static_cast<mapcontrol::UAVTrailType::Types>(settings.value("TRAIL_TYPE", trailType).toInt());
    trailInterval = settings.value("TRAIL_INTERVAL", trailInterval).toFloat();
263 264
    settings.endGroup();

265 266 267 268 269
    // SET CORRECT MENU CHECKBOXES
    // Set the correct trail interval
    if (trailType == mapcontrol::UAVTrailType::ByDistance)
    {
        // XXX
270 271 272
#ifdef Q_OS_WIN
#pragma message ("WARNING: Settings loading for trail type not implemented")
#else
273
#warning Settings loading for trail type not implemented
274
#endif
275 276 277 278 279 280
    }
    else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
    {
        // XXX
    }

281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296
    // SET TRAIL TYPE
    foreach (mapcontrol::UAVItem* uav, GetUAVS())
    {
        // Set the correct trail type
        uav->SetTrailType(trailType);
        // Set the correct trail interval
        if (trailType == mapcontrol::UAVTrailType::ByDistance)
        {
            uav->SetTrailDistance(trailInterval);
        }
        else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
        {
            uav->SetTrailTime(trailInterval);
        }
    }

297 298 299
    // SET INITIAL POSITION AND ZOOM
    internals::PointLatLng pos_lat_lon = internals::PointLatLng(lastLat, lastLon);
    SetCurrentPosition(pos_lat_lon);        // set the map position
300
    SetZoom(lastZoom); // set map zoom level
301 302 303 304 305 306 307 308 309 310
}

void QGCMapWidget::storeSettings()
{
    QSettings settings;
    settings.beginGroup("QGC_MAPWIDGET");
    internals::PointLatLng pos = CurrentPosition();
    settings.setValue("LAST_LATITUDE", pos.Lat());
    settings.setValue("LAST_LONGITUDE", pos.Lng());
    settings.setValue("LAST_ZOOM", ZoomReal());
311 312
    settings.setValue("TRAIL_TYPE", static_cast<int>(trailType));
    settings.setValue("TRAIL_INTERVAL", trailInterval);
313 314 315 316 317 318
    settings.endGroup();
    settings.sync();
}

void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
{
319 320
    // If a waypoint manager is available
    if (currWPManager)
321
    {
322 323 324 325 326 327 328 329 330 331 332
        // Create new waypoint
        internals::PointLatLng pos = map->FromLocalToLatLng(event->pos().x(), event->pos().y());
        Waypoint* wp = currWPManager->createWaypoint();
        //            wp->blockSignals(true);
        //            wp->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
        wp->setLatitude(pos.Lat());
        wp->setLongitude(pos.Lng());
        wp->setFrame((MAV_FRAME)currWPManager->getFrameRecommendation());
        wp->setAltitude(currWPManager->getAltitudeRecommendation());
        //            wp->blockSignals(false);
        //            currWPManager->notifyOfChangeEditable(wp);
333
    }
334

lm's avatar
lm committed
335
    OPMapWidget::mouseDoubleClickEvent(event);
336 337 338
}


lm's avatar
lm committed
339 340
/**
 *
341
 * @param uas the UAS/MAV to monitor/display with the map widget
lm's avatar
lm committed
342 343 344 345
 */
void QGCMapWidget::addUAS(UASInterface* uas)
{
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
346
    connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
347 348 349 350 351 352 353 354
    if (!waypointLines.value(uas->getUASID(), NULL)) {
        waypointLines.insert(uas->getUASID(), new QGraphicsItemGroup(map));
    } else {
        foreach (QGraphicsItem* item, waypointLines.value(uas->getUASID())->childItems())
        {
            delete item;
        }
    }
lm's avatar
lm committed
355 356
}

357 358 359
void QGCMapWidget::activeUASSet(UASInterface* uas)
{
    // Only execute if proper UAS is set
360
    this->uas = uas;
361 362

    // Disconnect old MAV manager
363 364
    if (currWPManager)
    {
365
        // Disconnect the waypoint manager / data storage from the UI
366 367 368 369
        disconnect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
        disconnect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
        disconnect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
        disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
370 371
    }

372 373 374 375 376
    // Attach the new waypoint manager if a new UAS was selected. Otherwise, indicate
    // that no such manager exists.
    if (uas)
    {
        currWPManager = uas->getWaypointManager();
377

378 379 380
        updateSelectedSystem(uas->getUASID());
        followUAVID = uas->getUASID();
        updateWaypointList(uas->getUASID());
381

382 383 384 385 386 387 388 389 390 391
        // Connect the waypoint manager / data storage to the UI
        connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
        connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
        connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
        connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
    }
    else
    {
        currWPManager = NULL;
    }
392 393
}

lm's avatar
lm committed
394 395 396 397 398 399 400 401 402 403 404 405 406
/**
 * Updates the global position of one MAV and append the last movement to the trail
 *
 * @param uas The unmanned air system
 * @param lat Latitude in WGS84 ellipsoid
 * @param lon Longitutde in WGS84 ellipsoid
 * @param alt Altitude over mean sea level
 * @param usec Timestamp of the position message in milliseconds FIXME will move to microseconds
 */
void QGCMapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec)
{
    Q_UNUSED(usec);

407 408
    // Immediate update
    if (maxUpdateInterval == 0)
409
    {
410 411 412 413 414 415 416 417 418
        // Get reference to graphic UAV item
        mapcontrol::UAVItem* uav = GetUAV(uas->getUASID());
        // Check if reference is valid, else create a new one
        if (uav == NULL)
        {
            MAV2DIcon* newUAV = new MAV2DIcon(map, this, uas);
            newUAV->setParentItem(map);
            UAVS.insert(uas->getUASID(), newUAV);
            uav = GetUAV(uas->getUASID());
419 420 421 422 423 424 425 426 427 428 429
            // Set the correct trail type
            uav->SetTrailType(trailType);
            // Set the correct trail interval
            if (trailType == mapcontrol::UAVTrailType::ByDistance)
            {
                uav->SetTrailDistance(trailInterval);
            }
            else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
            {
                uav->SetTrailTime(trailInterval);
            }
430 431 432 433 434
        }

        // Set new lat/lon position of UAV icon
        internals::PointLatLng pos_lat_lon = internals::PointLatLng(lat, lon);
        uav->SetUAVPos(pos_lat_lon, alt);
435 436
        // Follow status
        if (followUAVEnabled && uas->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
437 438
        // Convert from radians to degrees and apply
        uav->SetUAVHeading((uas->getYaw()/M_PI)*180.0f);
439
    }
440
}
lm's avatar
lm committed
441

442 443 444 445 446 447 448 449 450 451 452 453 454 455
/**
 * Pulls in the positions of all UAVs from the UAS manager
 */
void QGCMapWidget::updateGlobalPosition()
{
    QList<UASInterface*> systems = UASManager::instance()->getUASList();
    foreach (UASInterface* system, systems)
    {
        // Get reference to graphic UAV item
        mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
        // Check if reference is valid, else create a new one
        if (uav == NULL)
        {
            MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
456 457
            AddUAV(system->getUASID(), newUAV);
            uav = newUAV;
LM's avatar
LM committed
458 459 460
            uav->SetTrailTime(1);
            uav->SetTrailDistance(5);
            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
461 462 463 464 465
        }

        // Set new lat/lon position of UAV icon
        internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
466 467
        // Follow status
        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
468 469 470
        // Convert from radians to degrees and apply
        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
    }
471 472
}

473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502
void QGCMapWidget::updateLocalPosition()
{
    QList<UASInterface*> systems = UASManager::instance()->getUASList();
    foreach (UASInterface* system, systems)
    {
        // Get reference to graphic UAV item
        mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
        // Check if reference is valid, else create a new one
        if (uav == NULL)
        {
            MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
            AddUAV(system->getUASID(), newUAV);
            uav = newUAV;
            uav->SetTrailTime(1);
            uav->SetTrailDistance(5);
            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
        }

        // Set new lat/lon position of UAV icon
        internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
        // Follow status
        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
        // Convert from radians to degrees and apply
        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
    }
}

void QGCMapWidget::updateLocalPositionEstimates()
{
503
    updateLocalPosition();
504 505
}

506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521

void QGCMapWidget::updateSystemSpecs(int uas)
{
    foreach (mapcontrol::UAVItem* p, UAVS.values())
    {
        MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(p);
        if (icon && icon->getUASId() == uas)
        {
            // Set new airframe
            icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe());
            icon->drawIcon();
        }
    }
}

/**
522
 * Does not update the system type or configuration, only the selected status
523 524 525 526 527 528 529 530 531 532 533 534 535 536 537
 */
void QGCMapWidget::updateSelectedSystem(int uas)
{
    foreach (mapcontrol::UAVItem* p, UAVS.values())
    {
        MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(p);
        if (icon)
        {
            // Set as selected if ids match
            icon->setSelectedUAS((icon->getUASId() == uas));
        }
    }
}


538 539 540 541 542 543
// MAP NAVIGATION
void QGCMapWidget::showGoToDialog()
{
    bool ok;
    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
544
                                         QString("%1,%2").arg(CurrentPosition().Lat(), 0, 'g', 6).arg(CurrentPosition().Lng(), 0, 'g', 6), &ok);
545 546
    if (ok && !text.isEmpty())
    {
547
        QStringList split = text.split(",");
548 549
        if (split.length() == 2)
        {
550 551 552 553 554 555
            bool convert;
            double latitude = split.first().toDouble(&convert);
            ok &= convert;
            double longitude = split.last().toDouble(&convert);
            ok &= convert;

556 557
            if (ok)
            {
558 559 560 561 562 563 564 565 566 567 568 569
                internals::PointLatLng pos_lat_lon = internals::PointLatLng(latitude, longitude);
                SetCurrentPosition(pos_lat_lon);        // set the map position
            }
        }
    }
}


void QGCMapWidget::updateHomePosition(double latitude, double longitude, double altitude)
{
    Home->SetCoord(internals::PointLatLng(latitude, longitude));
    Home->SetAltitude(altitude);
570
    homeAltitude = altitude;
571
    SetShowHome(true);                      // display the HOME position on the map
572
    Home->RefreshPos();
573 574
}

575 576 577
void QGCMapWidget::goHome()
{
    SetCurrentPosition(Home->Coord());
578
    SetZoom(18); //zoom to "large RC park" size
579 580
}

581 582 583 584 585 586 587 588 589 590
/**
 * Limits the update rate on the specified interval. Set to zero (0) to run at maximum
 * telemetry speed. Recommended rate is 2 s.
 */
void QGCMapWidget::setUpdateRateLimit(float seconds)
{
    maxUpdateInterval = seconds;
    updateTimer.start(maxUpdateInterval*1000);
}

591 592 593 594 595 596 597 598 599 600 601 602 603 604
void QGCMapWidget::cacheVisibleRegion()
{
    internals::RectLatLng rect = map->SelectedArea();

    if (rect.IsEmpty())
    {
        QMessageBox msgBox(this);
        msgBox.setIcon(QMessageBox::Information);
        msgBox.setText("Cannot cache tiles for offline use");
        msgBox.setInformativeText("Please select an area first by holding down SHIFT or ALT and selecting the area with the left mouse button.");
        msgBox.setStandardButtons(QMessageBox::Ok);
        msgBox.setDefaultButton(QMessageBox::Ok);
        msgBox.exec();
    }
605 606 607 608 609 610
    else
    {
        RipMap();
        // Set empty area = unselect area
        map->SetSelectedArea(internals::RectLatLng());
    }
611 612
}

613

614 615 616 617 618 619
// WAYPOINT MAP INTERACTION FUNCTIONS

void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
{
    // Block circle updates
    Waypoint* wp = iconsToWaypoints.value(waypoint, NULL);
620 621 622 623 624

    // Delete UI element if wp doesn't exist
    if (!wp)
        WPDelete(waypoint);

625
    // Protect from vicious double update cycle
626
    if (firingWaypointChange == wp) return;
627 628
    // Not in cycle, block now from entering it
    firingWaypointChange = wp;
629
    // // qDebug() << "UPDATING WP FROM MAP";
630 631 632

    // Update WP values
    internals::PointLatLng pos = waypoint->Coord();
633 634 635

    // Block waypoint signals
    wp->blockSignals(true);
636 637
    wp->setLatitude(pos.Lat());
    wp->setLongitude(pos.Lng());
638
    // XXX Magic values
639 640
//    wp->setAltitude(homeAltitude + 50.0f);
//    wp->setAcceptanceRadius(10.0f);
641
    wp->blockSignals(false);
642

643

644 645
    internals::PointLatLng coord = waypoint->Coord();
    QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + "   " + QString::number(coord.Lng(), 'f', 6);
646
    // // qDebug() << "MAP WP COORD (MAP):" << coord_str << __FILE__ << __LINE__;
647
    QString wp_str = QString::number(wp->getLatitude(), 'f', 6) + "   " + QString::number(wp->getLongitude(), 'f', 6);
648
    // // qDebug() << "MAP WP COORD (WP):" << wp_str << __FILE__ << __LINE__;
649

650
    firingWaypointChange = NULL;
651 652

    emit waypointChanged(wp);
653
}
654 655

// WAYPOINT UPDATE FUNCTIONS
656 657

/**
658 659
 * This function is called if a a single waypoint is updated and
 * also if the whole list changes.
660
 */
661
void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
662
{
663
    //qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED";
664
    // Source of the event was in this widget, do nothing
665 666 667
    if (firingWaypointChange == wp) {
        return;
    }
668 669
    // Currently only accept waypoint updates from the UAS in focus
    // this has to be changed to accept read-only updates from other systems as well.
670
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
671
    if (currWPManager)
lm's avatar
lm committed
672
    {
673
        // Only accept waypoints in global coordinate frame
lm's avatar
lm committed
674 675
        if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType())
        {
676 677 678 679 680
            // We're good, this is a global waypoint

            // Get the index of this waypoint
            // note the call to getGlobalFrameAndNavTypeIndexOf()
            // as we're only handling global waypoints
681
            int wpindex = currWPManager->getGlobalFrameAndNavTypeIndexOf(wp);
682
            // If not found, return (this should never happen, but helps safety)
683
            if (wpindex < 0) return;
684 685 686
            // Mark this wp as currently edited
            firingWaypointChange = wp;

687 688
            qDebug() << "UPDATING WAYPOINT" << wpindex << "IN 2D MAP";

689
            // Check if wp exists yet in map
lm's avatar
lm committed
690 691
            if (!waypointsToIcons.contains(wp))
            {
692
                // Create icon for new WP
693 694 695
                QColor wpColor(Qt::red);
                if (uasInstance) wpColor = uasInstance->getColor();
                Waypoint2DIcon* icon = new Waypoint2DIcon(map, this, wp, wpColor, wpindex);
696 697 698 699 700 701
                ConnectWP(icon);
                icon->setParentItem(map);
                // Update maps to allow inverse data association
                waypointsToIcons.insert(wp, icon);
                iconsToWaypoints.insert(icon, wp);

lm's avatar
lm committed
702 703 704 705
                // Add line element if this is NOT the first waypoint
                if (wpindex > 0)
                {
                    // Get predecessor of this WP
706
                    QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
lm's avatar
lm committed
707 708 709 710 711
                    Waypoint* wp1 = wps.at(wpindex-1);
                    mapcontrol::WayPointItem* prevIcon = waypointsToIcons.value(wp1, NULL);
                    // If we got a valid graphics item, continue
                    if (prevIcon)
                    {
712
                        mapcontrol::WaypointLineItem* line = new mapcontrol::WaypointLineItem(prevIcon, icon, wpColor, map);
lm's avatar
lm committed
713
                        line->setParentItem(map);
lm's avatar
lm committed
714 715 716 717
                        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
                        if (group)
                        {
                            group->addToGroup(line);
lm's avatar
lm committed
718
                            group->setParentItem(map);
lm's avatar
lm committed
719 720 721
                        }
                    }
                }
lm's avatar
lm committed
722 723 724
            }
            else
            {
725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747
                // Waypoint exists, block it's signals and update it
                mapcontrol::WayPointItem* icon = waypointsToIcons.value(wp);
                // Make sure we don't die on a null pointer
                // this should never happen, just a precaution
                if (!icon) return;
                // Block outgoing signals to prevent an infinite signal loop
                // should not happen, just a precaution
                this->blockSignals(true);
                // Update the WP
                Waypoint2DIcon* wpicon = dynamic_cast<Waypoint2DIcon*>(icon);
                if (wpicon)
                {
                    // Let icon read out values directly from waypoint
                    icon->SetNumber(wpindex);
                    wpicon->updateWaypoint();
                }
                else
                {
                    // Use safe standard interfaces for non Waypoint-class based wps
                    icon->SetCoord(internals::PointLatLng(wp->getLatitude(), wp->getLongitude()));
                    icon->SetAltitude(wp->getAltitude());
                    icon->SetHeading(wp->getYaw());
                    icon->SetNumber(wpindex);
748
                }
749 750 751
                // Re-enable signals again
                this->blockSignals(false);
            }
752

753
            firingWaypointChange = NULL;
754

lm's avatar
lm committed
755 756 757
        }
        else
        {
758 759 760 761
            // Check if the index of this waypoint is larger than the global
            // waypoint list. This implies that the coordinate frame of this
            // waypoint was changed and the list containing only global
            // waypoints was shortened. Thus update the whole list
762
            if (waypointsToIcons.count() > currWPManager->getGlobalFrameAndNavTypeCount())
lm's avatar
lm committed
763
            {
764
                updateWaypointList(uas);
765 766
            }
        }
767
    }
768 769 770 771 772 773 774 775 776
}

/**
 * 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 QGCMapWidget::updateWaypointList(int uas)
{
777
    qDebug() << "UPDATE WP LIST IN 2D MAP CALLED FOR UAS" << uas;
778 779
    // Currently only accept waypoint updates from the UAS in focus
    // this has to be changed to accept read-only updates from other systems as well.
lm's avatar
lm committed
780
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
781
    if (currWPManager)
lm's avatar
lm committed
782
    {
783 784
        // ORDER MATTERS HERE!
        // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
785

786 787
        qDebug() << "DELETING NOW OLD WPS";

788 789 790 791 792 793 794 795 796 797
        // Delete connecting waypoint lines
        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
        if (group)
        {
            foreach (QGraphicsItem* item, group->childItems())
            {
                delete item;
            }
        }

798 799
        // Delete first all old waypoints
        // this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
800
        QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
801
        foreach (Waypoint* wp, waypointsToIcons.keys())
802
        {
803 804
            if (!wps.contains(wp))
            {
lm's avatar
lm committed
805 806
                // Get icon to work on
                mapcontrol::WayPointItem* icon = waypointsToIcons.value(wp);
807 808 809 810
                waypointsToIcons.remove(wp);
                iconsToWaypoints.remove(icon);
                WPDelete(icon);
            }
811 812
        }

813 814 815 816 817 818
        // Update all existing waypoints
        foreach (Waypoint* wp, waypointsToIcons.keys())
        {
            // Update remaining waypoints
            updateWaypoint(uas, wp);
        }
819

820 821 822
        // Update all potentially new waypoints
        foreach (Waypoint* wp, wps)
        {
823
            qDebug() << "UPDATING NEW WP" << wp->getId();
824 825 826
            // Update / add only if new
            if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
        }
lm's avatar
lm committed
827 828 829 830 831 832 833 834 835 836 837

        // Add line element if this is NOT the first waypoint
        mapcontrol::WayPointItem* prevIcon = NULL;
        foreach (Waypoint* wp, wps)
        {
            mapcontrol::WayPointItem* currIcon = waypointsToIcons.value(wp, NULL);
            // Do not work on first waypoint, but only increment counter
            // do not continue if icon is invalid
            if (prevIcon && currIcon)
            {
                // If we got a valid graphics item, continue
838 839 840
                QColor wpColor(Qt::red);
                if (uasInstance) wpColor = uasInstance->getColor();
                mapcontrol::WaypointLineItem* line = new mapcontrol::WaypointLineItem(prevIcon, currIcon, wpColor, map);
lm's avatar
lm committed
841
                line->setParentItem(map);
lm's avatar
lm committed
842 843 844 845
                QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
                if (group)
                {
                    group->addToGroup(line);
lm's avatar
lm committed
846
                    group->setParentItem(map);
lm's avatar
lm committed
847 848 849 850
                }
            }
            prevIcon = currIcon;
        }
851
    }
lm's avatar
lm committed
852
}
853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895


//// ADAPTER / HELPER FUNCTIONS
//float QGCMapWidget::metersToPixels(double meters)
//{
//    return meters/map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat());
//}

//double QGCMapWidget::headingP1P2(internals::PointLatLng p1, internals::PointLatLng p2)
//{
//    double lat1 = p1.Lat() * deg_to_rad;
//    double lon1 = p2.Lng() * deg_to_rad;

//    double lat2 = p2.Lat() * deg_to_rad;
//    double lon2 = p2.Lng() * deg_to_rad;

//    double delta_lon = lon2 - lon1;

//    double y = sin(delta_lon) * cos(lat2);
//    double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_lon);
//    double heading = atan2(y, x) * rad_to_deg;

//    heading += 360;
//    while (heading < 0) bear += 360;
//    while (heading >= 360) bear -= 360;

//    return heading;
//}

//internals::PointLatLng QGCMapWidget::targetLatLon(internals::PointLatLng source, double heading, double dist)
//{
//    double lat1 = source.Lat() * deg_to_rad;
//    double lon1 = source.Lng() * deg_to_rad;

//    heading *= deg_to_rad;

//    double ad = dist / earth_mean_radius;

//    double lat2 = asin(sin(lat1) * cos(ad) + cos(lat1) * sin(ad) * cos(heading));
//    double lon2 = lon1 + atan2(sin(bear) * sin(ad) * cos(lat1), cos(ad) - sin(lat1) * sin(lat2));

//    return internals::PointLatLng(lat2 * rad_to_deg, lon2 * rad_to_deg);
//}