MissionController.cc 105 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * 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 "SurveyComplexItem.h"
20
#include "FixedWingLandingComplexItem.h"
21
#include "StructureScanComplexItem.h"
22
#include "CorridorScanComplexItem.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 "KMLPlanDomDocument.h"
32
#include "QGCCorePlugin.h"
33
#include "TakeoffMissionItem.h"
34
#include "PlanViewSettings.h"
35

36 37
#define UPDATE_TIMEOUT 5000 ///< How often we check for bounding box changes

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

DonLakeFlyer's avatar
DonLakeFlyer committed
56 57 58 59
const QString MissionController::patternSurveyName          (QT_TRANSLATE_NOOP("MissionController", "Survey"));
const QString MissionController::patternFWLandingName       (QT_TRANSLATE_NOOP("MissionController", "Fixed Wing Landing"));
const QString MissionController::patternStructureScanName   (QT_TRANSLATE_NOOP("MissionController", "Structure Scan"));
const QString MissionController::patternCorridorScanName    (QT_TRANSLATE_NOOP("MissionController", "Corridor Scan"));
60

61
MissionController::MissionController(PlanMasterController* masterController, QObject *parent)
62
    : PlanElementController     (masterController, parent)
63 64 65
    , _controllerVehicle        (masterController->controllerVehicle())
    , _managerVehicle           (masterController->managerVehicle())
    , _missionManager           (masterController->managerVehicle()->missionManager())
66
    , _planViewSettings         (qgcApp()->toolbox()->settingsManager()->planViewSettings())
67
    , _appSettings              (qgcApp()->toolbox()->settingsManager()->appSettings())
68
{
69
    _resetMissionFlightStatus();
70

71 72
    _updateTimer.setSingleShot(true);
    connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout);
73 74

    connect(_planViewSettings->takeoffItemNotRequired(), &Fact::rawValueChanged, this, &MissionController::_takeoffItemNotRequiredChanged);
75 76 77 78
}

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

80 81
}

82 83 84 85 86 87 88 89 90
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;
91 92 93
    _missionFlightStatus.cruiseSpeed =          _controllerVehicle->defaultCruiseSpeed();
    _missionFlightStatus.hoverSpeed =           _controllerVehicle->defaultHoverSpeed();
    _missionFlightStatus.vehicleSpeed =         _controllerVehicle->multiRotor() || _managerVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed;
94
    _missionFlightStatus.vehicleYaw =           0.0;
95
    _missionFlightStatus.gimbalYaw =            std::numeric_limits<double>::quiet_NaN();
96
    _missionFlightStatus.gimbalPitch =          std::numeric_limits<double>::quiet_NaN();
97 98 99 100 101 102 103 104 105 106 107 108

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

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

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

125 126
}

127
void MissionController::start(bool flyView)
128
{
129
    qCDebug(MissionControllerLog) << "start flyView" << flyView;
130

131 132 133
    _managerVehicleChanged(_managerVehicle);
    connect(_masterController, &PlanMasterController::managerVehicleChanged, this, &MissionController::_managerVehicleChanged);

134
    PlanElementController::start(flyView);
135 136 137 138 139
    _init();
}

void MissionController::_init(void)
{
140
    // We start with an empty mission
141
    _visualItems = new QmlObjectListModel(this);
142
    _addMissionSettings(_visualItems);
143
    _initAllVisualItems();
144 145
}

146
// Called when new mission items have completed downloading from Vehicle
147
void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllRequested)
148
{
149
    qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle flyView:count" << _flyView << _missionManager->missionItems().count();
150

DonLakeFlyer's avatar
DonLakeFlyer committed
151
    // Fly view always reloads on _loadComplete
152 153 154 155
    // Plan view only reloads if:
    //  - Load was specifically requested
    //  - There is no current Plan
    if (_flyView || removeAllRequested || _itemsRequested || isEmpty()) {
156
        // Fly Mode (accept if):
Don Gagne's avatar
Don Gagne committed
157
        //      - Always accepts new items from the vehicle so Fly view is kept up to date
158
        // Edit Mode (accept if):
159 160
        //      - Remove all was requested from Fly view (clear mission on flight end)
        //      - A load from vehicle was manually requested
Don Gagne's avatar
Don Gagne committed
161
        //      - The initial automatic load from a vehicle completed and the current editor is empty
162

163 164 165
        _deinitAllVisualItems();
        _visualItems->deleteLater();
        _visualItems  = nullptr;
166
        _settingsItem = nullptr;
167 168
        _updateContainsItems(); // This will clear containsItems which will be set again below. This will re-pop Start Mission confirmation.

169
        QmlObjectListModel* newControllerMissionItems = new QmlObjectListModel(this);
170
        const QList<MissionItem*>& newMissionItems = _missionManager->missionItems();
171 172
        qCDebug(MissionControllerLog) << "loading from vehicle: count"<< newMissionItems.count();

173 174 175
        _missionItemCount = newMissionItems.count();
        emit missionItemCountChanged(_missionItemCount);

176
        MissionSettingsItem* settingsItem = _addMissionSettings(newControllerMissionItems);
177 178

        int i=0;
179
        if (_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() && newMissionItems.count() != 0) {
180
            // First item is fake home position
181 182
            MissionItem* fakeHomeItem = newMissionItems[0];
            if (fakeHomeItem->coordinate().latitude() != 0 || fakeHomeItem->coordinate().longitude() != 0) {
183
                settingsItem->setInitialHomePosition(fakeHomeItem->coordinate());
184
            }
185 186
            i = 1;
        }
187

188
        for (; i < newMissionItems.count(); i++) {
189
            const MissionItem* missionItem = newMissionItems[i];
190
            SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, _flyView, *missionItem, this);
191 192
            if (TakeoffMissionItem::isTakeoffCommand(static_cast<MAV_CMD>(simpleItem->command()))) {
                // This needs to be a TakeoffMissionItem
193
                TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(*missionItem, _masterController, _flyView, settingsItem, this);
194 195 196 197
                simpleItem->deleteLater();
                simpleItem = takeoffItem;
            }
            newControllerMissionItems->append(simpleItem);
198 199 200
        }

        _visualItems = newControllerMissionItems;
201
        _settingsItem = settingsItem;
202

203
        MissionController::_scanForAdditionalSettings(_visualItems, _masterController);
204

205
        _initAllVisualItems();
206
        _updateContainsItems();
207
        emit newItemsFromVehicle();
208
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
209
    _itemsRequested = false;
210 211
}

212
void MissionController::loadFromVehicle(void)
213
{
DonLakeFlyer's avatar
DonLakeFlyer committed
214 215 216 217 218 219 220 221
    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();
    }
222 223
}

224
void MissionController::sendToVehicle(void)
225
{
DonLakeFlyer's avatar
DonLakeFlyer committed
226 227 228 229 230
    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
231
        qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle";
DonLakeFlyer's avatar
DonLakeFlyer committed
232 233 234 235 236 237 238 239 240
        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
241 242
}

243 244 245 246 247
/// 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
248 249 250 251
    if (visualMissionItems->count() == 0) {
        return false;
    }

252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267
    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
268
    MissionSettingsItem* settingsItem = visualMissionItems->value<MissionSettingsItem*>(0);
269 270 271 272 273 274 275
    if (settingsItem) {
        endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent);
    }

    return endActionSet;
}

276
void MissionController::addMissionToKML(KMLPlanDomDocument& planKML)
277
{
278 279
    QObject*            deleteParent = new QObject();
    QList<MissionItem*> rgMissionItems;
280

281
    _convertToMissionItems(_visualItems, rgMissionItems, deleteParent);
282
    planKML.addMissionItems(_controllerVehicle, rgMissionItems);
283
    deleteParent->deleteLater();
284 285
}

Don Gagne's avatar
Don Gagne committed
286 287 288
void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems)
{
    if (vehicle) {
289
        QList<MissionItem*> rgMissionItems;
290

291
        _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle);
292

293
        // PlanManager takes control of MissionItems so no need to delete
294
        vehicle->missionManager()->writeMissionItems(rgMissionItems);
295 296
    }
}
297

298 299 300 301 302 303
int MissionController::_nextSequenceNumber(void)
{
    if (_visualItems->count() == 0) {
        qWarning() << "Internal error: Empty visual item list";
        return 0;
    } else {
304 305
        VisualMissionItem* lastItem = _visualItems->value<VisualMissionItem*>(_visualItems->count() - 1);
        return lastItem->lastSequenceNumber() + 1;
306 307 308
    }
}

309
VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordinate coordinate, MAV_CMD command, int visualItemIndex, bool makeCurrentItem)
310
{
311
    int sequenceNumber = _nextSequenceNumber();
312
    SimpleMissionItem * newItem = new SimpleMissionItem(_masterController, _flyView, false /* forLoad */, this);
313
    newItem->setSequenceNumber(sequenceNumber);
314
    newItem->setCoordinate(coordinate);
315
    newItem->setCommand(command);
316
    _initVisualItem(newItem);
317 318

    if (newItem->specifiesAltitude()) {
319 320 321 322 323 324 325 326 327
        const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, command);
        if (!uiInfo->isLandCommand()) {
            double  prevAltitude;
            int     prevAltitudeMode;

            if (_findPreviousAltitude(visualItemIndex, &prevAltitude, &prevAltitudeMode)) {
                newItem->altitude()->setRawValue(prevAltitude);
                newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
            }
328
        }
329
    }
330 331 332 333 334 335 336 337
    newItem->setMissionFlightStatus(_missionFlightStatus);
    if (visualItemIndex == -1) {
        _visualItems->append(newItem);
    } else {
        _visualItems->insert(visualItemIndex, newItem);
    }

    // We send the click coordinate through here to be able to set the planned home position from the user click location if needed
338
    _recalcAllWithCoordinate(coordinate);
339 340

    if (makeCurrentItem) {
341
        setCurrentPlanViewSeqNum(newItem->sequenceNumber(), true);
342 343 344 345 346
    }

    return newItem;
}

347 348 349 350 351 352

VisualMissionItem* MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem)
{
    return _insertSimpleMissionItemWorker(coordinate, MAV_CMD_NAV_WAYPOINT, visualItemIndex, makeCurrentItem);
}

353
VisualMissionItem* MissionController::insertTakeoffItem(QGeoCoordinate /*coordinate*/, int visualItemIndex, bool makeCurrentItem)
354 355
{
    int sequenceNumber = _nextSequenceNumber();
356
    TakeoffMissionItem * newItem = new TakeoffMissionItem(_controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF, _masterController, _flyView, _settingsItem, this);
357 358 359
    newItem->setSequenceNumber(sequenceNumber);
    _initVisualItem(newItem);

360 361 362
    if (newItem->specifiesAltitude()) {
        double  prevAltitude;
        int     prevAltitudeMode;
363

Don Gagne's avatar
Don Gagne committed
364
        if (_findPreviousAltitude(visualItemIndex, &prevAltitude, &prevAltitudeMode)) {
365
            newItem->altitude()->setRawValue(prevAltitude);
366
            newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
367
        }
368
    }
369
    newItem->setMissionFlightStatus(_missionFlightStatus);
370 371 372 373 374
    if (visualItemIndex == -1) {
        _visualItems->append(newItem);
    } else {
        _visualItems->insert(visualItemIndex, newItem);
    }
375

376
    _recalcAll();
377

378
    if (makeCurrentItem) {
379
        setCurrentPlanViewSeqNum(newItem->sequenceNumber(), true);
380 381 382
    }

    return newItem;
383 384
}

385
VisualMissionItem* MissionController::insertLandItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem)
386
{
387 388 389 390
    if (_managerVehicle->fixedWing()) {
        FixedWingLandingComplexItem* fwLanding = qobject_cast<FixedWingLandingComplexItem*>(insertComplexMissionItem(MissionController::patternFWLandingName, coordinate, visualItemIndex, makeCurrentItem));
        fwLanding->setLoiterDragAngleOnly(true);
        return fwLanding;
391
    } else {
392
        return _insertSimpleMissionItemWorker(coordinate, _managerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_RETURN_TO_LAUNCH, visualItemIndex, makeCurrentItem);
393
    }
394
}
395

396 397
VisualMissionItem* MissionController::insertROIMissionItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem)
{
398 399 400 401 402 403
    SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(_insertSimpleMissionItemWorker(coordinate, MAV_CMD_DO_SET_ROI_LOCATION, visualItemIndex, makeCurrentItem));

    if (!_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_DO_SET_ROI_LOCATION)) {
        simpleItem->setCommand(MAV_CMD_DO_SET_ROI)  ;
        simpleItem->missionItem().setParam1(MAV_ROI_LOCATION);
    }
404
    _recalcROISpecialVisuals();
405 406 407 408 409 410 411 412 413 414 415
    return simpleItem;
}

VisualMissionItem* MissionController::insertCancelROIMissionItem(int visualItemIndex, bool makeCurrentItem)
{
    SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(_insertSimpleMissionItemWorker(QGeoCoordinate(), MAV_CMD_DO_SET_ROI_NONE, visualItemIndex, makeCurrentItem));

    if (!_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_DO_SET_ROI_NONE)) {
        simpleItem->setCommand(MAV_CMD_DO_SET_ROI)  ;
        simpleItem->missionItem().setParam1(MAV_ROI_NONE);
    }
416
    _recalcROISpecialVisuals();
417
    return simpleItem;
418 419
}

420
VisualMissionItem* MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int visualItemIndex, bool makeCurrentItem)
421
{
422
    ComplexMissionItem* newItem = nullptr;
423

DonLakeFlyer's avatar
DonLakeFlyer committed
424
    if (itemName == patternSurveyName) {
425
        newItem = new SurveyComplexItem(_masterController, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
426
        newItem->setCoordinate(mapCenterCoordinate);
427
    } else if (itemName == patternFWLandingName) {
428
        newItem = new FixedWingLandingComplexItem(_masterController, _flyView, _visualItems /* parent */);
429
    } else if (itemName == patternStructureScanName) {
430
        newItem = new StructureScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
431
    } else if (itemName == patternCorridorScanName) {
432
        newItem = new CorridorScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
433 434
    } else {
        qWarning() << "Internal error: Unknown complex item:" << itemName;
435
        return nullptr;
436 437
    }

438
    _insertComplexMissionItemWorker(mapCenterCoordinate, newItem, visualItemIndex, makeCurrentItem);
439 440

    return newItem;
441 442
}

443
VisualMissionItem* MissionController::insertComplexMissionItemFromKMLOrSHP(QString itemName, QString file, int visualItemIndex, bool makeCurrentItem)
444
{
445
    ComplexMissionItem* newItem = nullptr;
446

DonLakeFlyer's avatar
DonLakeFlyer committed
447
    if (itemName == patternSurveyName) {
448
        newItem = new SurveyComplexItem(_masterController, _flyView, file, _visualItems);
449
    } else if (itemName == patternStructureScanName) {
450
        newItem = new StructureScanComplexItem(_masterController, _flyView, file, _visualItems);
451
    } else if (itemName == patternCorridorScanName) {
452
        newItem = new CorridorScanComplexItem(_masterController, _flyView, file, _visualItems);
453 454
    } else {
        qWarning() << "Internal error: Unknown complex item:" << itemName;
455
        return nullptr;
456 457
    }

458
    _insertComplexMissionItemWorker(QGeoCoordinate(), newItem, visualItemIndex, makeCurrentItem);
459 460

    return newItem;
461 462
}

463
void MissionController::_insertComplexMissionItemWorker(const QGeoCoordinate& mapCenterCoordinate, ComplexMissionItem* complexItem, int visualItemIndex, bool makeCurrentItem)
464 465
{
    int sequenceNumber = _nextSequenceNumber();
466
    bool surveyStyleItem = qobject_cast<SurveyComplexItem*>(complexItem) ||
467 468
            qobject_cast<CorridorScanComplexItem*>(complexItem) ||
            qobject_cast<StructureScanComplexItem*>(complexItem);
469

470
    if (surveyStyleItem) {
471
        bool rollSupported  = false;
472
        bool pitchSupported = false;
473
        bool yawSupported   = false;
474 475 476

        // If the vehicle is known to have a gimbal then we automatically point the gimbal straight down if not already set

477 478
        MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
        CameraSection* cameraSection = settingsItem->cameraSection();
479

480
        // Set camera to photo mode (leave alone if user already specified)
481
        if (cameraSection->cameraModeSupported() && !cameraSection->specifyCameraMode()) {
482
            cameraSection->setSpecifyCameraMode(true);
483
            cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE_SURVEY);
484
        }
485

486
        // Point gimbal straight down
487
        if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
488 489 490
            // If the user already specified a gimbal angle leave it alone
            if (!cameraSection->specifyGimbal()) {
                cameraSection->setSpecifyGimbal(true);
491
                cameraSection->gimbalPitch()->setRawValue(-90.0);
492 493
            }
        }
494
    }
495

496
    complexItem->setSequenceNumber(sequenceNumber);
497
    complexItem->setWizardMode(true);
498
    _initVisualItem(complexItem);
499

Don Gagne's avatar
Don Gagne committed
500
    if (visualItemIndex == -1) {
501 502
        _visualItems->append(complexItem);
    } else {
Don Gagne's avatar
Don Gagne committed
503
        _visualItems->insert(visualItemIndex, complexItem);
504
    }
505

506
    //-- Keep track of bounding box changes in complex items
507 508
    if(!complexItem->isSimpleItem()) {
        connect(complexItem, &ComplexMissionItem::boundingCubeChanged, this, &MissionController::_complexBoundingBoxChanged);
509
    }
510
    _recalcAllWithCoordinate(mapCenterCoordinate);
511

512
    if (makeCurrentItem) {
513
        setCurrentPlanViewSeqNum(complexItem->sequenceNumber(), true);
514
    }
515 516
}

517
void MissionController::removeMissionItem(int viIndex)
518
{
519 520
    if (viIndex <= 0 || viIndex >= _visualItems->count()) {
        qWarning() << "MissionController::removeMissionItem called with bad index - count:index" << _visualItems->count() << viIndex;
521 522 523
        return;
    }

524 525
    bool removeSurveyStyle = _visualItems->value<SurveyComplexItem*>(viIndex) || _visualItems->value<CorridorScanComplexItem*>(viIndex);
    VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(viIndex));
526

527
    _deinitVisualItem(item);
528
    item->deleteLater();
529

530 531
    if (removeSurveyStyle) {
        // Determine if the mission still has another survey style item in it
532 533
        bool foundSurvey = false;
        for (int i=1; i<_visualItems->count(); i++) {
534
            if (_visualItems->value<SurveyComplexItem*>(i) || _visualItems->value<CorridorScanComplexItem*>(i)) {
535 536 537 538 539
                foundSurvey = true;
                break;
            }
        }

540
        // If there is no longer a survey item in the mission remove added commands
541 542 543 544
        if (!foundSurvey) {
            bool rollSupported = false;
            bool pitchSupported = false;
            bool yawSupported = false;
545
            CameraSection* cameraSection = _settingsItem->cameraSection();
546
            if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
547
                if (cameraSection->specifyGimbal() && cameraSection->gimbalPitch()->rawValue().toDouble() == -90.0 && cameraSection->gimbalYaw()->rawValue().toDouble() == 0.0) {
548 549 550
                    cameraSection->setSpecifyGimbal(false);
                }
            }
551
            if (cameraSection->cameraModeSupported() && cameraSection->specifyCameraMode() && cameraSection->cameraMode()->rawValue().toInt() == 0) {
552 553
                cameraSection->setSpecifyCameraMode(false);
            }
554 555 556
        }
    }

557
    _recalcAll();
558 559 560 561 562 563 564 565 566 567

    // Adjust current item
    int newVIIndex;
    if (viIndex >= _visualItems->count()) {
        newVIIndex = _visualItems->count() - 1;
    } else {
        newVIIndex = viIndex;
    }
    setCurrentPlanViewSeqNum(_visualItems->value<VisualMissionItem*>(newVIIndex)->sequenceNumber(), true);

568
    setDirty(true);
569 570
}

571
void MissionController::removeAll(void)
572
{
573 574
    if (_visualItems) {
        _deinitAllVisualItems();
DonLakeFlyer's avatar
DonLakeFlyer committed
575
        _visualItems->clearAndDeleteContents();
Don Gagne's avatar
Don Gagne committed
576
        _visualItems->deleteLater();
577
        _settingsItem = nullptr;
578
        _visualItems = new QmlObjectListModel(this);
579
        _addMissionSettings(_visualItems);
580
        _initAllVisualItems();
581
        setDirty(true);
582
        _resetMissionFlightStatus();
Don Gagne's avatar
Don Gagne committed
583 584 585
    }
}

586
bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
587 588 589 590 591 592 593 594 595
{
    // 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)) {
596 597 598
        return false;
    }

599
    // Read complex items
600
    QList<SurveyComplexItem*> surveyItems;
601 602 603 604
    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];
605

606 607 608 609 610
        if (!itemValue.isObject()) {
            errorString = QStringLiteral("Mission item is not an object");
            return false;
        }

611
        SurveyComplexItem* item = new SurveyComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems /* parent */);
Don Gagne's avatar
Don Gagne committed
612 613
        const QJsonObject itemObject = itemValue.toObject();
        if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
614
            surveyItems.append(item);
615 616
        } else {
            return false;
617
        }
618
    }
619

620 621 622 623 624
    // 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
625
    QJsonArray itemArray(json[_jsonItemsKey].toArray());
626

627
    MissionSettingsItem* settingsItem = _addMissionSettings(visualItems);
628
    if (json.contains(_jsonPlannedHomePositionKey)) {
629
        SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems);
630
        if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) {
631
            settingsItem->setInitialHomePositionFromUser(item->coordinate());
632 633 634 635 636 637
            item->deleteLater();
        } else {
            return false;
        }
    }

638
    qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << surveyItems.count();
639 640 641 642
    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
643
        if (nextComplexItemIndex < surveyItems.count()) {
644
            SurveyComplexItem* complexItem = surveyItems[nextComplexItemIndex];
645 646 647 648 649 650 651 652 653 654 655 656 657 658

            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++];

659 660 661 662 663
            if (!itemValue.isObject()) {
                errorString = QStringLiteral("Mission item is not an object");
                return false;
            }

Don Gagne's avatar
Don Gagne committed
664
            const QJsonObject itemObject = itemValue.toObject();
665
            SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems);
Don Gagne's avatar
Don Gagne committed
666
            if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
667 668
                if (TakeoffMissionItem::isTakeoffCommand(item->mavCommand())) {
                    // This needs to be a TakeoffMissionItem
669
                    TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, true /* forLoad */, visualItems);
670 671 672 673
                    takeoffItem->load(itemObject, itemObject["id"].toInt(), errorString);
                    item->deleteLater();
                    item = takeoffItem;
                }
674
                qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber();
675
                nextSequenceNumber = item->lastSequenceNumber() + 1;
676
                visualItems->append(item);
677 678 679 680
            } else {
                return false;
            }
        }
681
    } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < surveyItems.count());
682 683 684 685

    return true;
}

686
bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
687 688 689 690 691 692
{
    // Validate root object keys
    QList<JsonHelper::KeyValidateInfo> rootKeyInfoList = {
        { _jsonPlannedHomePositionKey,      QJsonValue::Array,  true },
        { _jsonItemsKey,                    QJsonValue::Array,  true },
        { _jsonFirmwareTypeKey,             QJsonValue::Double, true },
693
        { _jsonVehicleTypeKey,              QJsonValue::Double, false },
694 695
        { _jsonCruiseSpeedKey,              QJsonValue::Double, false },
        { _jsonHoverSpeedKey,               QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
696 697 698 699 700 701 702
    };
    if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) {
        return false;
    }

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

703
    // Mission Settings
704
    AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings();
705

706 707
    if (_masterController->offline()) {
        // We only update if offline since if we are online we use the online vehicle settings
708
        appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType(static_cast<MAV_AUTOPILOT>(json[_jsonFirmwareTypeKey].toInt())));
709
        if (json.contains(_jsonVehicleTypeKey)) {
710
            appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType(static_cast<MAV_TYPE>(json[_jsonVehicleTypeKey].toInt())));
711
        }
712
    }
713
    if (json.contains(_jsonCruiseSpeedKey)) {
714
        appSettings->offlineEditingCruiseSpeed()->setRawValue(json[_jsonCruiseSpeedKey].toDouble());
715 716
    }
    if (json.contains(_jsonHoverSpeedKey)) {
717
        appSettings->offlineEditingHoverSpeed()->setRawValue(json[_jsonHoverSpeedKey].toDouble());
718 719
    }

720 721 722 723
    QGeoCoordinate homeCoordinate;
    if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) {
        return false;
    }
724
    MissionSettingsItem* settingsItem = new MissionSettingsItem(_masterController, _flyView, visualItems);
725 726
    settingsItem->setCoordinate(homeCoordinate);
    visualItems->insert(0, settingsItem);
Don Gagne's avatar
Don Gagne committed
727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752
    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) {
753
            SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems);
754
            if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) {
755 756
                if (TakeoffMissionItem::isTakeoffCommand(static_cast<MAV_CMD>(simpleItem->command()))) {
                    // This needs to be a TakeoffMissionItem
757
                    TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, true /* forLoad */, this);
758 759 760 761
                    takeoffItem->load(itemObject, nextSequenceNumber, errorString);
                    simpleItem->deleteLater();
                    simpleItem = takeoffItem;
                }
762
                qCDebug(MissionControllerLog) << "Loading simple item: nextSequenceNumber:command" << nextSequenceNumber << simpleItem->command();
763
                nextSequenceNumber = simpleItem->lastSequenceNumber() + 1;
Don Gagne's avatar
Don Gagne committed
764 765 766 767 768 769 770 771 772 773 774 775 776
                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();

777
            if (complexItemType == SurveyComplexItem::jsonComplexItemTypeValue) {
Don Gagne's avatar
Don Gagne committed
778
                qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber;
779
                SurveyComplexItem* surveyItem = new SurveyComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems);
Don Gagne's avatar
Don Gagne committed
780 781 782 783 784 785
                if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = surveyItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Survey load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(surveyItem);
786
            } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) {
DonLakeFlyer's avatar
DonLakeFlyer committed
787
                qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber;
788
                FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_masterController, _flyView, visualItems);
DonLakeFlyer's avatar
DonLakeFlyer committed
789 790 791 792 793 794
                if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = landingItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "FW Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(landingItem);
795 796
            } else if (complexItemType == StructureScanComplexItem::jsonComplexItemTypeValue) {
                qCDebug(MissionControllerLog) << "Loading Structure Scan: nextSequenceNumber" << nextSequenceNumber;
797
                StructureScanComplexItem* structureItem = new StructureScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems);
798 799 800 801 802 803
                if (!structureItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = structureItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Structure Scan load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(structureItem);
804 805
            } else if (complexItemType == CorridorScanComplexItem::jsonComplexItemTypeValue) {
                qCDebug(MissionControllerLog) << "Loading Corridor Scan: nextSequenceNumber" << nextSequenceNumber;
806
                CorridorScanComplexItem* corridorItem = new CorridorScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems);
807 808 809 810 811 812
                if (!corridorItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = corridorItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Corridor Scan load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(corridorItem);
Don Gagne's avatar
Don Gagne committed
813 814 815 816 817 818 819 820 821 822 823 824 825
            } 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);
826
            if (doJumpItem->command() == MAV_CMD_DO_JUMP) {
Don Gagne's avatar
Don Gagne committed
827
                bool found = false;
828
                int findDoJumpId = static_cast<int>(doJumpItem->missionItem().param1());
Don Gagne's avatar
Don Gagne committed
829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849
                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;
}

850 851 852 853 854 855 856 857
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;
858 859 860 861 862 863
    JsonHelper::validateQGCJsonFile(json,
                                    _jsonFileTypeValue,    // expected file type
                                    1,                     // minimum supported version
                                    2,                     // maximum supported version
                                    fileVersion,
                                    errorString);
864 865

    if (fileVersion == 1) {
866
        return _loadJsonMissionFileV1(json, visualItems, errorString);
867
    } else {
868
        return _loadJsonMissionFileV2(json, visualItems, errorString);
869 870 871
    }
}

872
bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString)
873
{
874 875
    bool firstItem = true;
    bool plannedHomePositionInFile = false;
876 877 878 879 880 881 882 883 884

    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;
885
            plannedHomePositionInFile = true;
886 887 888
        } else if (version[2] == "120") {
            // Old QGC file, no planned home position
            versionOk = true;
889
            plannedHomePositionInFile = false;
890 891 892 893
        }
    }

    if (versionOk) {
894
        MissionSettingsItem* settingsItem = _addMissionSettings(visualItems);
895

896
        while (!stream.atEnd()) {
897
            SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems);
898
            if (item->load(stream)) {
899
                if (firstItem && plannedHomePositionInFile) {
900
                    settingsItem->setInitialHomePositionFromUser(item->coordinate());
901
                } else {
902 903
                    if (TakeoffMissionItem::isTakeoffCommand(static_cast<MAV_CMD>(item->command()))) {
                        // This needs to be a TakeoffMissionItem
904
                        TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, true /* forLoad */, visualItems);
905 906 907 908
                        takeoffItem->load(stream);
                        item->deleteLater();
                        item = takeoffItem;
                    }
909 910 911
                    visualItems->append(item);
                }
                firstItem = false;
912
            } else {
913
                errorString = tr("The mission file is corrupted.");
914 915 916 917
                return false;
            }
        }
    } else {
918
        errorString = tr("The mission file is not compatible with this version of %1.").arg(qgcApp()->applicationName());
919 920 921
        return false;
    }

922
    if (!plannedHomePositionInFile) {
923 924 925
        // 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));
926
            if (item && item->command() == MAV_CMD_DO_JUMP) {
927
                item->missionItem().setParam1(static_cast<int>(item->missionItem().param1()) + 1);
Don Gagne's avatar
Don Gagne committed
928 929
            }
        }
930 931 932
    }

    return true;
933 934
}

935
void MissionController::_initLoadedVisualItems(QmlObjectListModel* loadedVisualItems)
936
{
Don Gagne's avatar
Don Gagne committed
937 938 939
    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteLater();
940
        _settingsItem = nullptr;
Don Gagne's avatar
Don Gagne committed
941 942
    }

943
    _visualItems = loadedVisualItems;
Don Gagne's avatar
Don Gagne committed
944 945

    if (_visualItems->count() == 0) {
946
        _addMissionSettings(_visualItems);
947 948
    } else {
        _settingsItem = _visualItems->value<MissionSettingsItem*>(0);
Don Gagne's avatar
Don Gagne committed
949 950
    }

951
    MissionController::_scanForAdditionalSettings(_visualItems, _masterController);
952

Don Gagne's avatar
Don Gagne committed
953
    _initAllVisualItems();
954
}
955

956 957 958 959 960 961
bool MissionController::load(const QJsonObject& json, QString& errorString)
{
    QString errorStr;
    QString errorMessage = tr("Mission: %1");
    QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this);

962
    if (!_loadJsonMissionFileV2(json, loadedVisualItems, errorStr)) {
963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987
        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;
988
    }
989

990 991 992 993 994 995 996 997 998 999 1000 1001 1002
    _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);
1003
    if (!_loadTextMissionFile(stream, loadedVisualItems, errorStr)) {
1004 1005 1006 1007 1008 1009
        errorString = errorMessage.arg(errorStr);
        return false;
    }

    _initLoadedVisualItems(loadedVisualItems);

Don Gagne's avatar
Don Gagne committed
1010
    return true;
1011 1012
}

1013
int MissionController::readyForSaveState(void) const
1014 1015 1016
{
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
DonLakeFlyer's avatar
DonLakeFlyer committed
1017
        if (visualItem->readyForSaveState() != VisualMissionItem::ReadyForSave) {
1018
            return visualItem->readyForSaveState();
1019 1020 1021
        }
    }

1022
    return VisualMissionItem::ReadyForSave;
1023 1024
}

1025
void MissionController::save(QJsonObject& json)
1026
{
1027
    json[JsonHelper::jsonVersionKey] = _missionFileVersion;
1028

1029
    // Mission settings
1030

1031 1032 1033 1034 1035 1036 1037
    MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
    if (!settingsItem) {
        qWarning() << "First item is not MissionSettingsItem";
        return;
    }
    QJsonValue coordinateValue;
    JsonHelper::saveGeoCoordinate(settingsItem->coordinate(), true /* writeAltitude */, coordinateValue);
1038 1039 1040 1041 1042
    json[_jsonPlannedHomePositionKey]   = coordinateValue;
    json[_jsonFirmwareTypeKey]          = _controllerVehicle->firmwareType();
    json[_jsonVehicleTypeKey]           = _controllerVehicle->vehicleType();
    json[_jsonCruiseSpeedKey]           = _controllerVehicle->defaultCruiseSpeed();
    json[_jsonHoverSpeedKey]            = _controllerVehicle->defaultHoverSpeed();
1043

1044
    // Save the visual items
1045

1046 1047 1048
    QJsonArray rgJsonMissionItems;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1049

1050 1051
        visualItem->save(rgJsonMissionItems);
    }
1052

1053 1054 1055
    // Mission settings has a special case for end mission action
    if (settingsItem) {
        QList<MissionItem*> rgMissionItems;
1056

1057 1058 1059 1060 1061
        if (_convertToMissionItems(_visualItems, rgMissionItems, this /* missionItemParent */)) {
            QJsonObject saveObject;
            MissionItem* missionItem = rgMissionItems[rgMissionItems.count() - 1];
            missionItem->save(saveObject);
            rgJsonMissionItems.append(saveObject);
1062
        }
1063 1064
        for (int i=0; i<rgMissionItems.count(); i++) {
            rgMissionItems[i]->deleteLater();
1065 1066 1067
        }
    }

1068
    json[_jsonItemsKey] = rgJsonMissionItems;
1069 1070
}

1071
void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
1072
{
Don Gagne's avatar
Don Gagne committed
1073
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
1074
    QGeoCoordinate  prevCoord =     prevItem->exitCoordinate();
1075 1076 1077 1078
    bool            distanceOk =    false;

    // Convert to fixed altitudes

1079
    distanceOk = true;
1080
    if (currentItem != _settingsItem && currentItem->coordinateHasRelativeAltitude()) {
1081 1082
        currentCoord.setAltitude(homeAlt + currentCoord.altitude());
    }
1083
    if (prevItem != _settingsItem && prevItem->exitCoordinateHasRelativeAltitude()) {
1084
        prevCoord.setAltitude(homeAlt + prevCoord.altitude());
1085 1086 1087
    }

    if (distanceOk) {
Don Gagne's avatar
Don Gagne committed
1088 1089 1090
        *altDifference = currentCoord.altitude() - prevCoord.altitude();
        *distance = prevCoord.distanceTo(currentCoord);
        *azimuth = prevCoord.azimuthTo(currentCoord);
1091
    } else {
Don Gagne's avatar
Don Gagne committed
1092
        *altDifference = 0.0;
1093
        *azimuth = 0.0;
1094
        *distance = 0.0;
1095 1096 1097
    }
}