QGCMapWidget.cc 29.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

    this->setContextMenuPolicy(Qt::ActionsContextMenu);

35 36 37 38 39 40 41 42 43 44 45 46
    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);
    QAction *cameraaction = new QAction(this);
    cameraaction->setText("Point Camera Here");
    connect(cameraaction,SIGNAL(triggered()),this,SLOT(cameraActionTriggered()));
    this->addAction(cameraaction);
47 48 49
}
void QGCMapWidget::guidedActionTriggered()
{
50 51 52 53 54
    if (!uas)
    {
        QMessageBox::information(0,"Error","Please connect first");
        return;
    }
55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75
    if (currWPManager)
    {
        if (defaultGuidedAlt == -1)
        {
            if (!guidedAltActionTriggered())
            {
                return;
            }
        }
        // Create new waypoint and send it to the WPManager to send out.
        internals::PointLatLng pos = map->FromLocalToLatLng(mousePressPos.x(), mousePressPos.y());
        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()
{
76 77 78 79 80
    if (!uas)
    {
        QMessageBox::information(0,"Error","Please connect first");
        return false;
    }
81 82 83 84 85 86 87 88 89 90 91
    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;
}
92 93
void QGCMapWidget::cameraActionTriggered()
{
94 95 96 97 98
    if (!uas)
    {
        QMessageBox::information(0,"Error","Please connect first");
        return;
    }
99 100 101 102 103 104 105 106
    ArduPilotMegaMAV *newmav = qobject_cast<ArduPilotMegaMAV*>(this->uas);
    if (newmav)
    {
        newmav->setMountConfigure(4,true,true,true);
        internals::PointLatLng pos = map->FromLocalToLatLng(mousePressPos.x(), mousePressPos.y());
        newmav->setMountControl(pos.Lat(),pos.Lng(),100,true);
    }
}
107 108 109 110 111 112 113 114 115 116

void QGCMapWidget::mousePressEvent(QMouseEvent *event)
{
    mapcontrol::OPMapWidget::mousePressEvent(event);
}

void QGCMapWidget::mouseReleaseEvent(QMouseEvent *event)
{
    mousePressPos = event->pos();
    mapcontrol::OPMapWidget::mouseReleaseEvent(event);
117 118 119 120 121 122
}

QGCMapWidget::~QGCMapWidget()
{
    SetShowHome(false);	// doing this appears to stop the map lib crashing on exit
    SetShowUAV(false);	//   "          "
123
    storeSettings();
124 125 126 127
}

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

131 132 133
    // Pass on to parent widget
    OPMapWidget::showEvent(event);

134 135
    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)), Qt::UniqueConnection);
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)), Qt::UniqueConnection);
136 137
    connect(UASManager::instance(), SIGNAL(homePositionChanged(double,double,double)), this, SLOT(updateHomePosition(double,double,double)));

lm's avatar
lm committed
138 139 140 141
    foreach (UASInterface* uas, UASManager::instance()->getUASList())
    {
        addUAS(uas);
    }
lm's avatar
lm committed
142 143


144 145 146
    if (!mapInitialized)
    {
        internals::PointLatLng pos_lat_lon = internals::PointLatLng(0, 0);
lm's avatar
lm committed
147

148 149
        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
150

151
        SetShowHome(true);					    // display the HOME position on the map
152 153
        Home->SetSafeArea(0);                         // set radius (meters)
        Home->SetShowSafeArea(false);                                         // show the safe area
154
        Home->SetCoord(pos_lat_lon);             // set the HOME position
lm's avatar
lm committed
155

156 157
        setFrameStyle(QFrame::NoFrame);      // no border frame
        setBackgroundBrush(QBrush(Qt::black)); // tile background
lm's avatar
lm committed
158

159 160
        // Set current home position
        updateHomePosition(UASManager::instance()->getHomeLatitude(), UASManager::instance()->getHomeLongitude(), UASManager::instance()->getHomeAltitude());
161

162 163
        // Set currently selected system
        activeUASSet(UASManager::instance()->getActiveUAS());
164

165 166
        // Connect map updates to the adapter slots
        connect(this, SIGNAL(WPValuesChanged(WayPointItem*)), this, SLOT(handleMapWaypointEdit(WayPointItem*)));
167

168 169
        SetCurrentPosition(pos_lat_lon);         // set the map position
        setFocus();
170

171 172 173
        // Start timer
        connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateGlobalPosition()));
        mapInitialized = true;
174
        //QTimer::singleShot(800, this, SLOT(loadSettings()));
175
    }
176
    updateTimer.start(maxUpdateInterval*1000);
177
    // Update all UAV positions
178
    updateGlobalPosition();
lm's avatar
lm committed
179 180
}

181
void QGCMapWidget::hideEvent(QHideEvent* event)
lm's avatar
lm committed
182
{
183
    updateTimer.stop();
184 185
    storeSettings();
    OPMapWidget::hideEvent(event);
lm's avatar
lm committed
186
}
lm's avatar
lm committed
187

188 189 190 191
/**
 * @param changePosition Load also the last position from settings and update the map position.
 */
void QGCMapWidget::loadSettings(bool changePosition)
192 193 194 195 196 197 198 199
{
    // Atlantic Ocean near Africa, coordinate origin
    double lastZoom = 1;
    double lastLat = 0;
    double lastLon = 0;

    QSettings settings;
    settings.beginGroup("QGC_MAPWIDGET");
200 201 202 203 204 205 206 207
    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();
208 209
    settings.endGroup();

210 211 212 213 214
    // SET CORRECT MENU CHECKBOXES
    // Set the correct trail interval
    if (trailType == mapcontrol::UAVTrailType::ByDistance)
    {
        // XXX
215 216 217
#ifdef Q_OS_WIN
#pragma message ("WARNING: Settings loading for trail type not implemented")
#else
218
#warning Settings loading for trail type not implemented
219
#endif
220 221 222 223 224 225
    }
    else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
    {
        // XXX
    }

226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241
    // 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);
        }
    }

242 243 244
    // SET INITIAL POSITION AND ZOOM
    internals::PointLatLng pos_lat_lon = internals::PointLatLng(lastLat, lastLon);
    SetCurrentPosition(pos_lat_lon);        // set the map position
245
    SetZoom(lastZoom); // set map zoom level
246 247 248 249 250 251 252 253 254 255
}

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());
256 257
    settings.setValue("TRAIL_TYPE", static_cast<int>(trailType));
    settings.setValue("TRAIL_INTERVAL", trailInterval);
258 259 260 261 262 263
    settings.endGroup();
    settings.sync();
}

void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
{
264 265
    // If a waypoint manager is available
    if (currWPManager)
266
    {
267 268 269 270 271 272 273 274 275 276 277
        // 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);
278
    }
279

lm's avatar
lm committed
280
    OPMapWidget::mouseDoubleClickEvent(event);
281 282 283
}


lm's avatar
lm committed
284 285
/**
 *
286
 * @param uas the UAS/MAV to monitor/display with the map widget
lm's avatar
lm committed
287 288 289 290
 */
void QGCMapWidget::addUAS(UASInterface* uas)
{
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
291
    connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
292 293 294 295 296 297 298 299
    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
300 301
}

302 303 304
void QGCMapWidget::activeUASSet(UASInterface* uas)
{
    // Only execute if proper UAS is set
305
    this->uas = uas;
306 307

    // Disconnect old MAV manager
308 309
    if (currWPManager)
    {
310
        // Disconnect the waypoint manager / data storage from the UI
311 312 313 314
        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*)));
315 316
    }

317 318 319 320 321
    // Attach the new waypoint manager if a new UAS was selected. Otherwise, indicate
    // that no such manager exists.
    if (uas)
    {
        currWPManager = uas->getWaypointManager();
322

323 324 325
        updateSelectedSystem(uas->getUASID());
        followUAVID = uas->getUASID();
        updateWaypointList(uas->getUASID());
326

327 328 329 330 331 332 333 334 335 336
        // 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;
    }
337 338
}

lm's avatar
lm committed
339 340 341 342 343 344 345 346 347 348 349 350 351
/**
 * 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);

352 353
    // Immediate update
    if (maxUpdateInterval == 0)
354
    {
355 356 357 358 359 360 361 362 363
        // 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());
364 365 366 367 368 369 370 371 372 373 374
            // 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);
            }
375 376 377 378 379
        }

        // Set new lat/lon position of UAV icon
        internals::PointLatLng pos_lat_lon = internals::PointLatLng(lat, lon);
        uav->SetUAVPos(pos_lat_lon, alt);
380 381
        // Follow status
        if (followUAVEnabled && uas->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
382 383
        // Convert from radians to degrees and apply
        uav->SetUAVHeading((uas->getYaw()/M_PI)*180.0f);
384
    }
385
}
lm's avatar
lm committed
386

387 388 389 390 391 392 393 394 395 396 397 398 399 400
/**
 * 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);
401 402
            AddUAV(system->getUASID(), newUAV);
            uav = newUAV;
LM's avatar
LM committed
403 404 405
            uav->SetTrailTime(1);
            uav->SetTrailDistance(5);
            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
406 407 408 409 410
        }

        // 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());
411 412
        // Follow status
        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
413 414 415
        // Convert from radians to degrees and apply
        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
    }
416 417
}

418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447
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()
{
448
    updateLocalPosition();
449 450
}

451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466

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();
        }
    }
}

/**
467
 * Does not update the system type or configuration, only the selected status
468 469 470 471 472 473 474 475 476 477 478 479 480 481 482
 */
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));
        }
    }
}


483 484 485 486 487 488
// MAP NAVIGATION
void QGCMapWidget::showGoToDialog()
{
    bool ok;
    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
489
                                         QString("%1,%2").arg(CurrentPosition().Lat(), 0, 'g', 6).arg(CurrentPosition().Lng(), 0, 'g', 6), &ok);
490 491
    if (ok && !text.isEmpty())
    {
492
        QStringList split = text.split(",");
493 494
        if (split.length() == 2)
        {
495 496 497 498 499 500
            bool convert;
            double latitude = split.first().toDouble(&convert);
            ok &= convert;
            double longitude = split.last().toDouble(&convert);
            ok &= convert;

501 502
            if (ok)
            {
503 504 505 506 507 508 509 510 511 512 513 514
                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);
515
    homeAltitude = altitude;
516 517 518
    SetShowHome(true);                      // display the HOME position on the map
}

519 520 521 522 523
void QGCMapWidget::goHome()
{
    SetCurrentPosition(Home->Coord());
}

524 525 526 527 528 529 530 531 532 533
/**
 * 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);
}

534 535 536 537 538 539 540 541 542 543 544 545 546 547
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();
    }
548 549 550 551 552 553
    else
    {
        RipMap();
        // Set empty area = unselect area
        map->SetSelectedArea(internals::RectLatLng());
    }
554 555
}

556

557 558 559 560 561 562
// WAYPOINT MAP INTERACTION FUNCTIONS

void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
{
    // Block circle updates
    Waypoint* wp = iconsToWaypoints.value(waypoint, NULL);
563 564 565 566 567

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

568
    // Protect from vicious double update cycle
569
    if (firingWaypointChange == wp) return;
570 571
    // Not in cycle, block now from entering it
    firingWaypointChange = wp;
572
    // // qDebug() << "UPDATING WP FROM MAP";
573 574 575

    // Update WP values
    internals::PointLatLng pos = waypoint->Coord();
576 577 578

    // Block waypoint signals
    wp->blockSignals(true);
579 580
    wp->setLatitude(pos.Lat());
    wp->setLongitude(pos.Lng());
581
    // XXX Magic values
582 583
//    wp->setAltitude(homeAltitude + 50.0f);
//    wp->setAcceptanceRadius(10.0f);
584
    wp->blockSignals(false);
585

586

587 588
    internals::PointLatLng coord = waypoint->Coord();
    QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + "   " + QString::number(coord.Lng(), 'f', 6);
589
    // // qDebug() << "MAP WP COORD (MAP):" << coord_str << __FILE__ << __LINE__;
590
    QString wp_str = QString::number(wp->getLatitude(), 'f', 6) + "   " + QString::number(wp->getLongitude(), 'f', 6);
591
    // // qDebug() << "MAP WP COORD (WP):" << wp_str << __FILE__ << __LINE__;
592

593
    firingWaypointChange = NULL;
594 595

    emit waypointChanged(wp);
596
}
597 598

// WAYPOINT UPDATE FUNCTIONS
599 600

/**
601 602
 * This function is called if a a single waypoint is updated and
 * also if the whole list changes.
603
 */
604
void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
605
{
606
    //qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED";
607
    // Source of the event was in this widget, do nothing
608 609 610
    if (firingWaypointChange == wp) {
        return;
    }
611 612
    // 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.
613
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
614
    if (currWPManager)
lm's avatar
lm committed
615
    {
616
        // Only accept waypoints in global coordinate frame
lm's avatar
lm committed
617 618
        if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType())
        {
619 620 621 622 623
            // 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
624
            int wpindex = currWPManager->getGlobalFrameAndNavTypeIndexOf(wp);
625
            // If not found, return (this should never happen, but helps safety)
626
            if (wpindex < 0) return;
627 628 629
            // Mark this wp as currently edited
            firingWaypointChange = wp;

630 631
            qDebug() << "UPDATING WAYPOINT" << wpindex << "IN 2D MAP";

632
            // Check if wp exists yet in map
lm's avatar
lm committed
633 634
            if (!waypointsToIcons.contains(wp))
            {
635
                // Create icon for new WP
636 637 638
                QColor wpColor(Qt::red);
                if (uasInstance) wpColor = uasInstance->getColor();
                Waypoint2DIcon* icon = new Waypoint2DIcon(map, this, wp, wpColor, wpindex);
639 640 641 642 643 644
                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
645 646 647 648
                // Add line element if this is NOT the first waypoint
                if (wpindex > 0)
                {
                    // Get predecessor of this WP
649
                    QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
lm's avatar
lm committed
650 651 652 653 654
                    Waypoint* wp1 = wps.at(wpindex-1);
                    mapcontrol::WayPointItem* prevIcon = waypointsToIcons.value(wp1, NULL);
                    // If we got a valid graphics item, continue
                    if (prevIcon)
                    {
655
                        mapcontrol::WaypointLineItem* line = new mapcontrol::WaypointLineItem(prevIcon, icon, wpColor, map);
lm's avatar
lm committed
656
                        line->setParentItem(map);
lm's avatar
lm committed
657 658 659 660
                        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
                        if (group)
                        {
                            group->addToGroup(line);
lm's avatar
lm committed
661
                            group->setParentItem(map);
lm's avatar
lm committed
662 663 664
                        }
                    }
                }
lm's avatar
lm committed
665 666 667
            }
            else
            {
668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690
                // 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);
691
                }
692 693 694
                // Re-enable signals again
                this->blockSignals(false);
            }
695

696
            firingWaypointChange = NULL;
697

lm's avatar
lm committed
698 699 700
        }
        else
        {
701 702 703 704
            // 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
705
            if (waypointsToIcons.count() > currWPManager->getGlobalFrameAndNavTypeCount())
lm's avatar
lm committed
706
            {
707
                updateWaypointList(uas);
708 709
            }
        }
710
    }
711 712 713 714 715 716 717 718 719
}

/**
 * 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)
{
720
    qDebug() << "UPDATE WP LIST IN 2D MAP CALLED FOR UAS" << uas;
721 722
    // 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
723
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
724
    if (currWPManager)
lm's avatar
lm committed
725
    {
726 727
        // ORDER MATTERS HERE!
        // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
728

729 730
        qDebug() << "DELETING NOW OLD WPS";

731 732 733 734 735 736 737 738 739 740
        // Delete connecting waypoint lines
        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
        if (group)
        {
            foreach (QGraphicsItem* item, group->childItems())
            {
                delete item;
            }
        }

741 742
        // Delete first all old waypoints
        // this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
743
        QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
744
        foreach (Waypoint* wp, waypointsToIcons.keys())
745
        {
746 747
            if (!wps.contains(wp))
            {
lm's avatar
lm committed
748 749
                // Get icon to work on
                mapcontrol::WayPointItem* icon = waypointsToIcons.value(wp);
750 751 752 753
                waypointsToIcons.remove(wp);
                iconsToWaypoints.remove(icon);
                WPDelete(icon);
            }
754 755
        }

756 757 758 759 760 761
        // Update all existing waypoints
        foreach (Waypoint* wp, waypointsToIcons.keys())
        {
            // Update remaining waypoints
            updateWaypoint(uas, wp);
        }
762

763 764 765
        // Update all potentially new waypoints
        foreach (Waypoint* wp, wps)
        {
766
            qDebug() << "UPDATING NEW WP" << wp->getId();
767 768 769
            // Update / add only if new
            if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
        }
lm's avatar
lm committed
770 771 772 773 774 775 776 777 778 779 780

        // 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
781 782 783
                QColor wpColor(Qt::red);
                if (uasInstance) wpColor = uasInstance->getColor();
                mapcontrol::WaypointLineItem* line = new mapcontrol::WaypointLineItem(prevIcon, currIcon, wpColor, map);
lm's avatar
lm committed
784
                line->setParentItem(map);
lm's avatar
lm committed
785 786 787 788
                QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
                if (group)
                {
                    group->addToGroup(line);
lm's avatar
lm committed
789
                    group->setParentItem(map);
lm's avatar
lm committed
790 791 792 793
                }
            }
            prevIcon = currIcon;
        }
794
    }
lm's avatar
lm committed
795
}
796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838


//// 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);
//}