MissionController.cc 90.3 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 "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 "KML.h"
32
#include "QGCCorePlugin.h"
33

34 35
#include "src/Wima/CircularSurveyComplexItem.h"

36
#ifndef __mobile__
37
#include "MainWindow.h"
38
#include "QGCQFileDialog.h"
39 40
#endif

41 42
#define UPDATE_TIMEOUT 5000 ///< How often we check for bounding box changes

43 44
QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog")

45
const char* MissionController::_settingsGroup =                 "MissionController";
Don Gagne's avatar
Don Gagne committed
46 47
const char* MissionController::_jsonFileTypeValue =             "Mission";
const char* MissionController::_jsonItemsKey =                  "items";
48
const char* MissionController::_jsonPlannedHomePositionKey =    "plannedHomePosition";
Don Gagne's avatar
Don Gagne committed
49
const char* MissionController::_jsonFirmwareTypeKey =           "firmwareType";
50 51 52
const char* MissionController::_jsonVehicleTypeKey =            "vehicleType";
const char* MissionController::_jsonCruiseSpeedKey =            "cruiseSpeed";
const char* MissionController::_jsonHoverSpeedKey =             "hoverSpeed";
Don Gagne's avatar
Don Gagne committed
53 54 55 56 57 58 59
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;
60

61 62 63
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"));
64

65
MissionController::MissionController(PlanMasterController* masterController, QObject *parent)
66 67 68 69 70 71 72 73 74 75 76 77 78 79
    : PlanElementController         (masterController, parent)
    , _missionManager               (_managerVehicle->missionManager())
    , _missionItemCount             (0)
    , _visualItems                  (nullptr)
    , _settingsItem                 (nullptr)
    , _firstItemsFromVehicle        (false)
    , _itemsRequested               (false)
    , _inRecalcSequence             (false)
    , _surveyMissionItemName        (tr("Survey"))
    , _circularSurveyMissionItemName(tr("Circular Survey"))
    , _appSettings                  (qgcApp()->toolbox()->settingsManager()->appSettings())
    , _progressPct                  (0)
    , _currentPlanViewIndex         (-1)
    , _currentPlanViewItem          (nullptr)
80
{
81
    _resetMissionFlightStatus();
82
    managerVehicleChanged(_managerVehicle);
83 84
    _updateTimer.setSingleShot(true);
    connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout);
85 86 87 88
}

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

90 91
}

92 93 94 95 96 97 98 99 100
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;
101 102 103
    _missionFlightStatus.cruiseSpeed =          _controllerVehicle->defaultCruiseSpeed();
    _missionFlightStatus.hoverSpeed =           _controllerVehicle->defaultHoverSpeed();
    _missionFlightStatus.vehicleSpeed =         _controllerVehicle->multiRotor() || _managerVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed;
104
    _missionFlightStatus.vehicleYaw =           0.0;
105
    _missionFlightStatus.gimbalYaw =            std::numeric_limits<double>::quiet_NaN();
106
    _missionFlightStatus.gimbalPitch =          std::numeric_limits<double>::quiet_NaN();
107 108 109 110 111 112 113 114 115 116 117 118

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

119 120 121
    _controllerVehicle->firmwarePlugin()->batteryConsumptionData(_controllerVehicle, _missionFlightStatus.mAhBattery, _missionFlightStatus.hoverAmps, _missionFlightStatus.cruiseAmps);
    if (_missionFlightStatus.mAhBattery != 0) {
        double batteryPercentRemainingAnnounce = qgcApp()->toolbox()->settingsManager()->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toDouble();
122
        _missionFlightStatus.ampMinutesAvailable = static_cast<double>(_missionFlightStatus.mAhBattery) / 1000.0 * 60.0 * ((100.0 - batteryPercentRemainingAnnounce) / 100.0);
123
    }
124 125 126 127 128 129 130 131 132 133 134

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

135 136
}

137
void MissionController::start(bool flyView)
138
{
139
    qCDebug(MissionControllerLog) << "start flyView" << flyView;
140

141
    PlanElementController::start(flyView);
142 143 144 145 146
    _init();
}

void MissionController::_init(void)
{
147
    // We start with an empty mission
148
    _visualItems = new QmlObjectListModel(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
149
    _addMissionSettings(_visualItems, false /* addToCenter */);
150
    _initAllVisualItems();
151 152
}

153
// Called when new mission items have completed downloading from Vehicle
154
void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllRequested)
155
{
156
    qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle flyView:count" << _flyView << _missionManager->missionItems().count();
157

DonLakeFlyer's avatar
DonLakeFlyer committed
158 159
    // Fly view always reloads on _loadComplete
    // Plan view only reloads on _loadComplete if specifically requested
160
    if (_flyView || removeAllRequested || _itemsRequested || _visualItems->count() <= 1) {
161
        // Fly Mode (accept if):
Don Gagne's avatar
Don Gagne committed
162
        //      - Always accepts new items from the vehicle so Fly view is kept up to date
163
        // Edit Mode (accept if):
164 165
        //      - 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
166
        //      - The initial automatic load from a vehicle completed and the current editor is empty
167

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

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

175
        int i = 0;
176
        if (_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() && newMissionItems.count() != 0) {
177
            // First item is fake home position
DonLakeFlyer's avatar
DonLakeFlyer committed
178
            _addMissionSettings(newControllerMissionItems, false /* addToCenter */);
179
            MissionSettingsItem* settingsItem = newControllerMissionItems->value<MissionSettingsItem*>(0);
180 181 182 183
            if (!settingsItem) {
                qWarning() << "First item is not settings item";
                return;
            }
184 185 186 187
            MissionItem* fakeHomeItem = newMissionItems[0];
            if (fakeHomeItem->coordinate().latitude() != 0 || fakeHomeItem->coordinate().longitude() != 0) {
                settingsItem->setCoordinate(fakeHomeItem->coordinate());
            }
188 189
            i = 1;
        }
190

191
        for (; i < newMissionItems.count(); i++) {
192
            const MissionItem* missionItem = newMissionItems[i];
193
            newControllerMissionItems->append(new SimpleMissionItem(_controllerVehicle, _flyView, *missionItem, this));
194 195 196
        }

        _deinitAllVisualItems();
Don Gagne's avatar
Don Gagne committed
197
        _visualItems->deleteLater();
198 199
        _settingsItem = nullptr;
        _visualItems  = nullptr;
200
        _updateContainsItems(); // This will clear containsItems which will be set again below. This will re-pop Start Mission confirmation.
201 202
        _visualItems = newControllerMissionItems;

203
        if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) {
204
            _addMissionSettings(_visualItems, !_flyView && _visualItems->count() > 0 /* addToCenter */);
205 206
        }

207
        MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle);
208

209
        _initAllVisualItems();
210
        _updateContainsItems();
211
        emit newItemsFromVehicle();
212
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
213
    _itemsRequested = false;
214 215
}

216
void MissionController::loadFromVehicle(void)
217
{
DonLakeFlyer's avatar
DonLakeFlyer committed
218 219 220 221 222 223 224 225
    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();
    }
226 227
}

228 229 230 231
void MissionController::_warnIfTerrainFrameUsed(void)
{
    for (int i=1; i<_visualItems->count(); i++) {
        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(i));
232
        if (simpleItem && simpleItem->altitudeMode() == QGroundControlQmlGlobal::AltitudeModeTerrainFrame) {
233 234 235 236 237 238
            qgcApp()->showMessage(tr("Warning: You are using MAV_FRAME_GLOBAL_TERRAIN_ALT in a mission. %1 does not support sending terrain tiles to vehicle.").arg(qgcApp()->applicationName()));
            break;
        }
    }
}

239
void MissionController::sendToVehicle(void)
240
{
DonLakeFlyer's avatar
DonLakeFlyer committed
241 242 243 244 245
    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
246
        qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle";
247
        _warnIfTerrainFrameUsed();
DonLakeFlyer's avatar
DonLakeFlyer committed
248 249 250 251 252 253 254 255 256
        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
257 258
}

259
/// Converts from visual items to MissionItems
260
///     @param missionItemParent QObject parent for newly allocated MissionItems, _surveyMissionItemName    (tr("Survey"))
261 262 263
/// @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
264 265 266 267
    if (visualMissionItems->count() == 0) {
        return false;
    }

268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283
    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
284
    MissionSettingsItem* settingsItem = visualMissionItems->value<MissionSettingsItem*>(0);
285 286 287 288 289 290 291
    if (settingsItem) {
        endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent);
    }

    return endActionSet;
}

292 293
void MissionController::convertToKMLDocument(QDomDocument& document)
{
294 295
    QObject*            deleteParent = new QObject();
    QList<MissionItem*> rgMissionItems;
296

297 298
    _convertToMissionItems(_visualItems, rgMissionItems, deleteParent);
    if (rgMissionItems.count() == 0) {
299 300
        return;
    }
301

302
    const double homePositionAltitude = _settingsItem->coordinate().altitude();
303 304 305 306 307

    QString coord;
    QStringList coords;
    // Drop home position
    bool dropPoint = true;
308
    for(const auto& item : rgMissionItems) {
309 310 311 312 313
        if(dropPoint) {
            dropPoint = false;
            continue;
        }
        const MissionCommandUIInfo* uiInfo = \
314
                qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command());
315 316

        if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) {
317
            double amslAltitude = item->param7() + (item->frame() == MAV_FRAME_GLOBAL ? 0 : homePositionAltitude);
yaoling's avatar
yaoling committed
318
            coord = QString::number(item->param6(),'f',7) \
319 320 321
                    + "," \
                    + QString::number(item->param5(),'f',7) \
                    + "," \
322
                    + QString::number(amslAltitude,'f',2);
323 324 325
            coords.append(coord);
        }
    }
326 327 328

    deleteParent->deleteLater();

329 330 331 332 333
    Kml kml;
    kml.points(coords);
    kml.save(document);
}

Don Gagne's avatar
Don Gagne committed
334 335 336
void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems)
{
    if (vehicle) {
337
        QList<MissionItem*> rgMissionItems;
338

339
        _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle);
340

341
        // PlanManager takes control of MissionItems so no need to delete
342
        vehicle->missionManager()->writeMissionItems(rgMissionItems);
343 344
    }
}
345

346 347 348 349 350 351
int MissionController::_nextSequenceNumber(void)
{
    if (_visualItems->count() == 0) {
        qWarning() << "Internal error: Empty visual item list";
        return 0;
    } else {
352 353
        VisualMissionItem* lastItem = _visualItems->value<VisualMissionItem*>(_visualItems->count() - 1);
        return lastItem->lastSequenceNumber() + 1;
354 355 356
    }
}

357
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
358
{
359
    int sequenceNumber = _nextSequenceNumber();
360
    SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
361
    newItem->setSequenceNumber(sequenceNumber);
362
    newItem->setCoordinate(coordinate);
363
    newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
364
    _initVisualItem(newItem);
365
    if (_visualItems->count() == 1 && (_controllerVehicle->fixedWing() || _controllerVehicle->vtol() || _controllerVehicle->multiRotor())) {
366
        MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF;
367
        if (_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
368 369
            newItem->setCommand(takeoffCmd);
        }
370
    }
371 372 373
    if (newItem->specifiesAltitude()) {
        double  prevAltitude;
        int     prevAltitudeMode;
374

375 376
        if (_findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) {
            newItem->altitude()->setRawValue(prevAltitude);
377
            newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
378
        }
379
    }
380
    newItem->setMissionFlightStatus(_missionFlightStatus);
381
    _visualItems->insert(i, newItem);
382

383 384
    // We send the click coordinate through here to be able to set the planned home position from the user click location if needed
    _recalcAllWithClickCoordinate(coordinate);
385

386
    return newItem->sequenceNumber();
387 388
}

389 390 391
int MissionController::insertROIMissionItem(QGeoCoordinate coordinate, int i)
{
    int sequenceNumber = _nextSequenceNumber();
392
    SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
393
    newItem->setSequenceNumber(sequenceNumber);
394
    newItem->setCommand((_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_DO_SET_ROI_LOCATION) ?
395 396
        MAV_CMD_DO_SET_ROI_LOCATION :
        MAV_CMD_DO_SET_ROI));
397
    _initVisualItem(newItem);
398
    newItem->setCoordinate(coordinate);
399

400 401
    double  prevAltitude;
    int     prevAltitudeMode;
402

403 404
    if (_findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) {
        newItem->altitude()->setRawValue(prevAltitude);
405
        newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
406 407 408 409 410 411 412 413
    }
    _visualItems->insert(i, newItem);

    _recalcAll();

    return newItem->sequenceNumber();
}

414
int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i)
415
{
416 417
    ComplexMissionItem* newItem;

418
    int sequenceNumber = _nextSequenceNumber();
419
    if (itemName == _surveyMissionItemName) {
420
        newItem = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
421
        newItem->setCoordinate(mapCenterCoordinate);
422
    } else if (itemName == patternFWLandingName) {
423
        newItem = new FixedWingLandingComplexItem(_controllerVehicle, _flyView, _visualItems /* parent */);
424
    } else if (itemName == patternStructureScanName) {
425
        newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
426
    } else if (itemName == patternCorridorScanName) {
427
        newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
428 429
    } else if (itemName == _circularSurveyMissionItemName) {
        newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
430 431 432 433 434
    } else {
        qWarning() << "Internal error: Unknown complex item:" << itemName;
        return sequenceNumber;
    }

435 436 437
    return _insertComplexMissionItemWorker(newItem, i);
}

438
int MissionController::insertComplexMissionItemFromKMLOrSHP(QString itemName, QString file, int i)
439 440 441 442
{
    ComplexMissionItem* newItem;

    if (itemName == _surveyMissionItemName) {
443
        newItem = new SurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems);
444 445
    } else if (itemName == _circularSurveyMissionItemName) {
        newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems);
446
    } else if (itemName == patternStructureScanName) {
447
        newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, file, _visualItems);
448
    } else if (itemName == patternCorridorScanName) {
449
        newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, file, _visualItems);
450 451 452 453 454 455 456 457 458 459 460 461 462
    } else {
        qWarning() << "Internal error: Unknown complex item:" << itemName;
        return _nextSequenceNumber();
    }

    return _insertComplexMissionItemWorker(newItem, i);
}

int MissionController::_insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int i)
{
    int sequenceNumber = _nextSequenceNumber();
    bool surveyStyleItem = qobject_cast<SurveyComplexItem*>(complexItem) || qobject_cast<CorridorScanComplexItem*>(complexItem);

463
    if (surveyStyleItem) {
464
        bool rollSupported  = false;
465
        bool pitchSupported = false;
466
        bool yawSupported   = false;
467 468 469

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

470 471
        MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
        CameraSection* cameraSection = settingsItem->cameraSection();
472

473
        // Set camera to photo mode (leave alone if user already specified)
474
        if (cameraSection->cameraModeSupported() && !cameraSection->specifyCameraMode()) {
475
            cameraSection->setSpecifyCameraMode(true);
476
            cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE_SURVEY);
477
        }
478

479
        // Point gimbal straight down
480
        if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
481 482 483
            // If the user already specified a gimbal angle leave it alone
            if (!cameraSection->specifyGimbal()) {
                cameraSection->setSpecifyGimbal(true);
484
                cameraSection->gimbalPitch()->setRawValue(-90.0);
485 486
            }
        }
487
    }
488

489 490
    complexItem->setSequenceNumber(sequenceNumber);
    _initVisualItem(complexItem);
491

492 493 494 495 496
    if (i == -1) {
        _visualItems->append(complexItem);
    } else {
        _visualItems->insert(i, complexItem);
    }
497

498
    //-- Keep track of bounding box changes in complex items
499 500
    if(!complexItem->isSimpleItem()) {
        connect(complexItem, &ComplexMissionItem::boundingCubeChanged, this, &MissionController::_complexBoundingBoxChanged);
501
    }
502 503
    _recalcAll();

504
    return complexItem->sequenceNumber();
505 506
}

507 508
void MissionController::removeMissionItem(int index)
{
509 510 511 512 513
    if (index <= 0 || index >= _visualItems->count()) {
        qWarning() << "MissionController::removeMissionItem called with bad index - count:index" << _visualItems->count() << index;
        return;
    }

514
    bool removeSurveyStyle = _visualItems->value<SurveyComplexItem*>(index) || _visualItems->value<CorridorScanComplexItem*>(index);
515
    VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));
516

517
    _deinitVisualItem(item);
518
    item->deleteLater();
519

520 521
    if (removeSurveyStyle) {
        // Determine if the mission still has another survey style item in it
522 523
        bool foundSurvey = false;
        for (int i=1; i<_visualItems->count(); i++) {
524
            if (_visualItems->value<SurveyComplexItem*>(i) || _visualItems->value<CorridorScanComplexItem*>(i)) {
525 526 527 528 529
                foundSurvey = true;
                break;
            }
        }

530
        // If there is no longer a survey item in the mission remove added commands
531 532 533 534
        if (!foundSurvey) {
            bool rollSupported = false;
            bool pitchSupported = false;
            bool yawSupported = false;
535 536
            MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
            CameraSection* cameraSection = settingsItem->cameraSection();
537
            if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
538
                if (cameraSection->specifyGimbal() && cameraSection->gimbalPitch()->rawValue().toDouble() == -90.0 && cameraSection->gimbalYaw()->rawValue().toDouble() == 0.0) {
539 540 541
                    cameraSection->setSpecifyGimbal(false);
                }
            }
542
            if (cameraSection->cameraModeSupported() && cameraSection->specifyCameraMode() && cameraSection->cameraMode()->rawValue().toInt() == 0) {
543 544
                cameraSection->setSpecifyCameraMode(false);
            }
545 546 547
        }
    }

548
    _recalcAll();
549
    setDirty(true);
550 551
}

552
void MissionController::removeAll(void)
553
{
554 555
    if (_visualItems) {
        _deinitAllVisualItems();
DonLakeFlyer's avatar
DonLakeFlyer committed
556
        _visualItems->clearAndDeleteContents();
Don Gagne's avatar
Don Gagne committed
557
        _visualItems->deleteLater();
558
        _settingsItem = nullptr;
559
        _visualItems = new QmlObjectListModel(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
560
        _addMissionSettings(_visualItems, false /* addToCenter */);
561
        _initAllVisualItems();
562
        setDirty(true);
563
        _resetMissionFlightStatus();
Don Gagne's avatar
Don Gagne committed
564 565 566
    }
}

567
bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
568 569 570 571 572 573 574 575 576
{
    // 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)) {
577 578 579
        return false;
    }

580
    // Read complex items
581
    QList<SurveyComplexItem*> surveyItems;
582 583 584 585
    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];
586

587 588 589 590 591
        if (!itemValue.isObject()) {
            errorString = QStringLiteral("Mission item is not an object");
            return false;
        }

592
        SurveyComplexItem* item = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems /* parent */);
Don Gagne's avatar
Don Gagne committed
593 594
        const QJsonObject itemObject = itemValue.toObject();
        if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
595
            surveyItems.append(item);
596 597
        } else {
            return false;
598
        }
599
    }
600

601 602 603 604 605
    // 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
606
    QJsonArray itemArray(json[_jsonItemsKey].toArray());
607

608
    qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << surveyItems.count();
609 610 611 612
    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
613
        if (nextComplexItemIndex < surveyItems.count()) {
614
            SurveyComplexItem* complexItem = surveyItems[nextComplexItemIndex];
615 616 617 618 619 620 621 622 623 624 625 626 627 628

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

629 630 631 632 633
            if (!itemValue.isObject()) {
                errorString = QStringLiteral("Mission item is not an object");
                return false;
            }

Don Gagne's avatar
Don Gagne committed
634
            const QJsonObject itemObject = itemValue.toObject();
635
            SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems);
Don Gagne's avatar
Don Gagne committed
636
            if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
637
                qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber();
638
                nextSequenceNumber = item->lastSequenceNumber() + 1;
639
                visualItems->append(item);
640 641 642 643
            } else {
                return false;
            }
        }
644
    } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < surveyItems.count());
645 646

    if (json.contains(_jsonPlannedHomePositionKey)) {
647
        SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems);
648

Don Gagne's avatar
Don Gagne committed
649
        if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) {
650
            MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems);
651 652 653
            settingsItem->setCoordinate(item->coordinate());
            visualItems->insert(0, settingsItem);
            item->deleteLater();
654 655 656 657
        } else {
            return false;
        }
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
658
        _addMissionSettings(visualItems, true /* addToCenter */);
659 660 661 662 663
    }

    return true;
}

664
bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
665 666 667 668 669 670
{
    // Validate root object keys
    QList<JsonHelper::KeyValidateInfo> rootKeyInfoList = {
        { _jsonPlannedHomePositionKey,      QJsonValue::Array,  true },
        { _jsonItemsKey,                    QJsonValue::Array,  true },
        { _jsonFirmwareTypeKey,             QJsonValue::Double, true },
671
        { _jsonVehicleTypeKey,              QJsonValue::Double, false },
672 673
        { _jsonCruiseSpeedKey,              QJsonValue::Double, false },
        { _jsonHoverSpeedKey,               QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
674 675 676 677 678 679 680
    };
    if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) {
        return false;
    }

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

681
    // Mission Settings
682
    AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings();
683

684 685
    if (_masterController->offline()) {
        // We only update if offline since if we are online we use the online vehicle settings
686
        appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType(static_cast<MAV_AUTOPILOT>(json[_jsonFirmwareTypeKey].toInt())));
687
        if (json.contains(_jsonVehicleTypeKey)) {
688
            appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType(static_cast<MAV_TYPE>(json[_jsonVehicleTypeKey].toInt())));
689
        }
690
    }
691
    if (json.contains(_jsonCruiseSpeedKey)) {
692
        appSettings->offlineEditingCruiseSpeed()->setRawValue(json[_jsonCruiseSpeedKey].toDouble());
693 694
    }
    if (json.contains(_jsonHoverSpeedKey)) {
695
        appSettings->offlineEditingHoverSpeed()->setRawValue(json[_jsonHoverSpeedKey].toDouble());
696 697
    }

698 699 700 701
    QGeoCoordinate homeCoordinate;
    if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) {
        return false;
    }
702
    MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems);
703 704
    settingsItem->setCoordinate(homeCoordinate);
    visualItems->insert(0, settingsItem);
Don Gagne's avatar
Don Gagne committed
705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730
    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) {
731
            SimpleMissionItem* simpleItem = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems);
732
            if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) {
733
                qCDebug(MissionControllerLog) << "Loading simple item: nextSequenceNumber:command" << nextSequenceNumber << simpleItem->command();
734
                nextSequenceNumber = simpleItem->lastSequenceNumber() + 1;
Don Gagne's avatar
Don Gagne committed
735 736 737 738 739 740 741 742 743 744 745 746 747
                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();

748
            if (complexItemType == SurveyComplexItem::jsonComplexItemTypeValue) {
Don Gagne's avatar
Don Gagne committed
749
                qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber;
750
                SurveyComplexItem* surveyItem = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
Don Gagne's avatar
Don Gagne committed
751 752 753 754 755 756
                if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = surveyItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Survey load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(surveyItem);
757
            } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) {
DonLakeFlyer's avatar
DonLakeFlyer committed
758
                qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber;
759
                FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_controllerVehicle, _flyView, visualItems);
DonLakeFlyer's avatar
DonLakeFlyer committed
760 761 762 763 764 765
                if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = landingItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "FW Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(landingItem);
766 767
            } else if (complexItemType == StructureScanComplexItem::jsonComplexItemTypeValue) {
                qCDebug(MissionControllerLog) << "Loading Structure Scan: nextSequenceNumber" << nextSequenceNumber;
768
                StructureScanComplexItem* structureItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
769 770 771 772 773 774
                if (!structureItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = structureItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Structure Scan load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(structureItem);
775 776
            } else if (complexItemType == CorridorScanComplexItem::jsonComplexItemTypeValue) {
                qCDebug(MissionControllerLog) << "Loading Corridor Scan: nextSequenceNumber" << nextSequenceNumber;
777
                CorridorScanComplexItem* corridorItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
778 779 780 781 782 783
                if (!corridorItem->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = corridorItem->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Corridor Scan load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(corridorItem);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
784 785 786 787 788 789 790 791 792
            } else if (complexItemType == CircularSurveyComplexItem::jsonComplexItemTypeValue) {
                qCDebug(MissionControllerLog) << "Loading Circular Survey: nextSequenceNumber" << nextSequenceNumber;
                CircularSurveyComplexItem* circularSurvey = new CircularSurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems);
                if (!circularSurvey->load(itemObject, nextSequenceNumber++, errorString)) {
                    return false;
                }
                nextSequenceNumber = circularSurvey->lastSequenceNumber() + 1;
                qCDebug(MissionControllerLog) << "Circular Survey load complete: nextSequenceNumber" << nextSequenceNumber;
                visualItems->append(circularSurvey);
793
            } else if (complexItemType == MissionSettingsItem::jsonComplexItemTypeValue) {
DonLakeFlyer's avatar
DonLakeFlyer committed
794
                qCDebug(MissionControllerLog) << "Loading Mission Settings: nextSequenceNumber" << nextSequenceNumber;
795
                MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems);
DonLakeFlyer's avatar
DonLakeFlyer committed
796 797 798 799 800 801
                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
802 803 804 805 806 807 808 809 810 811 812 813 814
            } 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);
815
            if (doJumpItem->command() == MAV_CMD_DO_JUMP) {
Don Gagne's avatar
Don Gagne committed
816
                bool found = false;
817
                int findDoJumpId = static_cast<int>(doJumpItem->missionItem().param1());
Don Gagne's avatar
Don Gagne committed
818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838
                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;
}

839 840 841 842 843 844 845 846
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;
847 848 849 850 851 852
    JsonHelper::validateQGCJsonFile(json,
                                    _jsonFileTypeValue,    // expected file type
                                    1,                     // minimum supported version
                                    2,                     // maximum supported version
                                    fileVersion,
                                    errorString);
853 854

    if (fileVersion == 1) {
855
        return _loadJsonMissionFileV1(json, visualItems, errorString);
856
    } else {
857
        return _loadJsonMissionFileV2(json, visualItems, errorString);
858 859 860
    }
}

861
bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString)
862
{
863 864
    bool firstItem = true;
    bool plannedHomePositionInFile = false;
865 866 867 868 869 870 871 872 873

    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;
874
            plannedHomePositionInFile = true;
875 876 877
        } else if (version[2] == "120") {
            // Old QGC file, no planned home position
            versionOk = true;
878
            plannedHomePositionInFile = false;
879 880 881 882
        }
    }

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

887
        while (!stream.atEnd()) {
888
            SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems);
889 890

            if (item->load(stream)) {
891 892 893 894 895 896
                if (firstItem && plannedHomePositionInFile) {
                    settingsItem->setCoordinate(item->coordinate());
                } else {
                    visualItems->append(item);
                }
                firstItem = false;
897
            } else {
898
                errorString = tr("The mission file is corrupted.");
899 900 901 902
                return false;
            }
        }
    } else {
903
        errorString = tr("The mission file is not compatible with this version of %1.").arg(qgcApp()->applicationName());
904 905 906
        return false;
    }

907
    if (!plannedHomePositionInFile) {
908 909 910
        // 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));
911
            if (item && item->command() == MAV_CMD_DO_JUMP) {
912
                item->missionItem().setParam1(static_cast<int>(item->missionItem().param1()) + 1);
Don Gagne's avatar
Don Gagne committed
913 914
            }
        }
915 916 917
    }

    return true;
918 919
}

920
void MissionController::_initLoadedVisualItems(QmlObjectListModel* loadedVisualItems)
921
{
Don Gagne's avatar
Don Gagne committed
922 923 924
    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteLater();
925
        _settingsItem = nullptr;
Don Gagne's avatar
Don Gagne committed
926 927
    }

928
    _visualItems = loadedVisualItems;
Don Gagne's avatar
Don Gagne committed
929 930

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

934
    MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle);
935

Don Gagne's avatar
Don Gagne committed
936
    _initAllVisualItems();
937
}
938

939 940 941 942 943 944
bool MissionController::load(const QJsonObject& json, QString& errorString)
{
    QString errorStr;
    QString errorMessage = tr("Mission: %1");
    QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this);

945
    if (!_loadJsonMissionFileV2(json, loadedVisualItems, errorStr)) {
946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970
        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;
971
    }
972

973 974 975 976 977 978 979 980 981 982 983 984 985
    _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);
986
    if (!_loadTextMissionFile(stream, loadedVisualItems, errorStr)) {
987 988 989 990 991 992
        errorString = errorMessage.arg(errorStr);
        return false;
    }

    _initLoadedVisualItems(loadedVisualItems);

Don Gagne's avatar
Don Gagne committed
993
    return true;
994 995
}

996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007
bool MissionController::readyForSaveSend(void) const
{
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
        if (!visualItem->readyForSave()) {
            return false;
        }
    }

    return true;
}

1008
void MissionController::save(QJsonObject& json)
1009
{
1010
    json[JsonHelper::jsonVersionKey] = _missionFileVersion;
1011

1012
    // Mission settings
1013

1014 1015 1016 1017 1018 1019 1020
    MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
    if (!settingsItem) {
        qWarning() << "First item is not MissionSettingsItem";
        return;
    }
    QJsonValue coordinateValue;
    JsonHelper::saveGeoCoordinate(settingsItem->coordinate(), true /* writeAltitude */, coordinateValue);
1021 1022 1023 1024 1025
    json[_jsonPlannedHomePositionKey]   = coordinateValue;
    json[_jsonFirmwareTypeKey]          = _controllerVehicle->firmwareType();
    json[_jsonVehicleTypeKey]           = _controllerVehicle->vehicleType();
    json[_jsonCruiseSpeedKey]           = _controllerVehicle->defaultCruiseSpeed();
    json[_jsonHoverSpeedKey]            = _controllerVehicle->defaultHoverSpeed();
1026

1027
    // Save the visual items
1028

1029 1030 1031
    QJsonArray rgJsonMissionItems;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1032

1033 1034
        visualItem->save(rgJsonMissionItems);
    }
1035

1036 1037 1038
    // Mission settings has a special case for end mission action
    if (settingsItem) {
        QList<MissionItem*> rgMissionItems;
1039

1040 1041 1042 1043 1044
        if (_convertToMissionItems(_visualItems, rgMissionItems, this /* missionItemParent */)) {
            QJsonObject saveObject;
            MissionItem* missionItem = rgMissionItems[rgMissionItems.count() - 1];
            missionItem->save(saveObject);
            rgJsonMissionItems.append(saveObject);
1045
        }
1046 1047
        for (int i=0; i<rgMissionItems.count(); i++) {
            rgMissionItems[i]->deleteLater();
1048 1049 1050
        }
    }

1051
    json[_jsonItemsKey] = rgJsonMissionItems;
1052 1053
}

1054
void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
1055
{
Don Gagne's avatar
Don Gagne committed
1056
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
1057
    QGeoCoordinate  prevCoord =     prevItem->exitCoordinate();
1058 1059 1060 1061
    bool            distanceOk =    false;

    // Convert to fixed altitudes

1062
    distanceOk = true;
1063
    if (currentItem != _settingsItem && currentItem->coordinateHasRelativeAltitude()) {
1064 1065
        currentCoord.setAltitude(homeAlt + currentCoord.altitude());
    }
1066
    if (prevItem != _settingsItem && prevItem->exitCoordinateHasRelativeAltitude()) {
1067
        prevCoord.setAltitude(homeAlt + prevCoord.altitude());
1068 1069 1070
    }

    if (distanceOk) {
Don Gagne's avatar
Don Gagne committed
1071 1072 1073
        *altDifference = currentCoord.altitude() - prevCoord.altitude();
        *distance = prevCoord.distanceTo(currentCoord);
        *azimuth = prevCoord.azimuthTo(currentCoord);
1074
    } else {
Don Gagne's avatar
Don Gagne committed
1075
        *altDifference = 0.0;
1076
        *azimuth = 0.0;
1077
        *distance = 0.0;
1078 1079 1080
    }
}

1081
double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, VisualMissionItem* homeItem)
1082 1083 1084 1085 1086 1087 1088
{
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
    QGeoCoordinate  homeCoord =     homeItem->exitCoordinate();
    bool            distanceOk =    false;

    distanceOk = true;

1089
    return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0;
1090 1091
}

1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113
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;
    }
}

1114 1115
void MissionController::_recalcWaypointLines(void)
{
1116 1117 1118
    bool                firstCoordinateItem =   true;
    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));

1119
    bool homePositionValid = _settingsItem->coordinate().isValid();
1120

1121
    qCDebug(MissionControllerLog) << "_recalcWaypointLines homePositionValid" << homePositionValid;
1122

Nate Weibley's avatar
Nate Weibley committed
1123 1124
    CoordVectHashTable old_table = _linesTable;
    _linesTable.clear();
1125
    _waypointLines.clear();
1126
    _waypointPath.clear();
1127

1128 1129 1130 1131 1132 1133 1134 1135 1136
    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;
1137 1138 1139
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

1140 1141 1142 1143 1144 1145
        // 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;
1146
            }
1147 1148
        }