QGCMapWidget.cc 16.8 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 "UASWaypointManager.h"
lm's avatar
lm committed
7 8

QGCMapWidget::QGCMapWidget(QWidget *parent) :
9
        mapcontrol::OPMapWidget(parent),
10 11
        currWPManager(NULL),
        firingWaypointChange(NULL)
lm's avatar
lm committed
12
{
lm's avatar
lm committed
13
    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
14
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)));
lm's avatar
lm committed
15 16 17 18
    foreach (UASInterface* uas, UASManager::instance()->getUASList())
    {
        addUAS(uas);
    }
lm's avatar
lm committed
19 20 21 22


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

lm's avatar
lm committed
23 24
    //        // **************
    //        // default home position
lm's avatar
lm committed
25

lm's avatar
lm committed
26 27 28
    //        home_position.coord = pos_lat_lon;
    //        home_position.altitude = altitude;
    //        home_position.locked = false;
lm's avatar
lm committed
29

lm's avatar
lm committed
30 31
    //        // **************
    //        // default magic waypoint params
lm's avatar
lm committed
32

lm's avatar
lm committed
33 34 35 36 37 38 39
    //        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
40

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

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

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

lm's avatar
lm committed
47 48
    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
49

lm's avatar
lm committed
50 51
    SetShowHome(true);					    // display the HOME position on the map
    SetShowUAV(true);					    // display the UAV position on the map
52
    //SetShowDiagnostics(true); // Not needed in flight / production mode
lm's avatar
lm committed
53

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

lm's avatar
lm committed
57 58
    UAV->SetTrailTime(uav_trail_time_list[0]);                           // seconds
    UAV->SetTrailDistance(uav_trail_distance_list[1]);                   // meters
lm's avatar
lm committed
59

60
    // UAV->SetTrailType(UAVTrailType::ByTimeElapsed);
lm's avatar
lm committed
61 62
    //  UAV->SetTrailType(UAVTrailType::ByDistance);

lm's avatar
lm committed
63 64
    GPS->SetTrailTime(uav_trail_time_list[0]);                           // seconds
    GPS->SetTrailDistance(uav_trail_distance_list[1]);                   // meters
lm's avatar
lm committed
65

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

lm's avatar
lm committed
68 69 70 71 72 73 74 75
    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

76 77 78 79 80 81 82 83 84
    // 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);

85 86 87 88 89

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


lm's avatar
lm committed
90 91 92 93 94 95 96
    setFocus();
}

QGCMapWidget::~QGCMapWidget()
{
    SetShowHome(false);	// doing this appears to stop the map lib crashing on exit
    SetShowUAV(false);	//   "          "
lm's avatar
lm committed
97
}
lm's avatar
lm committed
98 99 100 101 102 103 104

/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void QGCMapWidget::addUAS(UASInterface* uas)
{
105
    qDebug() << "ADDING UAS";
lm's avatar
lm committed
106 107
    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)));
108
    connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
lm's avatar
lm committed
109 110
}

111 112 113 114 115 116 117 118 119 120 121
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*)));
122
        disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChange(Waypoint*)));
123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141
    }

    if (uas) {
        currWPManager = uas->getWaypointManager();
//        QColor color = mav->getColor();
//        color.setAlphaF(0.9);
//        QPen* pen = new QPen(color);
//        pen->setWidth(3.0);
//        mavPens.insert(mav->getUASID(), pen);
//        // FIXME Remove after refactoring
//        waypointPath->setPen(pen);

        // 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*)));
142
        connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChange(Waypoint*)));
143 144 145 146
        updateSelectedSystem(uas->getUASID());
    }
}

lm's avatar
lm committed
147 148 149 150 151 152 153 154 155 156 157 158 159
/**
 * 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);

160 161 162 163
    // 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)
164
    {
165 166 167
        MAV2DIcon* newUAV = new MAV2DIcon(map, this, uas);
        newUAV->setParentItem(map);
        UAVS.insert(uas->getUASID(), newUAV);
168
        uav = GetUAV(uas->getUASID());
169
    }
lm's avatar
lm committed
170

171 172 173
    // Set new lat/lon position of UAV icon
    internals::PointLatLng pos_lat_lon = internals::PointLatLng(lat, lon);
    uav->SetUAVPos(pos_lat_lon, alt);
174
    // Convert from radians to degrees and apply
175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
    uav->SetUAVHeading((uas->getYaw()/M_PI)*180.0f);
}


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

/**
194
 * Does not update the system type or configuration, only the selected status
195 196 197 198 199 200 201 202 203 204 205 206 207 208 209
 */
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));
        }
    }
}


210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242
// MAP NAVIGATION
void QGCMapWidget::showGoToDialog()
{
    bool ok;
    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
                                         QString("%1,%2").arg(CurrentPosition().Lat()).arg( CurrentPosition().Lng()), &ok);
    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
}


243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268
// WAYPOINT MAP INTERACTION FUNCTIONS

//void QGCMapWidget::createWaypointAtMousePos(QMouseEvent)
//{

//}

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

    // Update WP values
    internals::PointLatLng pos = waypoint->Coord();
    wp->setLatitude(pos.Lat());
    wp->setLongitude(pos.Lng());
    wp->setAltitude(waypoint->Altitude());

    emit waypointChanged(wp);
    firingWaypointChange = NULL;
}
269 270

// WAYPOINT UPDATE FUNCTIONS
271 272

/**
273 274
 * This function is called if a a single waypoint is updated and
 * also if the whole list changes.
275
 */
276
void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
277
{
278 279
    // Source of the event was in this widget, do nothing
    if (firingWaypointChange == wp) return;
280 281 282 283 284 285 286 287 288 289 290 291 292
        // 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.
        if (UASManager::instance()->getUASForId(uas)->getWaypointManager() == currWPManager) {
            // Only accept waypoints in global coordinate frame
            if (wp->getFrame() == MAV_FRAME_GLOBAL && 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
                int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeIndexOf(wp);
                // If not found, return (this should never happen, but helps safety)
                if (wpindex == -1) return;
293 294
                // Mark this wp as currently edited
                firingWaypointChange = wp;
295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320

                // Check if wp exists yet in map
                if (!waypointsToIcons.contains(wp)) {
                    // Create icon for new WP
                    mapcontrol::WayPointItem* icon = WPCreate(internals::PointLatLng(wp->getLatitude(), wp->getLongitude()), wp->getAltitude(), wp->getDescription());
                    icon->SetHeading(wp->getYaw());
                    // Update maps to allow inverse data association
                    waypointsToIcons.insert(wp, icon);
                    iconsToWaypoints.insert(icon, wp);
                } 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
                    icon->SetCoord(internals::PointLatLng(wp->getLatitude(), wp->getLongitude()));
                    icon->SetAltitude(wp->getAltitude());
                    icon->SetHeading(wp->getYaw());
                    icon->SetNumber(wp->getId());
                    // Re-enable signals again
                    this->blockSignals(false);
                }
321 322
                firingWaypointChange = NULL;

323 324 325 326 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 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414
            } 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
                if (waypointsToIcons.size() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeCount()) {
                    updateWaypointList(uas);
                }
            }
        }
}

/**
 * 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.
    if (UASManager::instance()->getUASForId(uas)->getWaypointManager() == currWPManager) {
    qDebug() << "UPDATING WP LIST";
    // Get current WP list
    // compare to local WP maps and
    // update / remove all WPs

//    int localCount = waypointsToIcons.count();

    // ORDER MATTERS HERE!
    // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE

    // 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())
    {
        // Get icon to work on
        mapcontrol::WayPointItem* icon = waypointsToIcons.value(wp);
        if (!wps.contains(wp))
        {
            waypointsToIcons.remove(wp);
            iconsToWaypoints.remove(icon);
            delete icon;
            icon = NULL;
        }
    }

    // Update all existing waypoints
    foreach (Waypoint* wp, waypointsToIcons.keys())
    {
        // Update remaining waypoints
        updateWaypoint(uas, wp);
    }

    // Update all potentially new waypoints
    foreach (Waypoint* wp, wps)
    {
        // Update / add only if new
        if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
    }

//    int globalCount = uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeCount();

//        // Get already existing waypoints
//        UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
//        if (uasInstance) {
//            // Get update rect of old content, this is what will be redrawn
//            // in the last step
//            QRect updateRect = waypointPath->boundingBox().toRect();

//            // Get all waypoints, including non-global waypoints
//            QVector<Waypoint*> wpList = uasInstance->getWaypointManager()->getWaypointList();

//            // Clear if necessary
//            if (wpList.count() == 0) {
//                clearWaypoints(uas);
//                return;
//            }

//            // Trim internal list to number of global waypoints in the waypoint manager list
//            int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeCount();
//            if (overSize > 0) {
//                // Remove n waypoints at the end of the list
//                // the remaining waypoints will be updated
//                // in the next step
//                for (int i = 0; i < overSize; ++i) {
//                    wps.removeLast();
//                    mc->layer("Waypoints")->removeGeometry(wpIcons.last());
//                    wpIcons.removeLast();
//                    waypointPath->points().removeLast();
//                }
lm's avatar
lm committed
415
//            }
416 417 418 419 420 421 422 423 424 425 426

//            // Load all existing waypoints into map view
//            foreach (Waypoint* wp, wpList) {
//                // Block map draw updates, since we update everything in the next step
//                // but update internal data structures.
//                // Please note that updateWaypoint() ignores non-global waypoints
//                updateWaypoint(mav->getUASID(), wp, false);
//            }

//            // Update view
//            if (isVisible()) mc->updateRequest(updateRect);
lm's avatar
lm committed
427
//        }
428
                        }
lm's avatar
lm committed
429
}