QGCMapWidget.cc 23.6 KB
Newer Older
lm's avatar
lm committed
1
#include "QGCMapWidget.h"
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"
lm's avatar
lm committed
8 9

QGCMapWidget::QGCMapWidget(QWidget *parent) :
10
        mapcontrol::OPMapWidget(parent),
11
        currWPManager(NULL),
12
        firingWaypointChange(NULL),
13 14
        maxUpdateInterval(2), // 2 seconds
        followUAVEnabled(false)
lm's avatar
lm committed
15
{
16
    // Widget is inactive until shown
17 18

    // Set cache mode
19 20 21 22 23 24 25 26 27 28 29 30 31
}

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

void QGCMapWidget::showEvent(QShowEvent* event)
{
    // Pass on to parent widget
    OPMapWidget::showEvent(event);

lm's avatar
lm committed
32
    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
33
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)));
lm's avatar
lm committed
34 35 36 37
    foreach (UASInterface* uas, UASManager::instance()->getUASList())
    {
        addUAS(uas);
    }
lm's avatar
lm committed
38 39 40 41


    internals::PointLatLng pos_lat_lon = internals::PointLatLng(0, 0);

lm's avatar
lm committed
42 43
    //        // **************
    //        // default home position
lm's avatar
lm committed
44

lm's avatar
lm committed
45 46 47
    //        home_position.coord = pos_lat_lon;
    //        home_position.altitude = altitude;
    //        home_position.locked = false;
lm's avatar
lm committed
48

lm's avatar
lm committed
49 50
    //        // **************
    //        // default magic waypoint params
lm's avatar
lm committed
51

lm's avatar
lm committed
52 53 54 55 56 57 58
    //        magic_waypoint.map_wp_item = NULL;
    //        magic_waypoint.coord = home_position.coord;
    //        magic_waypoint.altitude = altitude;
    //        magic_waypoint.description = "Magic waypoint";
    //        magic_waypoint.locked = false;
    //        magic_waypoint.time_seconds = 0;
    //        magic_waypoint.hold_time_seconds = 0;
lm's avatar
lm committed
59

lm's avatar
lm committed
60
    const int safe_area_radius_list[] = {5, 10, 20, 50, 100, 200, 500, 1000, 2000, 5000};   // meters
lm's avatar
lm committed
61

lm's avatar
lm committed
62
    const int uav_trail_time_list[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};                      // seconds
lm's avatar
lm committed
63

lm's avatar
lm committed
64
    const int uav_trail_distance_list[] = {1, 2, 5, 10, 20, 50, 100, 200, 500};             // meters
lm's avatar
lm committed
65

lm's avatar
lm committed
66 67
    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
68

lm's avatar
lm committed
69 70
    SetShowHome(true);					    // display the HOME position on the map
    SetShowUAV(true);					    // display the UAV position on the map
71
    //SetShowDiagnostics(true); // Not needed in flight / production mode
lm's avatar
lm committed
72

lm's avatar
lm committed
73 74
    Home->SetSafeArea(safe_area_radius_list[0]);                         // set radius (meters)
    Home->SetShowSafeArea(true);                                         // show the safe area
lm's avatar
lm committed
75

lm's avatar
lm committed
76 77
    UAV->SetTrailTime(uav_trail_time_list[0]);                           // seconds
    UAV->SetTrailDistance(uav_trail_distance_list[1]);                   // meters
lm's avatar
lm committed
78

79
    // UAV->SetTrailType(UAVTrailType::ByTimeElapsed);
lm's avatar
lm committed
80 81
    //  UAV->SetTrailType(UAVTrailType::ByDistance);

lm's avatar
lm committed
82 83
    GPS->SetTrailTime(uav_trail_time_list[0]);                           // seconds
    GPS->SetTrailDistance(uav_trail_distance_list[1]);                   // meters
lm's avatar
lm committed
84

85
    // GPS->SetTrailType(UAVTrailType::ByTimeElapsed);
lm's avatar
lm committed
86

lm's avatar
lm committed
87 88 89 90 91 92 93 94
    SetCurrentPosition(pos_lat_lon);         // set the map position
    Home->SetCoord(pos_lat_lon);             // set the HOME position
    UAV->SetUAVPos(pos_lat_lon, 0.0);        // set the UAV position
    GPS->SetUAVPos(pos_lat_lon, 0.0);        // set the UAV position

    setFrameStyle(QFrame::NoFrame);      // no border frame
    setBackgroundBrush(QBrush(Qt::black)); // tile background

95 96 97 98 99 100 101 102 103
    // Set current home position
    updateHomePosition(UASManager::instance()->getHomeLatitude(), UASManager::instance()->getHomeLongitude(), UASManager::instance()->getHomeAltitude());

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

    // FIXME XXX this is a hack to trick OPs current 1-system design
    SetShowUAV(false);

104 105 106 107 108

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


lm's avatar
lm committed
109
    setFocus();
110 111 112 113

    // Start timer
    connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateGlobalPosition()));
    updateTimer.start(maxUpdateInterval*1000);
114
    updateGlobalPosition();
LM's avatar
LM committed
115
    QTimer::singleShot(1, this, SLOT(loadSettings()));
lm's avatar
lm committed
116 117
}

118
void QGCMapWidget::hideEvent(QHideEvent* event)
lm's avatar
lm committed
119
{
120 121
    storeSettings();
    OPMapWidget::hideEvent(event);
lm's avatar
lm committed
122
}
lm's avatar
lm committed
123

124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
void QGCMapWidget::loadSettings()
{
    // Atlantic Ocean near Africa, coordinate origin
    double lastZoom = 1;
    double lastLat = 0;
    double lastLon = 0;

    QSettings settings;
    settings.beginGroup("QGC_MAPWIDGET");
    lastLat = settings.value("LAST_LATITUDE", lastLat).toDouble();
    lastLon = settings.value("LAST_LONGITUDE", lastLon).toDouble();
    lastZoom = settings.value("LAST_ZOOM", lastZoom).toDouble();
    settings.endGroup();

    // SET INITIAL POSITION AND ZOOM
    internals::PointLatLng pos_lat_lon = internals::PointLatLng(lastLat, lastLon);
    SetCurrentPosition(pos_lat_lon);        // set the map position
141
    SetZoom(lastZoom); // set map zoom level
142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157
}

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());
    settings.endGroup();
    settings.sync();
}

void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
{
lm's avatar
lm committed
158

lm's avatar
lm committed
159
    // FIXME HACK!
lm's avatar
lm committed
160
    //if (currEditMode == EDIT_MODE_WAYPOINTS)
161 162 163 164 165
    {
        // If a waypoint manager is available
        if (currWPManager)
        {
            // Create new waypoint
lm's avatar
lm committed
166
            internals::PointLatLng pos = map->FromLocalToLatLng(event->pos().x(), event->pos().y());
167
            Waypoint* wp = currWPManager->createWaypoint();
lm's avatar
lm committed
168
//            wp->blockSignals(true);
lm's avatar
lm committed
169
//            wp->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
170 171
            wp->setLatitude(pos.Lat());
            wp->setLongitude(pos.Lng());
lm's avatar
lm committed
172 173 174
            wp->setAltitude(0);
//            wp->blockSignals(false);
//            currWPManager->notifyOfChange(wp);
175 176
        }
    }
lm's avatar
lm committed
177
    OPMapWidget::mouseDoubleClickEvent(event);
178 179 180
}


lm's avatar
lm committed
181 182 183 184 185 186
/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void QGCMapWidget::addUAS(UASInterface* uas)
{
187
    qDebug() << "ADDING UAS";
lm's avatar
lm committed
188 189
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
    //connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
190
    connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
lm's avatar
lm committed
191 192
}

193 194 195 196 197 198 199 200 201 202 203
void QGCMapWidget::activeUASSet(UASInterface* uas)
{
    // Only execute if proper UAS is set
    if (!uas || !dynamic_cast<UASInterface*>(uas)) return;

    // Disconnect old MAV manager
    if (currWPManager) {
        // Disconnect the waypoint manager / data storage from the UI
        disconnect(currWPManager, SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
        disconnect(currWPManager, SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
        disconnect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypoint(Waypoint*)));
204
        disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChange(Waypoint*)));
205 206 207 208 209 210 211 212 213 214 215 216
    }

    if (uas) {
        currWPManager = uas->getWaypointManager();

        // Delete all waypoints and add waypoint from new system
        updateWaypointList(uas->getUASID());

        // Connect the waypoint manager / data storage to the UI
        connect(currWPManager, SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
        connect(currWPManager, SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
        connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypoint(Waypoint*)));
217
        connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChange(Waypoint*)));
218
        updateSelectedSystem(uas->getUASID());
219
        followUAVID = uas->getUASID();
220 221 222
    }
}

lm's avatar
lm committed
223 224 225 226 227 228 229 230 231 232 233 234 235
/**
 * 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);

236 237
    // Immediate update
    if (maxUpdateInterval == 0)
238
    {
239 240 241 242 243 244 245 246 247 248 249 250 251 252
        // 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());
        }

        // Set new lat/lon position of UAV icon
        internals::PointLatLng pos_lat_lon = internals::PointLatLng(lat, lon);
        uav->SetUAVPos(pos_lat_lon, alt);
253 254
        // Follow status
        if (followUAVEnabled && uas->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
255 256
        // Convert from radians to degrees and apply
        uav->SetUAVHeading((uas->getYaw()/M_PI)*180.0f);
257
    }
258
}
lm's avatar
lm committed
259

260 261 262 263 264 265 266 267 268 269 270 271 272 273
/**
 * 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);
274 275
            AddUAV(system->getUASID(), newUAV);
            uav = newUAV;
276 277 278 279 280
        }

        // 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());
281 282
        // Follow status
        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
283 284 285
        // Convert from radians to degrees and apply
        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
    }
286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303
}


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

/**
304
 * Does not update the system type or configuration, only the selected status
305 306 307 308 309 310 311 312 313 314 315 316 317 318 319
 */
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));
        }
    }
}


320 321 322 323 324 325
// MAP NAVIGATION
void QGCMapWidget::showGoToDialog()
{
    bool ok;
    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
326
                                         QString("%1,%2").arg(CurrentPosition().Lat(), 0, 'g', 6).arg(CurrentPosition().Lng(), 0, 'g', 6), &ok);
327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351
    if (ok && !text.isEmpty()) {
        QStringList split = text.split(",");
        if (split.length() == 2) {
            bool convert;
            double latitude = split.first().toDouble(&convert);
            ok &= convert;
            double longitude = split.last().toDouble(&convert);
            ok &= convert;

            if (ok) {
                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);
    SetShowHome(true);                      // display the HOME position on the map
}

352 353 354 355 356
void QGCMapWidget::goHome()
{
    SetCurrentPosition(Home->Coord());
}

357 358 359 360 361 362 363 364 365 366
/**
 * 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);
}

367 368 369 370 371 372 373 374 375 376 377 378 379 380
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();
    }
381 382 383 384 385 386
    else
    {
        RipMap();
        // Set empty area = unselect area
        map->SetSelectedArea(internals::RectLatLng());
    }
387 388
}

389

390 391 392 393 394 395 396
// WAYPOINT MAP INTERACTION FUNCTIONS

void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
{
    // Block circle updates
    Waypoint* wp = iconsToWaypoints.value(waypoint, NULL);
    // Protect from vicious double update cycle
397
    if (firingWaypointChange == wp) return;
398 399
    // Not in cycle, block now from entering it
    firingWaypointChange = wp;
400
    qDebug() << "UPDATING WP FROM MAP";
401 402 403

    // Update WP values
    internals::PointLatLng pos = waypoint->Coord();
404 405 406

    // Block waypoint signals
    wp->blockSignals(true);
407 408 409
    wp->setLatitude(pos.Lat());
    wp->setLongitude(pos.Lng());
    wp->setAltitude(waypoint->Altitude());
410
    wp->blockSignals(false);
411

412

413 414 415 416 417 418
    internals::PointLatLng coord = waypoint->Coord();
    QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + "   " + QString::number(coord.Lng(), 'f', 6);
    qDebug() << "MAP WP COORD (MAP):" << coord_str << __FILE__ << __LINE__;
    QString wp_str = QString::number(wp->getLatitude(), 'f', 6) + "   " + QString::number(wp->getLongitude(), 'f', 6);
    qDebug() << "MAP WP COORD (WP):" << wp_str << __FILE__ << __LINE__;

419
    firingWaypointChange = NULL;
420 421

    emit waypointChanged(wp);
422
}
423 424

// WAYPOINT UPDATE FUNCTIONS
425 426

/**
427 428
 * This function is called if a a single waypoint is updated and
 * also if the whole list changes.
429
 */
430
void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
431
{
432 433
    // Source of the event was in this widget, do nothing
    if (firingWaypointChange == wp) return;
434 435
    // 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.
436 437
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
    if (uasInstance->getWaypointManager() == currWPManager || uas == -1) {
438 439 440 441 442 443 444
        // Only accept waypoints in global coordinate frame
        if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType()) {
            // 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
445
            int wpindex = currWPManager->getGlobalFrameAndNavTypeIndexOf(wp);
446 447 448 449 450 451 452 453
            // If not found, return (this should never happen, but helps safety)
            if (wpindex == -1) return;
            // Mark this wp as currently edited
            firingWaypointChange = wp;

            // Check if wp exists yet in map
            if (!waypointsToIcons.contains(wp)) {
                // Create icon for new WP
454 455 456
                QColor wpColor(Qt::red);
                if (uasInstance) wpColor = uasInstance->getColor();
                Waypoint2DIcon* icon = new Waypoint2DIcon(map, this, wp, wpColor, wpindex);
457 458 459 460 461 462
                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
463 464 465 466 467 468 469 470 471 472
                // Add line element if this is NOT the first waypoint
                if (wpindex > 0)
                {
                    // Get predecessor of this WP
                    QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
                    Waypoint* wp1 = wps.at(wpindex-1);
                    mapcontrol::WayPointItem* prevIcon = waypointsToIcons.value(wp1, NULL);
                    // If we got a valid graphics item, continue
                    if (prevIcon)
                    {
473
                        mapcontrol::WaypointLineItem* line = new mapcontrol::WaypointLineItem(prevIcon, icon, wpColor, map);
lm's avatar
lm committed
474
                        line->setParentItem(map);
lm's avatar
lm committed
475 476 477 478
                        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
                        if (group)
                        {
                            group->addToGroup(line);
lm's avatar
lm committed
479
                            group->setParentItem(map);
lm's avatar
lm committed
480 481 482
                        }
                    }
                }
483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506
            } else {
                // 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);
507
                }
508 509 510
                // Re-enable signals again
                this->blockSignals(false);
            }
511

512
            firingWaypointChange = NULL;
513

514 515 516 517 518
        } else {
            // 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
519
            if (waypointsToIcons.size() > currWPManager->getGlobalFrameAndNavTypeCount()) {
520
                updateWaypointList(uas);
521 522
            }
        }
523
    }
524 525 526 527 528 529 530 531 532 533 534
}

/**
 * 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)
{
    // 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
535
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
536
    if ((uasInstance && (uasInstance->getWaypointManager() == currWPManager)) || uas == -1) {
537 538
        // ORDER MATTERS HERE!
        // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
539

540 541 542 543
        // Delete first all old waypoints
        // this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
        QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
        foreach (Waypoint* wp, waypointsToIcons.keys())
544
        {
545 546 547 548 549 550 551 552
            // Get icon to work on
            mapcontrol::WayPointItem* icon = waypointsToIcons.value(wp);
            if (!wps.contains(wp))
            {
                waypointsToIcons.remove(wp);
                iconsToWaypoints.remove(icon);
                WPDelete(icon);
            }
553 554
        }

555 556 557 558 559 560
        // Update all existing waypoints
        foreach (Waypoint* wp, waypointsToIcons.keys())
        {
            // Update remaining waypoints
            updateWaypoint(uas, wp);
        }
561

562 563 564 565 566 567
        // Update all potentially new waypoints
        foreach (Waypoint* wp, wps)
        {
            // Update / add only if new
            if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
        }
lm's avatar
lm committed
568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588

        // Delete connecting waypoint lines
        QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
        if (group)
        {
            foreach (QGraphicsItem* item, group->childItems())
            {
                delete item;
            }
        }

        // 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
589 590 591
                QColor wpColor(Qt::red);
                if (uasInstance) wpColor = uasInstance->getColor();
                mapcontrol::WaypointLineItem* line = new mapcontrol::WaypointLineItem(prevIcon, currIcon, wpColor, map);
lm's avatar
lm committed
592
                line->setParentItem(map);
lm's avatar
lm committed
593 594 595 596
                QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
                if (group)
                {
                    group->addToGroup(line);
lm's avatar
lm committed
597
                    group->setParentItem(map);
lm's avatar
lm committed
598 599 600 601
                }
            }
            prevIcon = currIcon;
        }
602
    }
lm's avatar
lm committed
603
}
604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646


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