MissionController.cc 79.4 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9 10


11
#include "MissionCommandUIInfo.h"
12 13 14 15
#include "MissionController.h"
#include "MultiVehicleManager.h"
#include "MissionManager.h"
#include "CoordinateVector.h"
16
#include "FirmwarePlugin.h"
17
#include "QGCApplication.h"
18
#include "SimpleMissionItem.h"
19
#include "SurveyMissionItem.h"
20
#include "FixedWingLandingComplexItem.h"
21
#include "StructureScanComplexItem.h"
22
#include "StructureScanComplexItem.h"
23
#include "JsonHelper.h"
24
#include "ParameterManager.h"
25
#include "QGroundControlQmlGlobal.h"
26
#include "SettingsManager.h"
27
#include "AppSettings.h"
28
#include "MissionSettingsItem.h"
29
#include "QGCQGeoCoordinate.h"
30
#include "PlanMasterController.h"
31
#include "KML.h"
32

33
#ifndef __mobile__
34
#include "MainWindow.h"
35
#include "QGCQFileDialog.h"
36 37
#endif

38 39
QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog")

40
const char* MissionController::_settingsGroup =                 "MissionController";
Don Gagne's avatar
Don Gagne committed
41 42
const char* MissionController::_jsonFileTypeValue =             "Mission";
const char* MissionController::_jsonItemsKey =                  "items";
43
const char* MissionController::_jsonPlannedHomePositionKey =    "plannedHomePosition";
Don Gagne's avatar
Don Gagne committed
44
const char* MissionController::_jsonFirmwareTypeKey =           "firmwareType";
45 46 47
const char* MissionController::_jsonVehicleTypeKey =            "vehicleType";
const char* MissionController::_jsonCruiseSpeedKey =            "cruiseSpeed";
const char* MissionController::_jsonHoverSpeedKey =             "hoverSpeed";
Don Gagne's avatar
Don Gagne committed
48 49 50 51 52 53 54
const char* MissionController::_jsonParamsKey =                 "params";

// Deprecated V1 format keys
const char* MissionController::_jsonComplexItemsKey =           "complexItems";
const char* MissionController::_jsonMavAutopilotKey =           "MAV_AUTOPILOT";

const int   MissionController::_missionFileVersion =            2;
55

56 57 58
MissionController::MissionController(PlanMasterController* masterController, QObject *parent)
    : PlanElementController(masterController, parent)
    , _missionManager(_managerVehicle->missionManager())
59
    , _visualItems(NULL)
60
    , _settingsItem(NULL)
61
    , _firstItemsFromVehicle(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
62
    , _itemsRequested(false)
63 64
    , _surveyMissionItemName(tr("Survey"))
    , _fwLandingMissionItemName(tr("Fixed Wing Landing"))
65
    , _structureScanMissionItemName(tr("Structure Scan"))
66
    , _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings())
67
    , _progressPct(0)
68 69
    , _currentPlanViewIndex(-1)
    , _currentPlanViewItem(NULL)
70
{
71
    _resetMissionFlightStatus();
72
    managerVehicleChanged(_managerVehicle);
73 74 75 76
}

MissionController::~MissionController()
{
Don Gagne's avatar
Don Gagne committed
77

78 79
}

80 81 82 83 84 85 86 87 88
void MissionController::_resetMissionFlightStatus(void)
{
    _missionFlightStatus.totalDistance =        0.0;
    _missionFlightStatus.maxTelemetryDistance = 0.0;
    _missionFlightStatus.totalTime =            0.0;
    _missionFlightStatus.hoverTime =            0.0;
    _missionFlightStatus.cruiseTime =           0.0;
    _missionFlightStatus.hoverDistance =        0.0;
    _missionFlightStatus.cruiseDistance =       0.0;
89 90 91
    _missionFlightStatus.cruiseSpeed =          _controllerVehicle->defaultCruiseSpeed();
    _missionFlightStatus.hoverSpeed =           _controllerVehicle->defaultHoverSpeed();
    _missionFlightStatus.vehicleSpeed =         _controllerVehicle->multiRotor() || _managerVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed;
92
    _missionFlightStatus.vehicleYaw =           0.0;
93
    _missionFlightStatus.gimbalYaw =            std::numeric_limits<double>::quiet_NaN();
94
    _missionFlightStatus.gimbalPitch =          std::numeric_limits<double>::quiet_NaN();
95 96 97 98 99 100 101 102 103 104 105 106

    // Battery information

    _missionFlightStatus.mAhBattery =           0;
    _missionFlightStatus.hoverAmps =            0;
    _missionFlightStatus.cruiseAmps =           0;
    _missionFlightStatus.ampMinutesAvailable =  0;
    _missionFlightStatus.hoverAmpsTotal =       0;
    _missionFlightStatus.cruiseAmpsTotal =      0;
    _missionFlightStatus.batteryChangePoint =   -1;
    _missionFlightStatus.batteriesRequired =    -1;

107 108 109 110
    _controllerVehicle->firmwarePlugin()->batteryConsumptionData(_controllerVehicle, _missionFlightStatus.mAhBattery, _missionFlightStatus.hoverAmps, _missionFlightStatus.cruiseAmps);
    if (_missionFlightStatus.mAhBattery != 0) {
        double batteryPercentRemainingAnnounce = qgcApp()->toolbox()->settingsManager()->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toDouble();
        _missionFlightStatus.ampMinutesAvailable = (double)_missionFlightStatus.mAhBattery / 1000.0 * 60.0 * ((100.0 - batteryPercentRemainingAnnounce) / 100.0);
111
    }
112 113 114 115 116 117 118 119 120 121 122

    emit missionDistanceChanged(_missionFlightStatus.totalDistance);
    emit missionTimeChanged();
    emit missionHoverDistanceChanged(_missionFlightStatus.hoverDistance);
    emit missionCruiseDistanceChanged(_missionFlightStatus.cruiseDistance);
    emit missionHoverTimeChanged();
    emit missionCruiseTimeChanged();
    emit missionMaxTelemetryChanged(_missionFlightStatus.maxTelemetryDistance);
    emit batteryChangePointChanged(_missionFlightStatus.batteryChangePoint);
    emit batteriesRequiredChanged(_missionFlightStatus.batteriesRequired);

123 124
}

125 126
void MissionController::start(bool editMode)
{
127 128
    qCDebug(MissionControllerLog) << "start editMode" << editMode;

129
    PlanElementController::start(editMode);
130 131 132 133 134
    _init();
}

void MissionController::_init(void)
{
135
    // We start with an empty mission
136
    _visualItems = new QmlObjectListModel(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
137
    _addMissionSettings(_visualItems, false /* addToCenter */);
138
    _initAllVisualItems();
139 140
}

141
// Called when new mission items have completed downloading from Vehicle
142
void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllRequested)
143
{
144 145
    qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle";

DonLakeFlyer's avatar
DonLakeFlyer committed
146 147 148
    // Fly view always reloads on _loadComplete
    // Plan view only reloads on _loadComplete if specifically requested
    if (!_editMode || removeAllRequested || _itemsRequested) {
149
        // Fly Mode (accept if):
Don Gagne's avatar
Don Gagne committed
150
        //      - Always accepts new items from the vehicle so Fly view is kept up to date
151
        // Edit Mode (accept if):
152
        //      - Either a load from vehicle was manually requested or
Don Gagne's avatar
Don Gagne committed
153
        //      - The initial automatic load from a vehicle completed and the current editor is empty
154
        //      - Remove all way requested from Fly view (clear mission on flight end)
155

156
        QmlObjectListModel* newControllerMissionItems = new QmlObjectListModel(this);
157
        const QList<MissionItem*>& newMissionItems = _missionManager->missionItems();
158 159 160
        qCDebug(MissionControllerLog) << "loading from vehicle: count"<< newMissionItems.count();

        int i = 0;
161
        if (_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() && newMissionItems.count() != 0) {
162
            // First item is fake home position
DonLakeFlyer's avatar
DonLakeFlyer committed
163
            _addMissionSettings(newControllerMissionItems, false /* addToCenter */);
164
            MissionSettingsItem* settingsItem = newControllerMissionItems->value<MissionSettingsItem*>(0);
165 166 167 168
            if (!settingsItem) {
                qWarning() << "First item is not settings item";
                return;
            }
169 170 171 172
            MissionItem* fakeHomeItem = newMissionItems[0];
            if (fakeHomeItem->coordinate().latitude() != 0 || fakeHomeItem->coordinate().longitude() != 0) {
                settingsItem->setCoordinate(fakeHomeItem->coordinate());
            }
173 174
            i = 1;
        }
175

176 177
        for (; i<newMissionItems.count(); i++) {
            const MissionItem* missionItem = newMissionItems[i];
178
            newControllerMissionItems->append(new SimpleMissionItem(_controllerVehicle, _editMode, *missionItem, this));
179 180 181
        }

        _deinitAllVisualItems();
Don Gagne's avatar
Don Gagne committed
182
        _visualItems->deleteLater();
183
        _settingsItem = NULL;
184 185
        _visualItems = NULL;
        _updateContainsItems(); // This will clear containsItems which will be set again below. This will re-pop Start Mission confirmation.
186 187
        _visualItems = newControllerMissionItems;

188
        if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) {
DonLakeFlyer's avatar
DonLakeFlyer committed
189
            _addMissionSettings(_visualItems, _editMode && _visualItems->count() > 0 /* addToCenter */);
190 191
        }

192
        MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle);
193

194
        _initAllVisualItems();
195
        _updateContainsItems();
196
        emit newItemsFromVehicle();
197
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
198
    _itemsRequested = false;
199 200
}

201
void MissionController::loadFromVehicle(void)
202
{
DonLakeFlyer's avatar
DonLakeFlyer committed
203 204 205 206 207 208 209 210
    if (_masterController->offline()) {
        qCWarning(MissionControllerLog) << "MissionControllerLog::loadFromVehicle called while offline";
    } else if (syncInProgress()) {
        qCWarning(MissionControllerLog) << "MissionControllerLog::loadFromVehicle called while syncInProgress";
    } else {
        _itemsRequested = true;
        _managerVehicle->missionManager()->loadFromVehicle();
    }
211 212
}

213
void MissionController::sendToVehicle(void)
214
{
DonLakeFlyer's avatar
DonLakeFlyer committed
215 216 217 218 219
    if (_masterController->offline()) {
        qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while offline";
    } else if (syncInProgress()) {
        qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while syncInProgress";
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
220
        qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle";
DonLakeFlyer's avatar
DonLakeFlyer committed
221 222 223 224 225 226 227 228 229
        if (_visualItems->count() == 1) {
            // This prevents us from sending a possibly bogus home position to the vehicle
            QmlObjectListModel emptyModel;
            sendItemsToVehicle(_managerVehicle, &emptyModel);
        } else {
            sendItemsToVehicle(_managerVehicle, _visualItems);
        }
        setDirty(false);
    }
Don Gagne's avatar
Don Gagne committed
230 231
}

232 233 234 235 236
/// Converts from visual items to MissionItems
///     @param missionItemParent QObject parent for newly allocated MissionItems
/// @return true: Mission end action was added to end of list
bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
237 238 239 240
    if (visualMissionItems->count() == 0) {
        return false;
    }

241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256
    bool endActionSet = false;
    int lastSeqNum = 0;

    for (int i=0; i<visualMissionItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(visualMissionItems->get(i));

        lastSeqNum = visualItem->lastSequenceNumber();
        visualItem->appendMissionItems(rgMissionItems, missionItemParent);

        qCDebug(MissionControllerLog) << "_convertToMissionItems seqNum:lastSeqNum:command"
                                      << visualItem->sequenceNumber()
                                      << lastSeqNum
                                      << visualItem->commandName();
    }

    // Mission settings has a special case for end mission action
257
    MissionSettingsItem* settingsItem = visualMissionItems->value<MissionSettingsItem*>(0);
258 259 260 261 262 263 264
    if (settingsItem) {
        endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent);
    }

    return endActionSet;
}

265 266 267 268
void MissionController::convertToKMLDocument(QDomDocument& document)
{
    QJsonObject missionJson;
    QmlObjectListModel* visualItems = new QmlObjectListModel();
269
    QList<MissionItem*> missionItems;
270 271 272
    QString error;
    save(missionJson);
    _loadItemsFromJson(missionJson, visualItems, error);
273 274 275 276 277
    _convertToMissionItems(visualItems, missionItems, this);

    if (missionItems.count() == 0) {
        return;
    }
278 279 280 281 282 283 284

    float altitude = missionJson[_jsonPlannedHomePositionKey].toArray()[2].toDouble();

    QString coord;
    QStringList coords;
    // Drop home position
    bool dropPoint = true;
285
    for(const auto& item : missionItems) {
286 287 288 289 290
        if(dropPoint) {
            dropPoint = false;
            continue;
        }
        const MissionCommandUIInfo* uiInfo = \
291
                qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command());
292 293

        if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) {
yaoling's avatar
yaoling committed
294
            coord = QString::number(item->param6(),'f',7) \
295 296 297 298
                    + "," \
                    + QString::number(item->param5(),'f',7) \
                    + "," \
                    + QString::number(item->param7() + altitude,'f',2);
299 300 301 302 303 304 305 306
            coords.append(coord);
        }
    }
    Kml kml;
    kml.points(coords);
    kml.save(document);
}

Don Gagne's avatar
Don Gagne committed
307 308 309
void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems)
{
    if (vehicle) {
310
        QList<MissionItem*> rgMissionItems;
311

312
        _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle);
313

314
        // PlanManager takes control of MissionItems so no need to delete
315
        vehicle->missionManager()->writeMissionItems(rgMissionItems);
316 317
    }
}
318

319 320 321 322 323 324
int MissionController::_nextSequenceNumber(void)
{
    if (_visualItems->count() == 0) {
        qWarning() << "Internal error: Empty visual item list";
        return 0;
    } else {
325 326
        VisualMissionItem* lastItem = _visualItems->value<VisualMissionItem*>(_visualItems->count() - 1);
        return lastItem->lastSequenceNumber() + 1;
327 328 329
    }
}

330
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
331
{
332
    int sequenceNumber = _nextSequenceNumber();
333
    SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, this);
334
    newItem->setSequenceNumber(sequenceNumber);
335
    newItem->setCoordinate(coordinate);
336 337 338
    newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
    _initVisualItem(newItem);
    if (_visualItems->count() == 1) {
339 340 341 342
        MavlinkQmlSingleton::Qml_MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF : MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF;
        if (_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains((MAV_CMD)takeoffCmd)) {
            newItem->setCommand(takeoffCmd);
        }
343
    }
344
    newItem->setDefaultsForCommand();
Don Gagne's avatar
Don Gagne committed
345
    if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) {
346 347
        double      prevAltitude;
        MAV_FRAME   prevFrame;
348

349 350 351
        if (_findPreviousAltitude(i, &prevAltitude, &prevFrame)) {
            newItem->missionItem().setFrame(prevFrame);
            newItem->missionItem().setParam7(prevAltitude);
352
        }
353
    }
354
    newItem->setMissionFlightStatus(_missionFlightStatus);
355
    _visualItems->insert(i, newItem);
356 357 358

    _recalcAll();

359
    return newItem->sequenceNumber();
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
int MissionController::insertROIMissionItem(QGeoCoordinate coordinate, int i)
{
    int sequenceNumber = _nextSequenceNumber();
    SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, this);
    newItem->setSequenceNumber(sequenceNumber);
    newItem->setCoordinate(coordinate);
    newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_DO_SET_ROI);
    _initVisualItem(newItem);
    newItem->setDefaultsForCommand();

    double      prevAltitude;
    MAV_FRAME   prevFrame;

    if (_findPreviousAltitude(i, &prevAltitude, &prevFrame)) {
        newItem->missionItem().setFrame(prevFrame);
        newItem->missionItem().setParam7(prevAltitude);
    }
    _visualItems->insert(i, newItem);

    _recalcAll();

    return newItem->sequenceNumber();
}

386
int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i)
387
{
388 389
    ComplexMissionItem* newItem;

390
    int sequenceNumber = _nextSequenceNumber();
391
    if (itemName == _surveyMissionItemName) {
392
        newItem = new SurveyMissionItem(_controllerVehicle, _visualItems);
393
        newItem->setCoordinate(mapCenterCoordinate);
394 395 396 397
        // If the vehicle is known to have a gimbal then we automatically point the gimbal straight down if not already set
        bool rollSupported = false;
        bool pitchSupported = false;
        bool yawSupported = false;
398 399 400
        MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
        CameraSection* cameraSection = settingsItem->cameraSection();
        // Set camera to photo mode (leave alone if user already specified)
401
        if (cameraSection->cameraModeSupported() && !cameraSection->specifyCameraMode()) {
402
            cameraSection->setSpecifyCameraMode(true);
403
            cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE_SURVEY);
404 405
        }
        // Point gimbal straight down
406
        if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
407 408 409
            // If the user already specified a gimbal angle leave it alone
            if (!cameraSection->specifyGimbal()) {
                cameraSection->setSpecifyGimbal(true);
410
                cameraSection->gimbalPitch()->setRawValue(-90.0);
411 412
            }
        }
413
    } else if (itemName == _fwLandingMissionItemName) {
414
        newItem = new FixedWingLandingComplexItem(_controllerVehicle, _visualItems);
415 416
    } else if (itemName == _structureScanMissionItemName) {
        newItem = new StructureScanComplexItem(_controllerVehicle, _visualItems);
417 418 419 420
    } else {
        qWarning() << "Internal error: Unknown complex item:" << itemName;
        return sequenceNumber;
    }
421
    newItem->setSequenceNumber(sequenceNumber);
422
    _initVisualItem(newItem);
423

424
    _visualItems->insert(i, newItem);
425 426 427

    _recalcAll();

428
    return newItem->sequenceNumber();
429 430
}

431 432
void MissionController::removeMissionItem(int index)
{
433 434 435 436 437 438
    if (index <= 0 || index >= _visualItems->count()) {
        qWarning() << "MissionController::removeMissionItem called with bad index - count:index" << _visualItems->count() << index;
        return;
    }

    bool surveyRemoved = _visualItems->value<SurveyMissionItem*>(index);
439
    VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));
440

441
    _deinitVisualItem(item);
442
    item->deleteLater();
443

444 445 446 447 448 449 450 451 452 453
    if (surveyRemoved) {
        // Determine if the mission still has another survey in it
        bool foundSurvey = false;
        for (int i=1; i<_visualItems->count(); i++) {
            if (_visualItems->value<SurveyMissionItem*>(i)) {
                foundSurvey = true;
                break;
            }
        }

454
        // If there is no longer a survey item in the mission remove added commands
455 456 457 458
        if (!foundSurvey) {
            bool rollSupported = false;
            bool pitchSupported = false;
            bool yawSupported = false;
459 460
            MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
            CameraSection* cameraSection = settingsItem->cameraSection();
461
            if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
462
                if (cameraSection->specifyGimbal() && cameraSection->gimbalPitch()->rawValue().toDouble() == -90.0 && cameraSection->gimbalYaw()->rawValue().toDouble() == 0.0) {
463 464 465
                    cameraSection->setSpecifyGimbal(false);
                }
            }
466
            if (cameraSection->cameraModeSupported() && cameraSection->specifyCameraMode() && cameraSection->cameraMode()->rawValue().toInt() == 0) {
467 468
                cameraSection->setSpecifyCameraMode(false);
            }
469 470 471
        }
    }

472
    _recalcAll();
473
    setDirty(true);
474 475
}

476
void MissionController::removeAll(void)
477
{
478 479
    if (_visualItems) {
        _deinitAllVisualItems();
Don Gagne's avatar
Don Gagne committed
480
        _visualItems->deleteLater();
481
        _settingsItem = NULL;
482
        _visualItems = new QmlObjectListModel(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
483
        _addMissionSettings(_visualItems, false /* addToCenter */);
484
        _initAllVisualItems();
485
        setDirty(true);
486
        _resetMissionFlightStatus();
Don Gagne's avatar
Don Gagne committed
487 488 489
    }
}

490
bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
491 492 493 494 495 496 497 498 499
{
    // Validate root object keys
    QList<JsonHelper::KeyValidateInfo> rootKeyInfoList = {
        { _jsonPlannedHomePositionKey,      QJsonValue::Object, true },
        { _jsonItemsKey,                    QJsonValue::Array,  true },
        { _jsonMavAutopilotKey,             QJsonValue::Double, true },
        { _jsonComplexItemsKey,             QJsonValue::Array,  true },
    };
    if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) {
500 501 502
        return false;
    }

503
    // Read complex items
504
    QList<SurveyMissionItem*> surveyItems;
505 506 507 508
    QJsonArray complexArray(json[_jsonComplexItemsKey].toArray());
    qCDebug(MissionControllerLog) << "Json load: complex item count" << complexArray.count();
    for (int i=0; i<complexArray.count(); i++) {
        const QJsonValue& itemValue = complexArray[i];
509

510 511 512 513 514
        if (!itemValue.isObject()) {
            errorString = QStringLiteral("Mission item is not an object");
            return false;
        }

515
        SurveyMissionItem* item = new SurveyMissionItem(_controllerVehicle, visualItems);
Don Gagne's avatar
Don Gagne committed
516 517
        const QJsonObject itemObject = itemValue.toObject();
        if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
518
            surveyItems.append(item);
519 520
        } else {
            return false;
521
        }
522
    }
523

524 525 526 527 528
    // Read simple items, interspersing complex items into the full list

    int nextSimpleItemIndex= 0;
    int nextComplexItemIndex= 0;
    int nextSequenceNumber = 1; // Start with 1 since home is in 0
Don Gagne's avatar
Don Gagne committed
529
    QJsonArray itemArray(json[_jsonItemsKey].toArray());
530

531
    qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << surveyItems.count();
532 533 534 535
    do {
        qCDebug(MissionControllerLog) << "Json load: simple item loop nextSimpleItemIndex:nextComplexItemIndex:nextSequenceNumber" << nextSimpleItemIndex << nextComplexItemIndex << nextSequenceNumber;

        // If there is a complex item that should be next in sequence add it in
536 537
        if (nextComplexItemIndex < surveyItems.count()) {
            SurveyMissionItem* complexItem = surveyItems[nextComplexItemIndex];
538 539 540 541 542 543 544 545 546 547 548 549 550 551

            if (complexItem->sequenceNumber() == nextSequenceNumber) {
                qCDebug(MissionControllerLog) << "Json load: injecting complex item expectedSequence:actualSequence:" << nextSequenceNumber << complexItem->sequenceNumber();
                visualItems->append(complexItem);
                nextSequenceNumber = complexItem->lastSequenceNumber() + 1;
                nextComplexItemIndex++;
                continue;
            }
        }

        // Add the next available simple item
        if (nextSimpleItemIndex < itemArray.count()) {
            const QJsonValue& itemValue = itemArray[nextSimpleItemIndex++];

552 553 554 555 556
            if (!itemValue.isObject()) {
                errorString = QStringLiteral("Mission item is not an object");
                return false;
            }

Don Gagne's avatar
Don Gagne committed
557
            const QJsonObject itemObject = itemValue.toObject();
558
            SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems);
Don Gagne's avatar
Don Gagne committed
559
            if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
560
                qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber();
561
                nextSequenceNumber = item->lastSequenceNumber() + 1;
562
                visualItems->append(item);
563 564 565 566
            } else {
                return false;
            }
        }
567
    } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < surveyItems.count());
568 569

    if (json.contains(_jsonPlannedHomePositionKey)) {
570
        SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems);
571

Don Gagne's avatar
Don Gagne committed
572
        if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) {
573
            MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems);
574 575 576
            settingsItem->setCoordinate(item->coordinate());
            visualItems->insert(0, settingsItem);
            item->deleteLater();
577 578 579 580
        } else {
            return false;
        }
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
581
        _addMissionSettings(visualItems, true /* addToCenter */);
582 583 584 585 586
    }

    return true;
}

587
bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
588 589 590 591 592 593
{
    // Validate root object keys
    QList<JsonHelper::KeyValidateInfo> rootKeyInfoList = {
        { _jsonPlannedHomePositionKey,      QJsonValue::Array,  true },
        { _jsonItemsKey,                    QJsonValue::Array,  true },
        { _jsonFirmwareTypeKey,             QJsonValue::Double, true },
594
        { _jsonVehicleTypeKey,              QJsonValue::Double, false },
595 596
        { _jsonCruiseSpeedKey,              QJsonValue::Double, false },
        { _jsonHoverSpeedKey,               QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
597 598 599 600 601 602 603
    };
    if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) {
        return false;
    }

    qCDebug(MissionControllerLog) << "MissionController::_loadJsonMissionFileV2 itemCount:" << json[_jsonItemsKey].toArray().count();

604
    // Mission Settings
605
    AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings();
606

607 608
    if (_masterController->offline()) {
        // We only update if offline since if we are online we use the online vehicle settings
609 610 611 612
        appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType((MAV_AUTOPILOT)json[_jsonFirmwareTypeKey].toInt()));
        if (json.contains(_jsonVehicleTypeKey)) {
            appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType((MAV_TYPE)json[_jsonVehicleTypeKey].toInt()));
        }
613
    }
614
    if (json.contains(_jsonCruiseSpeedKey)) {
615
        appSettings->offlineEditingCruiseSpeed()->setRawValue(json[_jsonCruiseSpeedKey].toDouble());
616 617
    }
    if (json.contains(_jsonHoverSpeedKey)) {
618
        appSettings->offlineEditingHoverSpeed()->setRawValue(json[_jsonHoverSpeedKey].toDouble());
619 620
    }

621 622 623 624 625
    QGeoCoordinate homeCoordinate;
    if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) {
        return false;
    }
    MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems);
626 627
    settingsItem->setCoordinate(homeCoordinate);
    visualItems->insert(0, settingsItem);
Don Gagne's avatar
Don Gagne committed
628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653
    qCDebug(MissionControllerLog) << "plannedHomePosition" << homeCoordinate;

    // Read mission items

    int nextSequenceNumber = 1; // Start with 1 since home is in 0
    const QJsonArray rgMissionItems(json[_jsonItemsKey].toArray());
    for (int i=0; i<rgMissionItems.count(); i++) {
        // Convert to QJsonObject
        const QJsonValue& itemValue = rgMissionItems[i];
        if (!itemValue.isObject()) {
            errorString = tr("Mission item %1 is not an object").arg(i);
            return false;
        }
        const QJsonObject itemObject = itemValue.toObject();

        // Load item based on type

        QList<JsonHelper::KeyValidateInfo> itemKeyInfoList = {
            { VisualMissionItem::jsonTypeKey,  QJsonValue::String, true },
        };
        if (!JsonHelper::validateKeys(itemObject, itemKeyInfoList, errorString)) {
            return false;
        }
        QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString();

        if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) {
654
            SimpleMissionItem* simpleItem = new SimpleMissionItem(_controllerVehicle, visualItems);
655
            if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) {
656
                qCDebug(MissionControllerLog) << "Loading simple item: nextSequenceNumber:command" << nextSequenceNumber << simpleItem->command();
657
                nextSequenceNumber = simpleItem->lastSequenceNumber() + 1;
Don Gagne's avatar
Don Gagne committed
658 659 660 661 662 663 664 665 666 667 668 669 670 671 672
                visualItems->append(simpleItem);
            } else {
                return false;
            }
        } else if (itemType == VisualMissionItem::jsonTypeComplexItemValue) {
            QList<JsonHelper::KeyValidateInfo> complexItemKeyInfoList = {
                { ComplexMissionItem::jsonComplexItemTypeKey,  QJsonValue::String, true },
            };
            if (!JsonHelper::validateKeys(itemObject, complexItemKeyInfoList, errorString)) {
                return false;
            }
            QString complexItemType = itemObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();

            if (complexItemType == SurveyMissionItem::jsonComplexItemTypeValue) {
                qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber;
673
                SurveyMissionItem* surveyItem = new SurveyMissionItem(_controllerVehicle, visualItems);
Don Gagne's avatar
Don Gagne committed
674 675 676 677 678 679
                if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = surveyItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Survey load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(surveyItem);
680
            } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) {
DonLakeFlyer's avatar
DonLakeFlyer committed
681
                qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber;
682
                FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_controllerVehicle, visualItems);
DonLakeFlyer's avatar
DonLakeFlyer committed
683 684 685 686 687 688
                if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = landingItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "FW Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(landingItem);
689 690 691 692 693 694 695 696 697
            } else if (complexItemType == StructureScanComplexItem::jsonComplexItemTypeValue) {
                qCDebug(MissionControllerLog) << "Loading Structure Scan: nextSequenceNumber" << nextSequenceNumber;
                StructureScanComplexItem* structureItem = new StructureScanComplexItem(_controllerVehicle, visualItems);
                if (!structureItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = structureItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Structure Scan load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(structureItem);
698
            } else if (complexItemType == MissionSettingsItem::jsonComplexItemTypeValue) {
DonLakeFlyer's avatar
DonLakeFlyer committed
699
                qCDebug(MissionControllerLog) << "Loading Mission Settings: nextSequenceNumber" << nextSequenceNumber;
700
                MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, visualItems);
DonLakeFlyer's avatar
DonLakeFlyer committed
701 702 703 704 705 706
                if (!settingsItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = settingsItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Mission Settings load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(settingsItem);
Don Gagne's avatar
Don Gagne committed
707 708 709 710 711 712 713 714 715 716 717 718 719
            } else {
                errorString = tr("Unsupported complex item type: %1").arg(complexItemType);
            }
        } else {
            errorString = tr("Unknown item type: %1").arg(itemType);
            return false;
        }
    }

    // Fix up the DO_JUMP commands jump sequence number by finding the item with the matching doJumpId
    for (int i=0; i<visualItems->count(); i++) {
        if (visualItems->value<VisualMissionItem*>(i)->isSimpleItem()) {
            SimpleMissionItem* doJumpItem = visualItems->value<SimpleMissionItem*>(i);
Don Gagne's avatar
Don Gagne committed
720
            if ((MAV_CMD)doJumpItem->command() == MAV_CMD_DO_JUMP) {
Don Gagne's avatar
Don Gagne committed
721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743
                bool found = false;
                int findDoJumpId = doJumpItem->missionItem().param1();
                for (int j=0; j<visualItems->count(); j++) {
                    if (visualItems->value<VisualMissionItem*>(j)->isSimpleItem()) {
                        SimpleMissionItem* targetItem = visualItems->value<SimpleMissionItem*>(j);
                        if (targetItem->missionItem().doJumpId() == findDoJumpId) {
                            doJumpItem->missionItem().setParam1(targetItem->sequenceNumber());
                            found = true;
                            break;
                        }
                    }
                }
                if (!found) {
                    errorString = tr("Could not find doJumpId: %1").arg(findDoJumpId);
                    return false;
                }
            }
        }
    }

    return true;
}

744 745 746 747 748 749 750 751
bool MissionController::_loadItemsFromJson(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
{
    // V1 file format has no file type key and version key is string. Convert to new format.
    if (!json.contains(JsonHelper::jsonFileTypeKey)) {
        json[JsonHelper::jsonFileTypeKey] = _jsonFileTypeValue;
    }

    int fileVersion;
752 753 754 755 756 757
    JsonHelper::validateQGCJsonFile(json,
                                    _jsonFileTypeValue,    // expected file type
                                    1,                     // minimum supported version
                                    2,                     // maximum supported version
                                    fileVersion,
                                    errorString);
758 759

    if (fileVersion == 1) {
760
        return _loadJsonMissionFileV1(json, visualItems, errorString);
761
    } else {
762
        return _loadJsonMissionFileV2(json, visualItems, errorString);
763 764 765
    }
}

766
bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString)
767
{
768 769
    bool firstItem = true;
    bool plannedHomePositionInFile = false;
770 771 772 773 774 775 776 777 778

    QString firstLine = stream.readLine();
    const QStringList& version = firstLine.split(" ");

    bool versionOk = false;
    if (version.size() == 3 && version[0] == "QGC" && version[1] == "WPL") {
        if (version[2] == "110") {
            // ArduPilot file, planned home position is already in position 0
            versionOk = true;
779
            plannedHomePositionInFile = true;
780 781 782
        } else if (version[2] == "120") {
            // Old QGC file, no planned home position
            versionOk = true;
783
            plannedHomePositionInFile = false;
784 785 786 787
        }
    }

    if (versionOk) {
788
        // Start with planned home in center
DonLakeFlyer's avatar
DonLakeFlyer committed
789
        _addMissionSettings(visualItems, true /* addToCenter */);
790 791
        MissionSettingsItem* settingsItem = visualItems->value<MissionSettingsItem*>(0);

792
        while (!stream.atEnd()) {
793
            SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, visualItems);
794 795

            if (item->load(stream)) {
796 797 798 799 800 801
                if (firstItem && plannedHomePositionInFile) {
                    settingsItem->setCoordinate(item->coordinate());
                } else {
                    visualItems->append(item);
                }
                firstItem = false;
802
            } else {
803
                errorString = tr("The mission file is corrupted.");
804 805 806 807
                return false;
            }
        }
    } else {
808
        errorString = tr("The mission file is not compatible with this version of %1.").arg(qgcApp()->applicationName());
809 810 811
        return false;
    }

812
    if (!plannedHomePositionInFile) {
813 814 815 816 817
        // Update sequence numbers in DO_JUMP commands to take into account added home position in index 0
        for (int i=1; i<visualItems->count(); i++) {
            SimpleMissionItem* item = qobject_cast<SimpleMissionItem*>(visualItems->get(i));
            if (item && item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) {
                item->missionItem().setParam1((int)item->missionItem().param1() + 1);
Don Gagne's avatar
Don Gagne committed
818 819
            }
        }
820 821 822
    }

    return true;
823 824
}

825
void MissionController::_initLoadedVisualItems(QmlObjectListModel* loadedVisualItems)
826
{
Don Gagne's avatar
Don Gagne committed
827 828 829
    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteLater();
830
        _settingsItem = NULL;
Don Gagne's avatar
Don Gagne committed
831 832
    }

833
    _visualItems = loadedVisualItems;
Don Gagne's avatar
Don Gagne committed
834 835

    if (_visualItems->count() == 0) {
DonLakeFlyer's avatar
DonLakeFlyer committed
836
        _addMissionSettings(_visualItems, true /* addToCenter */);
Don Gagne's avatar
Don Gagne committed
837 838
    }

839
    MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle);
840

Don Gagne's avatar
Don Gagne committed
841
    _initAllVisualItems();
842
}
843

844 845 846 847 848 849
bool MissionController::load(const QJsonObject& json, QString& errorString)
{
    QString errorStr;
    QString errorMessage = tr("Mission: %1");
    QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this);

850
    if (!_loadJsonMissionFileV2(json, loadedVisualItems, errorStr)) {
851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875
        errorString = errorMessage.arg(errorStr);
        return false;
    }
    _initLoadedVisualItems(loadedVisualItems);

    return true;
}

bool MissionController::loadJsonFile(QFile& file, QString& errorString)
{
    QString         errorStr;
    QString         errorMessage = tr("Mission: %1");
    QJsonDocument   jsonDoc;
    QByteArray      bytes = file.readAll();

    if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorStr)) {
        errorString = errorMessage.arg(errorStr);
        return false;
    }

    QJsonObject json = jsonDoc.object();
    QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this);
    if (!_loadItemsFromJson(json, loadedVisualItems, errorStr)) {
        errorString = errorMessage.arg(errorStr);
        return false;
876
    }
877

878 879 880 881 882 883 884 885 886 887 888 889 890
    _initLoadedVisualItems(loadedVisualItems);

    return true;
}

bool MissionController::loadTextFile(QFile& file, QString& errorString)
{
    QString     errorStr;
    QString     errorMessage = tr("Mission: %1");
    QByteArray  bytes = file.readAll();
    QTextStream stream(bytes);

    QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this);
891
    if (!_loadTextMissionFile(stream, loadedVisualItems, errorStr)) {
892 893 894 895 896 897
        errorString = errorMessage.arg(errorStr);
        return false;
    }

    _initLoadedVisualItems(loadedVisualItems);

Don Gagne's avatar
Don Gagne committed
898
    return true;
899 900
}

901
void MissionController::save(QJsonObject& json)
902
{
903
    json[JsonHelper::jsonVersionKey] = _missionFileVersion;
904

905
    // Mission settings
906

907 908 909 910 911 912 913 914
    MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
    if (!settingsItem) {
        qWarning() << "First item is not MissionSettingsItem";
        return;
    }
    QJsonValue coordinateValue;
    JsonHelper::saveGeoCoordinate(settingsItem->coordinate(), true /* writeAltitude */, coordinateValue);
    json[_jsonPlannedHomePositionKey] = coordinateValue;
915 916 917 918
    json[_jsonFirmwareTypeKey] = _controllerVehicle->firmwareType();
    json[_jsonVehicleTypeKey] = _controllerVehicle->vehicleType();
    json[_jsonCruiseSpeedKey] = _controllerVehicle->defaultCruiseSpeed();
    json[_jsonHoverSpeedKey] = _controllerVehicle->defaultHoverSpeed();
919

920
    // Save the visual items
921

922 923 924
    QJsonArray rgJsonMissionItems;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
925

926 927
        visualItem->save(rgJsonMissionItems);
    }
928

929 930 931
    // Mission settings has a special case for end mission action
    if (settingsItem) {
        QList<MissionItem*> rgMissionItems;
932

933 934 935 936 937
        if (_convertToMissionItems(_visualItems, rgMissionItems, this /* missionItemParent */)) {
            QJsonObject saveObject;
            MissionItem* missionItem = rgMissionItems[rgMissionItems.count() - 1];
            missionItem->save(saveObject);
            rgJsonMissionItems.append(saveObject);
938
        }
939 940
        for (int i=0; i<rgMissionItems.count(); i++) {
            rgMissionItems[i]->deleteLater();
941 942 943
        }
    }

944
    json[_jsonItemsKey] = rgJsonMissionItems;
945 946
}

947
void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
948
{
Don Gagne's avatar
Don Gagne committed
949
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
950
    QGeoCoordinate  prevCoord =     prevItem->exitCoordinate();
951 952 953 954
    bool            distanceOk =    false;

    // Convert to fixed altitudes

955
    distanceOk = true;
956
    if (currentItem != _settingsItem && currentItem->coordinateHasRelativeAltitude()) {
957 958
        currentCoord.setAltitude(homeAlt + currentCoord.altitude());
    }
959
    if (prevItem != _settingsItem && prevItem->exitCoordinateHasRelativeAltitude()) {
960
        prevCoord.setAltitude(homeAlt + prevCoord.altitude());
961 962 963
    }

    if (distanceOk) {
Don Gagne's avatar
Don Gagne committed
964 965 966
        *altDifference = currentCoord.altitude() - prevCoord.altitude();
        *distance = prevCoord.distanceTo(currentCoord);
        *azimuth = prevCoord.azimuthTo(currentCoord);
967
    } else {
Don Gagne's avatar
Don Gagne committed
968
        *altDifference = 0.0;
969
        *azimuth = 0.0;
970
        *distance = 0.0;
971 972 973
    }
}

974
double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, VisualMissionItem* homeItem)
975 976 977 978 979 980 981
{
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
    QGeoCoordinate  homeCoord =     homeItem->exitCoordinate();
    bool            distanceOk =    false;

    distanceOk = true;

982
    return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0;
983 984
}

985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006
void MissionController::_addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair)
{
    if (prevItemPairHashTable.contains(pair)) {
        // Pair already exists and connected, just re-use
        _linesTable[pair] = prevItemPairHashTable.take(pair);
    } else {
        // Create a new segment and wire update notifiers
        auto linevect       = new CoordinateVector(pair.first->isSimpleItem() ? pair.first->coordinate() : pair.first->exitCoordinate(), pair.second->coordinate(), this);
        auto originNotifier = pair.first->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged;
        auto endNotifier    = &VisualMissionItem::coordinateChanged;

        // Use signals/slots to update the coordinate endpoints
        connect(pair.first,     originNotifier, linevect, &CoordinateVector::setCoordinate1);
        connect(pair.second,    endNotifier,    linevect, &CoordinateVector::setCoordinate2);

        // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change
        // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes
        connect(pair.second, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcMissionFlightStatus);
        _linesTable[pair] = linevect;
    }
}

1007 1008
void MissionController::_recalcWaypointLines(void)
{
1009 1010 1011
    bool                firstCoordinateItem =   true;
    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));

1012
    bool homePositionValid = _settingsItem->coordinate().isValid();
1013

1014
    qCDebug(MissionControllerLog) << "_recalcWaypointLines homePositionValid" << homePositionValid;
1015

Nate Weibley's avatar
Nate Weibley committed
1016 1017
    CoordVectHashTable old_table = _linesTable;
    _linesTable.clear();
1018
    _waypointLines.clear();
1019
    _waypointPath.clear();
1020

1021 1022 1023 1024 1025 1026 1027 1028 1029
    bool linkEndToHome;
    SimpleMissionItem* lastItem = _visualItems->value<SimpleMissionItem*>(_visualItems->count() - 1);
    if (lastItem && (int)lastItem->command() == MAV_CMD_NAV_RETURN_TO_LAUNCH) {
        linkEndToHome = true;
    } else {
        linkEndToHome = _settingsItem->missionEndRTL();
    }

    bool linkStartToHome = false;
1030 1031 1032
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

1033 1034 1035 1036 1037 1038
        // If we still haven't found the first coordinate item and we hit a takeoff command this means the mission starts from the ground.
        // Link the first item back to home to show that.
        if (firstCoordinateItem && item->isSimpleItem()) {
            MAV_CMD command = (MAV_CMD)qobject_cast<SimpleMissionItem*>(item)->command();
            if (command == MAV_CMD_NAV_TAKEOFF || command == MAV_CMD_NAV_VTOL_TAKEOFF) {
                linkStartToHome = true;
1039
            }
1040 1041
        }

1042 1043 1044 1045 1046 1047
        if (item->specifiesCoordinate() && !item->isStandaloneCoordinate()) {
            firstCoordinateItem = false;
            if (lastCoordinateItem != _settingsItem || (homePositionValid && linkStartToHome)) {
                if (_editMode) {
                    VisualItemPair pair(lastCoordinateItem, item);
                    _addWaypointLineSegment(old_table, pair);
1048 1049
                }
            }
1050 1051
            _waypointPath.append(QVariant::fromValue(item->coordinate()));
            lastCoordinateItem = item;
1052 1053
        }
    }
1054 1055 1056 1057 1058 1059

    if (linkStartToHome && homePositionValid) {
        _waypointPath.prepend(QVariant::fromValue(_settingsItem->coordinate()));
    }

    if (linkEndToHome && lastCoordinateItem != _settingsItem && homePositionValid) {
1060 1061 1062 1063 1064 1065
        if (_editMode) {
            VisualItemPair pair(lastCoordinateItem, _settingsItem);
            _addWaypointLineSegment(old_table, pair);
        } else {
            _waypointPath.append(QVariant::fromValue(_settingsItem->coordinate()));
        }
1066
    }
1067 1068 1069 1070

    {
        // Create a temporary QObjectList and replace the model data
        QObjectList objs;
Nate Weibley's avatar
Nate Weibley committed
1071 1072
        objs.reserve(_linesTable.count());
        foreach(CoordinateVector *vect, _linesTable.values()) {
1073 1074 1075 1076 1077 1078 1079 1080 1081 1082
            objs.append(vect);
        }

        // We don't delete here because many links may still be valid
        _waypointLines.swapObjectList(objs);
    }

    // Anything left in the old table is an obsolete line object that can go
    qDeleteAll(old_table);

DonLakeFlyer's avatar
DonLakeFlyer committed
1083
    _recalcMissionFlightStatus();
1084

1085 1086 1087 1088 1089 1090 1091 1092
    if (_waypointPath.count() == 0) {
        // MapPolyLine has a bug where if you can from a path which has elements to an empty path the line drawn
        // is not cleared from the map. This hack works around that since it causes the previous lines to be remove
        // as then doesn't draw anything on the map.
        _waypointPath.append(QVariant::fromValue(QGeoCoordinate(0, 0)));
        _waypointPath.append(QVariant::fromValue(QGeoCoordinate(0, 0)));
    }

1093
    emit waypointLinesChanged();
1094
    emit waypointPathChanged();
1095 1096
}

1097 1098 1099 1100 1101 1102
void MissionController::_updateBatteryInfo(int waypointIndex)
{
    if (_missionFlightStatus.mAhBattery != 0) {
        _missionFlightStatus.hoverAmpsTotal = (_missionFlightStatus.hoverTime / 60.0) * _missionFlightStatus.hoverAmps;
        _missionFlightStatus.cruiseAmpsTotal = (_missionFlightStatus.cruiseTime / 60.0) * _missionFlightStatus.cruiseAmps;
        _missionFlightStatus.batteriesRequired = ceil((_missionFlightStatus.hoverAmpsTotal + _missionFlightStatus.cruiseAmpsTotal) / _missionFlightStatus.ampMinutesAvailable);
1103 1104 1105
        // FIXME: Battery change point code pretty much doesn't work. The reason is that is treats complex items as a black box. It needs to be able to look
        // inside complex items in order to determine a swap point that is interior to a complex item. Current the swap point display in PlanToolbar is
        // disabled to do this problem.
1106
        if (waypointIndex != -1 && _missionFlightStatus.batteriesRequired == 2 && _missionFlightStatus.batteryChangePoint == -1) {
1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129
            _missionFlightStatus.batteryChangePoint = waypointIndex - 1;
        }
    }
}

void MissionController::_addHoverTime(double hoverTime, double hoverDistance, int waypointIndex)
{
    _missionFlightStatus.totalTime += hoverTime;
    _missionFlightStatus.hoverTime += hoverTime;
    _missionFlightStatus.hoverDistance += hoverDistance;
    _missionFlightStatus.totalDistance += hoverDistance;
    _updateBatteryInfo(waypointIndex);
}

void MissionController::_addCruiseTime(double cruiseTime, double cruiseDistance, int waypointIndex)
{
    _missionFlightStatus.totalTime += cruiseTime;
    _missionFlightStatus.cruiseTime += cruiseTime;
    _missionFlightStatus.cruiseDistance += cruiseDistance;
    _missionFlightStatus.totalDistance += cruiseDistance;
    _updateBatteryInfo(waypointIndex);
}

1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176
/// Adds additional time to a mission as specified by the command
void MissionController::_addCommandTimeDelay(SimpleMissionItem* simpleItem, bool vtolInHover)
{
    double seconds = 0;

    if (!simpleItem) {
        return;
    }

    // This routine is currently quite minimal and only handles the simple cases.
    switch ((int)simpleItem->command()) {
    case MAV_CMD_NAV_WAYPOINT:
    case MAV_CMD_CONDITION_DELAY:
        seconds = simpleItem->missionItem().param1();
        break;
    }

    _addTimeDistance(vtolInHover, 0, 0, seconds, 0, -1);
}

/// Adds the specified time to the appropriate hover or cruise time values.
///     @param vtolInHover true: vtol is currrent in hover mode
///     @param hoverTime    Amount of time tp add to hover
///     @param cruiseTime   Amount of time to add to cruise
///     @param extraTime    Amount of additional time to add to hover/cruise
///     @param seqNum       Sequence number of waypoint for these values, -1 for no waypoint associated
void MissionController::_addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum)
{
    if (_controllerVehicle->vtol()) {
        if (vtolInHover) {
            _addHoverTime(hoverTime, distance, seqNum);
            _addHoverTime(extraTime, 0, -1);
        } else {
            _addCruiseTime(cruiseTime, distance, seqNum);
            _addCruiseTime(extraTime, 0, -1);
        }
    } else {
        if (_controllerVehicle->multiRotor()) {
            _addHoverTime(hoverTime, distance, seqNum);
            _addHoverTime(extraTime, 0, -1);
        } else {
            _addCruiseTime(cruiseTime, distance, seqNum);
            _addCruiseTime(extraTime, 0, -1);
        }
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1177
void MissionController::_recalcMissionFlightStatus()
1178
{
1179
    if (!_visualItems->count()) {
1180
        return;
1181
    }
1182 1183 1184 1185

    bool                firstCoordinateItem =   true;
    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));

1186
    bool showHomePosition = _settingsItem->coordinate().isValid();
1187

DonLakeFlyer's avatar
DonLakeFlyer committed
1188
    qCDebug(MissionControllerLog) << "_recalcMissionFlightStatus";
1189

1190 1191 1192
    // If home position is valid we can calculate distances between all waypoints.
    // If home position is not valid we can only calculate distances between waypoints which are
    // both relative altitude.
1193

1194
    // No values for first item
1195
    lastCoordinateItem->setAltDifference(0.0);
1196
    lastCoordinateItem->setAzimuth(0.0);
1197
    lastCoordinateItem->setDistance(0.0);
1198

1199 1200
    double minAltSeen = 0.0;
    double maxAltSeen = 0.0;
1201 1202
    const double homePositionAltitude = _settingsItem->coordinate().altitude();
    minAltSeen = maxAltSeen = _settingsItem->coordinate().altitude();
1203

1204
    _resetMissionFlightStatus();
1205

DonLakeFlyer's avatar
DonLakeFlyer committed
1206
    bool vtolInHover = true;
1207
    bool linkStartToHome = false;
1208 1209 1210 1211 1212 1213 1214 1215 1216 1217
    bool linkEndToHome = false;

    if (showHomePosition) {
        SimpleMissionItem* lastItem = _visualItems->value<SimpleMissionItem*>(_visualItems->count() - 1);
        if (lastItem && (int)lastItem->command() == MAV_CMD_NAV_RETURN_TO_LAUNCH) {
            linkEndToHome = true;
        } else {
            linkEndToHome = _settingsItem->missionEndRTL();
        }
    }
1218

DonLakeFlyer's avatar
DonLakeFlyer committed
1219
    for (int i=0; i<_visualItems->count(); i++) {
1220
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1221 1222 1223
        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
        ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item);

1224 1225
        // Assume the worst
        item->setAzimuth(0.0);
1226
        item->setDistance(0.0);
1227

DonLakeFlyer's avatar
DonLakeFlyer committed
1228 1229 1230
        // Look for speed changed
        double newSpeed = item->specifiedFlightSpeed();
        if (!qIsNaN(newSpeed)) {
1231
            if (_controllerVehicle->multiRotor()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1232
                _missionFlightStatus.hoverSpeed = newSpeed;
1233
            } else if (_controllerVehicle->vtol()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1234 1235
                if (vtolInHover) {
                    _missionFlightStatus.hoverSpeed = newSpeed;
1236
                } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
1237
                    _missionFlightStatus.cruiseSpeed = newSpeed;
1238
                }
DonLakeFlyer's avatar
DonLakeFlyer committed
1239 1240
            } else {
                _missionFlightStatus.cruiseSpeed = newSpeed;
1241
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
1242 1243