MissionController.cc 89 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
#define UPDATE_TIMEOUT 5000 ///< How often we check for bounding box changes

36 37
QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog")

38
const char* MissionController::_settingsGroup =                 "MissionController";
Don Gagne's avatar
Don Gagne committed
39 40
const char* MissionController::_jsonFileTypeValue =             "Mission";
const char* MissionController::_jsonItemsKey =                  "items";
41
const char* MissionController::_jsonPlannedHomePositionKey =    "plannedHomePosition";
Don Gagne's avatar
Don Gagne committed
42
const char* MissionController::_jsonFirmwareTypeKey =           "firmwareType";
43 44 45
const char* MissionController::_jsonVehicleTypeKey =            "vehicleType";
const char* MissionController::_jsonCruiseSpeedKey =            "cruiseSpeed";
const char* MissionController::_jsonHoverSpeedKey =             "hoverSpeed";
Don Gagne's avatar
Don Gagne committed
46 47 48 49 50 51 52
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;
53

54 55 56
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"));
57

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

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

82 83
}

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

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

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

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

127 128
}

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

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

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

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

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

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

164 165 166
        _missionItemCount = newMissionItems.count();
        emit missionItemCountChanged(_missionItemCount);

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

183
        for (; i < newMissionItems.count(); i++) {
184
            const MissionItem* missionItem = newMissionItems[i];
185
            newControllerMissionItems->append(new SimpleMissionItem(_controllerVehicle, _flyView, *missionItem, this));
186 187 188
        }

        _deinitAllVisualItems();
Don Gagne's avatar
Don Gagne committed
189
        _visualItems->deleteLater();
190 191
        _settingsItem = nullptr;
        _visualItems  = nullptr;
192
        _updateContainsItems(); // This will clear containsItems which will be set again below. This will re-pop Start Mission confirmation.
193 194
        _visualItems = newControllerMissionItems;

195
        if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) {
196
            _addMissionSettings(_visualItems, !_flyView && _visualItems->count() > 0 /* addToCenter */);
197 198
        }

199
        MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle);
200

201
        _initAllVisualItems();
202
        _updateContainsItems();
203
        emit newItemsFromVehicle();
204
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
205
    _itemsRequested = false;
206 207
}

208
void MissionController::loadFromVehicle(void)
209
{
DonLakeFlyer's avatar
DonLakeFlyer committed
210 211 212 213 214 215 216 217
    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();
    }
218 219
}

220 221 222 223
void MissionController::_warnIfTerrainFrameUsed(void)
{
    for (int i=1; i<_visualItems->count(); i++) {
        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(i));
224
        if (simpleItem && simpleItem->altitudeMode() == QGroundControlQmlGlobal::AltitudeModeTerrainFrame) {
225 226 227 228 229 230
            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;
        }
    }
}

231
void MissionController::sendToVehicle(void)
232
{
DonLakeFlyer's avatar
DonLakeFlyer committed
233 234 235 236 237
    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
238
        qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle";
239
        _warnIfTerrainFrameUsed();
DonLakeFlyer's avatar
DonLakeFlyer committed
240 241 242 243 244 245 246 247 248
        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
249 250
}

251 252 253 254 255
/// 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
256 257 258 259
    if (visualMissionItems->count() == 0) {
        return false;
    }

260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275
    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
276
    MissionSettingsItem* settingsItem = visualMissionItems->value<MissionSettingsItem*>(0);
277 278 279 280 281 282 283
    if (settingsItem) {
        endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent);
    }

    return endActionSet;
}

284 285
void MissionController::convertToKMLDocument(QDomDocument& document)
{
286 287
    QObject*            deleteParent = new QObject();
    QList<MissionItem*> rgMissionItems;
288

289 290
    _convertToMissionItems(_visualItems, rgMissionItems, deleteParent);
    if (rgMissionItems.count() == 0) {
291 292
        return;
    }
293

294
    const double homePositionAltitude = _settingsItem->coordinate().altitude();
295 296 297 298 299

    QString coord;
    QStringList coords;
    // Drop home position
    bool dropPoint = true;
300
    for(const auto& item : rgMissionItems) {
301 302 303 304 305
        if(dropPoint) {
            dropPoint = false;
            continue;
        }
        const MissionCommandUIInfo* uiInfo = \
306
                qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command());
307 308

        if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) {
309
            double amslAltitude = item->param7() + (item->frame() == MAV_FRAME_GLOBAL ? 0 : homePositionAltitude);
yaoling's avatar
yaoling committed
310
            coord = QString::number(item->param6(),'f',7) \
311 312 313
                    + "," \
                    + QString::number(item->param5(),'f',7) \
                    + "," \
314
                    + QString::number(amslAltitude,'f',2);
315 316 317
            coords.append(coord);
        }
    }
318 319 320

    deleteParent->deleteLater();

321 322 323 324 325
    Kml kml;
    kml.points(coords);
    kml.save(document);
}

Don Gagne's avatar
Don Gagne committed
326 327 328
void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems)
{
    if (vehicle) {
329
        QList<MissionItem*> rgMissionItems;
330

331
        _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle);
332

333
        // PlanManager takes control of MissionItems so no need to delete
334
        vehicle->missionManager()->writeMissionItems(rgMissionItems);
335 336
    }
}
337

338 339 340 341 342 343
int MissionController::_nextSequenceNumber(void)
{
    if (_visualItems->count() == 0) {
        qWarning() << "Internal error: Empty visual item list";
        return 0;
    } else {
344 345
        VisualMissionItem* lastItem = _visualItems->value<VisualMissionItem*>(_visualItems->count() - 1);
        return lastItem->lastSequenceNumber() + 1;
346 347 348
    }
}

349
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
350
{
351
    int sequenceNumber = _nextSequenceNumber();
352
    SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
353
    newItem->setSequenceNumber(sequenceNumber);
354
    newItem->setCoordinate(coordinate);
355
    newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
356
    _initVisualItem(newItem);
357
    if (_visualItems->count() == 1 && (_controllerVehicle->fixedWing() || _controllerVehicle->vtol() || _controllerVehicle->multiRotor())) {
358
        MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF;
359
        if (_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
360 361
            newItem->setCommand(takeoffCmd);
        }
362
    }
363 364 365
    if (newItem->specifiesAltitude()) {
        double  prevAltitude;
        int     prevAltitudeMode;
366

367 368
        if (_findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) {
            newItem->altitude()->setRawValue(prevAltitude);
369
            newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
370
        }
371
    }
372
    newItem->setMissionFlightStatus(_missionFlightStatus);
373
    _visualItems->insert(i, newItem);
374

375 376
    // 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);
377

378
    return newItem->sequenceNumber();
379 380
}

381 382 383
int MissionController::insertROIMissionItem(QGeoCoordinate coordinate, int i)
{
    int sequenceNumber = _nextSequenceNumber();
384
    SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
385
    newItem->setSequenceNumber(sequenceNumber);
386
    newItem->setCommand((_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_DO_SET_ROI_LOCATION) ?
387 388
        MAV_CMD_DO_SET_ROI_LOCATION :
        MAV_CMD_DO_SET_ROI));
389
    _initVisualItem(newItem);
390
    newItem->setCoordinate(coordinate);
391

392 393
    double  prevAltitude;
    int     prevAltitudeMode;
394

395 396
    if (_findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) {
        newItem->altitude()->setRawValue(prevAltitude);
397
        newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
398 399 400 401 402 403 404 405
    }
    _visualItems->insert(i, newItem);

    _recalcAll();

    return newItem->sequenceNumber();
}

406
int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i)
407
{
408 409
    ComplexMissionItem* newItem;

410
    int sequenceNumber = _nextSequenceNumber();
411
    if (itemName == _surveyMissionItemName) {
412
        newItem = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
413
        newItem->setCoordinate(mapCenterCoordinate);
414
    } else if (itemName == patternFWLandingName) {
415
        newItem = new FixedWingLandingComplexItem(_controllerVehicle, _flyView, _visualItems /* parent */);
416
    } else if (itemName == patternStructureScanName) {
417
        newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
418
    } else if (itemName == patternCorridorScanName) {
419
        newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */);
420 421 422 423 424
    } else {
        qWarning() << "Internal error: Unknown complex item:" << itemName;
        return sequenceNumber;
    }

425 426 427
    return _insertComplexMissionItemWorker(newItem, i);
}

428
int MissionController::insertComplexMissionItemFromKMLOrSHP(QString itemName, QString file, int i)
429 430 431 432
{
    ComplexMissionItem* newItem;

    if (itemName == _surveyMissionItemName) {
433
        newItem = new SurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems);
434
    } else if (itemName == patternStructureScanName) {
435
        newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, file, _visualItems);
436
    } else if (itemName == patternCorridorScanName) {
437
        newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, file, _visualItems);
438 439 440 441 442 443 444 445 446 447 448
    } else {
        qWarning() << "Internal error: Unknown complex item:" << itemName;
        return _nextSequenceNumber();
    }

    return _insertComplexMissionItemWorker(newItem, i);
}

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

453
    if (surveyStyleItem) {
454
        bool rollSupported  = false;
455
        bool pitchSupported = false;
456
        bool yawSupported   = false;
457 458 459

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

460 461
        MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
        CameraSection* cameraSection = settingsItem->cameraSection();
462

463
        // Set camera to photo mode (leave alone if user already specified)
464
        if (cameraSection->cameraModeSupported() && !cameraSection->specifyCameraMode()) {
465
            cameraSection->setSpecifyCameraMode(true);
466
            cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE_SURVEY);
467
        }
468

469
        // Point gimbal straight down
470
        if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
471 472 473
            // If the user already specified a gimbal angle leave it alone
            if (!cameraSection->specifyGimbal()) {
                cameraSection->setSpecifyGimbal(true);
474
                cameraSection->gimbalPitch()->setRawValue(-90.0);
475 476
            }
        }
477
    }
478

479 480
    complexItem->setSequenceNumber(sequenceNumber);
    _initVisualItem(complexItem);
481

482 483 484 485 486
    if (i == -1) {
        _visualItems->append(complexItem);
    } else {
        _visualItems->insert(i, complexItem);
    }
487

488
    //-- Keep track of bounding box changes in complex items
489 490
    if(!complexItem->isSimpleItem()) {
        connect(complexItem, &ComplexMissionItem::boundingCubeChanged, this, &MissionController::_complexBoundingBoxChanged);
491
    }
492 493
    _recalcAll();

494
    return complexItem->sequenceNumber();
495 496
}

497 498
void MissionController::removeMissionItem(int index)
{
499 500 501 502 503
    if (index <= 0 || index >= _visualItems->count()) {
        qWarning() << "MissionController::removeMissionItem called with bad index - count:index" << _visualItems->count() << index;
        return;
    }

504
    bool removeSurveyStyle = _visualItems->value<SurveyComplexItem*>(index) || _visualItems->value<CorridorScanComplexItem*>(index);
505
    VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));
506

507
    _deinitVisualItem(item);
508
    item->deleteLater();
509

510 511
    if (removeSurveyStyle) {
        // Determine if the mission still has another survey style item in it
512 513
        bool foundSurvey = false;
        for (int i=1; i<_visualItems->count(); i++) {
514
            if (_visualItems->value<SurveyComplexItem*>(i) || _visualItems->value<CorridorScanComplexItem*>(i)) {
515 516 517 518 519
                foundSurvey = true;
                break;
            }
        }

520
        // If there is no longer a survey item in the mission remove added commands
521 522 523 524
        if (!foundSurvey) {
            bool rollSupported = false;
            bool pitchSupported = false;
            bool yawSupported = false;
525 526
            MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
            CameraSection* cameraSection = settingsItem->cameraSection();
527
            if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
528
                if (cameraSection->specifyGimbal() && cameraSection->gimbalPitch()->rawValue().toDouble() == -90.0 && cameraSection->gimbalYaw()->rawValue().toDouble() == 0.0) {
529 530 531
                    cameraSection->setSpecifyGimbal(false);
                }
            }
532
            if (cameraSection->cameraModeSupported() && cameraSection->specifyCameraMode() && cameraSection->cameraMode()->rawValue().toInt() == 0) {
533 534
                cameraSection->setSpecifyCameraMode(false);
            }
535 536 537
        }
    }

538
    _recalcAll();
539
    setDirty(true);
540 541
}

542
void MissionController::removeAll(void)
543
{
544 545
    if (_visualItems) {
        _deinitAllVisualItems();
DonLakeFlyer's avatar
DonLakeFlyer committed
546
        _visualItems->clearAndDeleteContents();
Don Gagne's avatar
Don Gagne committed
547
        _visualItems->deleteLater();
548
        _settingsItem = nullptr;
549
        _visualItems = new QmlObjectListModel(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
550
        _addMissionSettings(_visualItems, false /* addToCenter */);
551
        _initAllVisualItems();
552
        setDirty(true);
553
        _resetMissionFlightStatus();
Don Gagne's avatar
Don Gagne committed
554 555 556
    }
}

557
bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
558 559 560 561 562 563 564 565 566
{
    // 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)) {
567 568 569
        return false;
    }

570
    // Read complex items
571
    QList<SurveyComplexItem*> surveyItems;
572 573 574 575
    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];
576

577 578 579 580 581
        if (!itemValue.isObject()) {
            errorString = QStringLiteral("Mission item is not an object");
            return false;
        }

582
        SurveyComplexItem* item = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems /* parent */);
Don Gagne's avatar
Don Gagne committed
583 584
        const QJsonObject itemObject = itemValue.toObject();
        if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
585
            surveyItems.append(item);
586 587
        } else {
            return false;
588
        }
589
    }
590

591 592 593 594 595
    // 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
596
    QJsonArray itemArray(json[_jsonItemsKey].toArray());
597

598
    qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << surveyItems.count();
599 600 601 602
    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
603
        if (nextComplexItemIndex < surveyItems.count()) {
604
            SurveyComplexItem* complexItem = surveyItems[nextComplexItemIndex];
605 606 607 608 609 610 611 612 613 614 615 616 617 618

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

619 620 621 622 623
            if (!itemValue.isObject()) {
                errorString = QStringLiteral("Mission item is not an object");
                return false;
            }

Don Gagne's avatar
Don Gagne committed
624
            const QJsonObject itemObject = itemValue.toObject();
625
            SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems);
Don Gagne's avatar
Don Gagne committed
626
            if (item->load(itemObject, itemObject["id"].toInt(), errorString)) {
627
                qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber();
628
                nextSequenceNumber = item->lastSequenceNumber() + 1;
629
                visualItems->append(item);
630 631 632 633
            } else {
                return false;
            }
        }
634
    } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < surveyItems.count());
635 636

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

Don Gagne's avatar
Don Gagne committed
639
        if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) {
640
            MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems);
641 642 643
            settingsItem->setCoordinate(item->coordinate());
            visualItems->insert(0, settingsItem);
            item->deleteLater();
644 645 646 647
        } else {
            return false;
        }
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
648
        _addMissionSettings(visualItems, true /* addToCenter */);
649 650 651 652 653
    }

    return true;
}

654
bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString)
Don Gagne's avatar
Don Gagne committed
655 656 657 658 659 660
{
    // Validate root object keys
    QList<JsonHelper::KeyValidateInfo> rootKeyInfoList = {
        { _jsonPlannedHomePositionKey,      QJsonValue::Array,  true },
        { _jsonItemsKey,                    QJsonValue::Array,  true },
        { _jsonFirmwareTypeKey,             QJsonValue::Double, true },
661
        { _jsonVehicleTypeKey,              QJsonValue::Double, false },
662 663
        { _jsonCruiseSpeedKey,              QJsonValue::Double, false },
        { _jsonHoverSpeedKey,               QJsonValue::Double, false },
Don Gagne's avatar
Don Gagne committed
664 665 666 667 668 669 670
    };
    if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) {
        return false;
    }

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

671
    // Mission Settings
672
    AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings();
673

674 675
    if (_masterController->offline()) {
        // We only update if offline since if we are online we use the online vehicle settings
676
        appSettings->offlineEditingFirmwareType()->setRawValue(AppSettings::offlineEditingFirmwareTypeFromFirmwareType(static_cast<MAV_AUTOPILOT>(json[_jsonFirmwareTypeKey].toInt())));
677
        if (json.contains(_jsonVehicleTypeKey)) {
678
            appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType(static_cast<MAV_TYPE>(json[_jsonVehicleTypeKey].toInt())));
679
        }
680
    }
681
    if (json.contains(_jsonCruiseSpeedKey)) {
682
        appSettings->offlineEditingCruiseSpeed()->setRawValue(json[_jsonCruiseSpeedKey].toDouble());
683 684
    }
    if (json.contains(_jsonHoverSpeedKey)) {
685
        appSettings->offlineEditingHoverSpeed()->setRawValue(json[_jsonHoverSpeedKey].toDouble());
686 687
    }

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

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

820 821 822 823 824 825 826 827
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;
828 829 830 831 832 833
    JsonHelper::validateQGCJsonFile(json,
                                    _jsonFileTypeValue,    // expected file type
                                    1,                     // minimum supported version
                                    2,                     // maximum supported version
                                    fileVersion,
                                    errorString);
834 835

    if (fileVersion == 1) {
836
        return _loadJsonMissionFileV1(json, visualItems, errorString);
837
    } else {
838
        return _loadJsonMissionFileV2(json, visualItems, errorString);
839 840 841
    }
}

842
bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString)
843
{
844 845
    bool firstItem = true;
    bool plannedHomePositionInFile = false;
846 847 848 849 850 851 852 853 854

    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;
855
            plannedHomePositionInFile = true;
856 857 858
        } else if (version[2] == "120") {
            // Old QGC file, no planned home position
            versionOk = true;
859
            plannedHomePositionInFile = false;
860 861 862 863
        }
    }

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

868
        while (!stream.atEnd()) {
869
            SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems);
870 871

            if (item->load(stream)) {
872 873 874 875 876 877
                if (firstItem && plannedHomePositionInFile) {
                    settingsItem->setCoordinate(item->coordinate());
                } else {
                    visualItems->append(item);
                }
                firstItem = false;
878
            } else {
879
                errorString = tr("The mission file is corrupted.");
880 881 882 883
                return false;
            }
        }
    } else {
884
        errorString = tr("The mission file is not compatible with this version of %1.").arg(qgcApp()->applicationName());
885 886 887
        return false;
    }

888
    if (!plannedHomePositionInFile) {
889 890 891
        // 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));
892
            if (item && item->command() == MAV_CMD_DO_JUMP) {
893
                item->missionItem().setParam1(static_cast<int>(item->missionItem().param1()) + 1);
Don Gagne's avatar
Don Gagne committed
894 895
            }
        }
896 897 898
    }

    return true;
899 900
}

901
void MissionController::_initLoadedVisualItems(QmlObjectListModel* loadedVisualItems)
902
{
Don Gagne's avatar
Don Gagne committed
903 904 905
    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteLater();
906
        _settingsItem = nullptr;
Don Gagne's avatar
Don Gagne committed
907 908
    }

909
    _visualItems = loadedVisualItems;
Don Gagne's avatar
Don Gagne committed
910 911

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

915
    MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle);
916

Don Gagne's avatar
Don Gagne committed
917
    _initAllVisualItems();
918
}
919

920 921 922 923 924 925
bool MissionController::load(const QJsonObject& json, QString& errorString)
{
    QString errorStr;
    QString errorMessage = tr("Mission: %1");
    QmlObjectListModel* loadedVisualItems = new QmlObjectListModel(this);

926
    if (!_loadJsonMissionFileV2(json, loadedVisualItems, errorStr)) {
927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951
        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;
952
    }
953

954 955 956 957 958 959 960 961 962 963 964 965 966
    _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);
967
    if (!_loadTextMissionFile(stream, loadedVisualItems, errorStr)) {
968 969 970 971 972 973
        errorString = errorMessage.arg(errorStr);
        return false;
    }

    _initLoadedVisualItems(loadedVisualItems);

Don Gagne's avatar
Don Gagne committed
974
    return true;
975 976
}

977 978 979 980 981 982 983 984 985 986 987 988
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;
}

989
void MissionController::save(QJsonObject& json)
990
{
991
    json[JsonHelper::jsonVersionKey] = _missionFileVersion;
992

993
    // Mission settings
994

995 996 997 998 999 1000 1001
    MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
    if (!settingsItem) {
        qWarning() << "First item is not MissionSettingsItem";
        return;
    }
    QJsonValue coordinateValue;
    JsonHelper::saveGeoCoordinate(settingsItem->coordinate(), true /* writeAltitude */, coordinateValue);
1002 1003 1004 1005 1006
    json[_jsonPlannedHomePositionKey]   = coordinateValue;
    json[_jsonFirmwareTypeKey]          = _controllerVehicle->firmwareType();
    json[_jsonVehicleTypeKey]           = _controllerVehicle->vehicleType();
    json[_jsonCruiseSpeedKey]           = _controllerVehicle->defaultCruiseSpeed();
    json[_jsonHoverSpeedKey]            = _controllerVehicle->defaultHoverSpeed();
1007

1008
    // Save the visual items
1009

1010 1011 1012
    QJsonArray rgJsonMissionItems;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1013

1014 1015
        visualItem->save(rgJsonMissionItems);
    }
1016

1017 1018 1019
    // Mission settings has a special case for end mission action
    if (settingsItem) {
        QList<MissionItem*> rgMissionItems;
1020

1021 1022 1023 1024 1025
        if (_convertToMissionItems(_visualItems, rgMissionItems, this /* missionItemParent */)) {
            QJsonObject saveObject;
            MissionItem* missionItem = rgMissionItems[rgMissionItems.count() - 1];
            missionItem->save(saveObject);
            rgJsonMissionItems.append(saveObject);
1026
        }
1027 1028
        for (int i=0; i<rgMissionItems.count(); i++) {
            rgMissionItems[i]->deleteLater();
1029 1030 1031
        }
    }

1032
    json[_jsonItemsKey] = rgJsonMissionItems;
1033 1034
}

1035
void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
1036
{
Don Gagne's avatar
Don Gagne committed
1037
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
1038
    QGeoCoordinate  prevCoord =     prevItem->exitCoordinate();
1039 1040 1041 1042
    bool            distanceOk =    false;

    // Convert to fixed altitudes

1043
    distanceOk = true;
1044
    if (currentItem != _settingsItem && currentItem->coordinateHasRelativeAltitude()) {
1045 1046
        currentCoord.setAltitude(homeAlt + currentCoord.altitude());
    }
1047
    if (prevItem != _settingsItem && prevItem->exitCoordinateHasRelativeAltitude()) {
1048
        prevCoord.setAltitude(homeAlt + prevCoord.altitude());
1049 1050 1051
    }

    if (distanceOk) {
Don Gagne's avatar
Don Gagne committed
1052 1053 1054
        *altDifference = currentCoord.altitude() - prevCoord.altitude();
        *distance = prevCoord.distanceTo(currentCoord);
        *azimuth = prevCoord.azimuthTo(currentCoord);
1055
    } else {
Don Gagne's avatar
Don Gagne committed
1056
        *altDifference = 0.0;
1057
        *azimuth = 0.0;
1058
        *distance = 0.0;
1059 1060 1061
    }
}

1062
double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, VisualMissionItem* homeItem)
1063 1064 1065 1066 1067 1068 1069
{
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
    QGeoCoordinate  homeCoord =     homeItem->exitCoordinate();
    bool            distanceOk =    false;

    distanceOk = true;

1070
    return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0;
1071 1072
}

1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094
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;
    }
}

1095 1096
void MissionController::_recalcWaypointLines(void)
{
1097 1098 1099
    bool                firstCoordinateItem =   true;
    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));

1100
    bool homePositionValid = _settingsItem->coordinate().isValid();
1101

1102
    qCDebug(MissionControllerLog) << "_recalcWaypointLines homePositionValid" << homePositionValid;
1103

Nate Weibley's avatar
Nate Weibley committed
1104 1105
    CoordVectHashTable old_table = _linesTable;
    _linesTable.clear();
1106
    _waypointLines.clear();
1107
    _waypointPath.clear();
1108

1109 1110 1111 1112 1113 1114 1115 1116 1117
    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;
1118 1119 1120
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

1121 1122 1123 1124 1125 1126
        // 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;
1127
            }
1128 1129
        }

1130 1131 1132
        if (item->specifiesCoordinate() && !item->isStandaloneCoordinate()) {
            firstCoordinateItem = false;
            if (lastCoordinateItem != _settingsItem || (homePositionValid && linkStartToHome)) {
1133
                if (!_flyView) {
1134 1135
                    VisualItemPair pair(lastCoordinateItem, item);
                    _addWaypointLineSegment(old_table, pair);
1136 1137
                }
            }
1138 1139
            _waypointPath.append(QVariant::fromValue(item->coordinate()));
            lastCoordinateItem = item;
1140 1141
        }
    }
1142 1143 1144 1145 1146 1147

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

    if (linkEndToHome && lastCoordinateItem != _settingsItem && homePositionValid) {
1148
        if (!_flyView) {
1149 1150 1151 1152 1153
            VisualItemPair pair(lastCoordinateItem, _settingsItem);
            _addWaypointLineSegment(old_table, pair);
        } else {
            _waypointPath.append(QVariant::fromValue(_settingsItem->coordinate()));
        }
1154
    }
1155 1156 1157 1158

    {
        // Create a temporary QObjectList and replace the model data
        QObjectList objs;
Nate Weibley's avatar
Nate Weibley committed
1159
        objs.reserve(_linesTable.count());
1160
        for(CoordinateVector *vect: _linesTable.values()) {
1161 1162 1163 1164 1165 1166 1167 1168 1169 1170
            objs.append(vect);
        }

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

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

DonLakeFlyer's avatar
DonLakeFlyer committed
1171
    _recalcMissionFlightStatus();
1172

1173 1174 1175 1176 1177 1178 1179 1180
    if (_waypointPath.count() == 0) {
        // MapPolyLine has a bug where if you can from a path which has elements to an empty path the line drawn
        // is not cleared from the map. This hack works around that since it causes the previous lines to be remove
        // as then doesn't draw anything on the map.
        _waypointPath.append(QVariant::fromValue(QGeoCoordinate(0, 0)));
        _waypointPath.append(QVariant::fromValue(QGeoCoordinate(0, 0)));
    }

1181
    emit waypointLinesChanged();
1182
    emit waypointPathChanged();
1183 1184
}

1185 1186 1187 1188 1189 1190
void MissionController::_updateBatteryInfo(int waypointIndex)
{
    if (_missionFlightStatus.mAhBattery != 0) {
        _missionFlightStatus.hoverAmpsTotal = (_missionFlightStatus.hoverTime / 60.0) * _missionFlightStatus.hoverAmps;
        _missionFlightStatus.cruiseAmpsTotal = (_missionFlightStatus.cruiseTime / 60.0) * _missionFlightStatus.cruiseAmps;
        _missionFlightStatus.batteriesRequired = ceil((_missionFlightStatus.hoverAmpsTotal + _missionFlightStatus.cruiseAmpsTotal) / _missionFlightStatus.ampMinutesAvailable);
1191 1192 1193
        // FIXME: Battery change point code pretty much doesn't work. The reason is that is treats complex items as a black box. It needs to be able to look
        // inside complex items in order to determine a swap point that is interior to a complex item. Current the swap point display in PlanToolbar is
        // disabled to do this problem.
1194
        if (waypointIndex != -1 && _missionFlightStatus.batteriesRequired == 2 && _missionFlightStatus.batteryChangePoint == -1) {
1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217
            _missionFlightStatus.batteryChangePoint = waypointIndex - 1;
        }
    }
}

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

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

1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244
/// Adds the specified time to the appropriate hover or cruise time values.
///     @param vtolInHover true: vtol is currrent in hover mode
///     @param hoverTime    Amount of time tp add to hover
///     @param cruiseTime   Amount of time to add to cruise
///     @param extraTime    Amount of additional time to add to hover/cruise
///     @param seqNum       Sequence number of waypoint for these values, -1 for no waypoint associated
void MissionController::_addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum)
{
    if (_controllerVehicle->vtol()) {
        if (vtolInHover) {
            _addHoverTime(hoverTime, distance, seqNum);
            _addHoverTime(extraTime, 0, -1);
        } else {
            _addCruiseTime(cruiseTime, distance, seqNum);
            _addCruiseTime(extraTime, 0, -1);
        }
    } else {
        if (_controllerVehicle->multiRotor()) {
            _addHoverTime(hoverTime, distance, seqNum);
            _addHoverTime(extraTime, 0, -1);
        } else {
            _addCruiseTime(cruiseTime, distance, seqNum);
            _addCruiseTime(extraTime, 0, -1);
        }
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1245
void MissionController::_recalcMissionFlightStatus()
1246
{
1247
    if (!_visualItems->count()) {
1248
        return;
1249
    }
1250 1251 1252 1253

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

1254
    bool showHomePosition = _settingsItem->coordinate().isValid();
1255

DonLakeFlyer's avatar
DonLakeFlyer committed
1256
    qCDebug(MissionControllerLog) << "_recalcMissionFlightStatus";
1257

1258 1259 1260
    // If home position is valid we can calculate distances between all waypoints.
    // If home position is not valid we can only calculate distances between waypoints which are
    // both relative altitude.
1261

1262
    // No values for first item
1263
    lastCoordinateItem->setAltDifference(0.0);
1264
    lastCoordinateItem->setAzimuth(0.0);
1265
    lastCoordinateItem->setDistance(0.0);
1266

1267 1268
    double minAltSeen = 0.0;
    double maxAltSeen = 0.0;
1269 1270
    const double homePositionAltitude = _settingsItem->coordinate().altitude();
    minAltSeen = maxAltSeen = _settingsItem->coordinate().altitude();
1271

1272
    _resetMissionFlightStatus();
1273

DonLakeFlyer's avatar
DonLakeFlyer committed
1274
    bool vtolInHover = true;
1275
    bool linkStartToHome = false;
1276 1277 1278 1279 1280 1281 1282 1283 1284 1285
    bool linkEndToHome = false;

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

DonLakeFlyer's avatar
DonLakeFlyer committed
1287
    for (int i=0; i<_visualItems->count(); i++) {
1288
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1289 1290 1291
        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
        ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item);

1292 1293
        // Assume the worst
        item->setAzimuth(0.0);
1294
        item->setDistance(0.0);
1295

DonLakeFlyer's avatar
DonLakeFlyer committed
1296 1297 1298
        // Look for speed changed
        double newSpeed = item->specifiedFlightSpeed();
        if (!qIsNaN(newSpeed)) {
1299
            if (_controllerVehicle->multiRotor()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1300
                _missionFlightStatus.hoverSpeed = newSpeed;
1301
            } else if (_controllerVehicle->vtol()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1302 1303
                if (vtolInHover) {
                    _missionFlightStatus.hoverSpeed = newSpeed;
1304
                } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
1305
                    _missionFlightStatus.cruiseSpeed = newSpeed;
1306
                }
DonLakeFlyer's avatar
DonLakeFlyer committed
1307 1308
            } else {
                _missionFlightStatus.cruiseSpeed = newSpeed;
1309
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
1310 1311 1312 1313
            _missionFlightStatus.vehicleSpeed = newSpeed;
        }

        // Look for gimbal change
1314 1315 1316 1317 1318 1319 1320
        double gimbalYaw = item->specifiedGimbalYaw();
        if (!qIsNaN(gimbalYaw)) {
            _missionFlightStatus.gimbalYaw = gimbalYaw;
        }
        double gimbalPitch = item->specifiedGimbalPitch();
        if (!qIsNaN(gimbalPitch)) {
            _missionFlightStatus.gimbalPitch = gimbalPitch;
DonLakeFlyer's avatar
DonLakeFlyer committed
1321 1322 1323 1324 1325
        }

        if (i == 0) {
            // We only process speed and gimbal from Mission Settings item
            continue;
1326 1327
        }

1328
        // Link back to home if first item is takeoff and we have home position
1329
        if (firstCoordinateItem && simpleItem && (simpleItem->command() == MAV_CMD_NAV_TAKEOFF || simpleItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF)) {
1330
            if (showHomePosition) {
1331
                linkStartToHome = true;
1332 1333 1334 1335 1336 1337 1338
                if (_controllerVehicle->multiRotor() || _controllerVehicle->vtol()) {
                    // We have to special case takeoff, assuming vehicle takes off straight up to specified altitude
                    double azimuth, distance, altDifference;
                    _calcPrevWaypointValues(homePositionAltitude, _settingsItem, simpleItem, &azimuth, &distance, &altDifference);
                    double takeoffTime = qAbs(altDifference) / _appSettings->offlineEditingAscentSpeed()->rawValue().toDouble();
                    _addHoverTime(takeoffTime, 0, -1);
                }
1339 1340 1341 1342
            }
        }

        // Update VTOL state
1343
        if (simpleItem && _controllerVehicle->vtol()) {
1344
            switch (simpleItem->command()) {
1345
            case MAV_CMD_NAV_TAKEOFF:
1346 1347
                vtolInHover = false;
                break;
1348
            case MAV_CMD_NAV_VTOL_TAKEOFF:
DonLakeFlyer's avatar
DonLakeFlyer committed
1349 1350
                vtolInHover = true;
                break;
1351
            case MAV_CMD_NAV_LAND:
1352 1353
                vtolInHover = false;
                break;
1354
            case MAV_CMD_NAV_VTOL_LAND:
DonLakeFlyer's avatar
DonLakeFlyer committed
1355 1356
                vtolInHover = true;
                break;
1357
            case MAV_CMD_DO_VTOL_TRANSITION:
1358 1359 1360 1361 1362 1363
            {
                int transitionState = simpleItem->missionItem().param1();
                if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_MC) {
                    vtolInHover = true;
                } else if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_FW) {
                    vtolInHover = false;
1364 1365
                }
            }
1366 1367 1368
                break;
            default:
                break;
1369
            }
Don Gagne's avatar
Don Gagne committed
1370 1371
        }

1372
        _addTimeDistance(vtolInHover, 0, 0, item->additionalTimeDelay(), 0, -1);
1373

1374
        if (item->specifiesCoordinate()) {
1375 1376
            // Keep track of the min/max altitude for all waypoints so we can show altitudes as a percentage

1377
            double absoluteAltitude = item->coordinate().altitude();
1378
            if (item->coordinateHasRelativeAltitude()) {
1379 1380 1381 1382 1383
                absoluteAltitude += homePositionAltitude;
            }
            minAltSeen = std::min(minAltSeen, absoluteAltitude);
            maxAltSeen = std::max(maxAltSeen, absoluteAltitude);

1384 1385 1386 1387
            double terrainAltitude = item->terrainAltitude();
            if (!qIsNaN(terrainAltitude)) {
                minAltSeen = std::min(minAltSeen, terrainAltitude);
                maxAltSeen = std::max(maxAltSeen, terrainAltitude);
1388 1389 1390
            }

            if (!item->isStandaloneCoordinate()) {
Don Gagne's avatar
Don Gagne committed
1391
                firstCoordinateItem = false;
1392 1393 1394 1395 1396 1397 1398

                // Update vehicle yaw assuming direction to next waypoint
                if (item != lastCoordinateItem) {
                    _missionFlightStatus.vehicleYaw = lastCoordinateItem->exitCoordinate().azimuthTo(item->coordinate());
                    lastCoordinateItem->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);
                }

1399
                if (lastCoordinateItem != _settingsItem || linkStartToHome) {
1400 1401
                    // This is a subsequent waypoint or we are forcing the first waypoint back to home
                    double azimuth, distance, altDifference;
1402

1403
                    _calcPrevWaypointValues(homePositionAltitude, item, lastCoordinateItem, &azimuth, &distance, &altDifference);
1404 1405 1406
                    item->setAltDifference(altDifference);
                    item->setAzimuth(azimuth);
                    item->setDistance(distance);
1407

1408
                    _missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, _calcDistanceToHome(item, _settingsItem));
1409

DonLakeFlyer's avatar
DonLakeFlyer committed
1410 1411 1412
                    // Calculate time/distance
                    double hoverTime = distance / _missionFlightStatus.hoverSpeed;
                    double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
1413
                    _addTimeDistance(vtolInHover, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
1414
                }
DonLakeFlyer's avatar
DonLakeFlyer committed
1415

1416
                if (complexItem) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1417 1418
                    // Add in distance/time inside complex items as well
                    double distance = complexItem->complexDistance();
1419
                    _missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, complexItem->greatestDistanceTo(complexItem->exitCoordinate()));
DonLakeFlyer's avatar
DonLakeFlyer committed
1420

1421 1422
                    double hoverTime = distance / _missionFlightStatus.hoverSpeed;
                    double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
1423
                    _addTimeDistance(vtolInHover, hoverTime, cruiseTime, 0, distance, item->sequenceNumber());
1424
                }
DonLakeFlyer's avatar
DonLakeFlyer committed
1425 1426

                item->setMissionFlightStatus(_missionFlightStatus);
1427

1428 1429
                lastCoordinateItem = item;
            }
1430 1431
        }
    }
1432
    lastCoordinateItem->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);
1433

1434 1435 1436 1437 1438 1439 1440 1441
    if (linkEndToHome && lastCoordinateItem != _settingsItem) {
        double azimuth, distance, altDifference;
        _calcPrevWaypointValues(homePositionAltitude, lastCoordinateItem, _settingsItem, &azimuth, &distance, &altDifference);

        // Calculate time/distance
        double hoverTime = distance / _missionFlightStatus.hoverSpeed;
        double cruiseTime = distance / _missionFlightStatus.cruiseSpeed;
        double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble();
1442
        _addTimeDistance(vtolInHover, hoverTime, cruiseTime, distance, landTime, -1);
1443 1444
    }

1445 1446 1447
    if (_missionFlightStatus.mAhBattery != 0 && _missionFlightStatus.batteryChangePoint == -1) {
        _missionFlightStatus.batteryChangePoint = 0;
    }
1448

DonLakeFlyer's avatar
DonLakeFlyer committed
1449 1450 1451 1452 1453 1454 1455
    emit missionMaxTelemetryChanged(_missionFlightStatus.maxTelemetryDistance);
    emit missionDistanceChanged(_missionFlightStatus.totalDistance);
    emit missionHoverDistanceChanged(_missionFlightStatus.hoverDistance);
    emit missionCruiseDistanceChanged(_missionFlightStatus.cruiseDistance);
    emit missionTimeChanged();
    emit missionHoverTimeChanged();
    emit missionCruiseTimeChanged();
1456 1457
    emit batteryChangePointChanged(_missionFlightStatus.batteryChangePoint);
    emit batteriesRequiredChanged(_missionFlightStatus.batteriesRequired);
1458

1459 1460
    // Walk the list again calculating altitude percentages
    double altRange = maxAltSeen - minAltSeen;
1461 1462
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1463 1464 1465

        if (item->specifiesCoordinate()) {
            double absoluteAltitude = item->coordinate().altitude();
1466
            if (item->coordinateHasRelativeAltitude()) {
1467 1468 1469 1470
                absoluteAltitude += homePositionAltitude;
            }
            if (altRange == 0.0) {
                item->setAltPercent(0.0);
1471
                item->setTerrainPercent(qQNaN());
1472
                item->setTerrainCollision(false);
1473 1474
            } else {
                item->setAltPercent((absoluteAltitude - minAltSeen) / altRange);
1475 1476 1477 1478 1479 1480 1481
                double terrainAltitude = item->terrainAltitude();
                if (qIsNaN(terrainAltitude)) {
                    item->setTerrainPercent(qQNaN());
                    item->setTerrainCollision(false);
                } else {
                    item->setTerrainPercent((terrainAltitude - minAltSeen) / altRange);
                    item->setTerrainCollision(absoluteAltitude < terrainAltitude);
1482
                }
1483
            }
1484 1485
        }
    }
1486 1487

    _updateTimer.start(UPDATE_TIMEOUT);
1488 1489
}

1490 1491 1492
// This will update the sequence numbers to be sequential starting from 0
void MissionController::_recalcSequence(void)
{
1493 1494 1495 1496 1497
    if (_inRecalcSequence) {
        // Don't let this call recurse due to signalling
        return;
    }

1498 1499
    // Setup ascending sequence numbers for all visual items

1500
    _inRecalcSequence = true;
1501 1502 1503
    int sequenceNumber = 0;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1504 1505
        item->setSequenceNumber(sequenceNumber);
        sequenceNumber = item->lastSequenceNumber() + 1;
1506 1507
    }    
    _inRecalcSequence = false;
1508 1509
}

1510 1511 1512
// This will update the child item hierarchy
void MissionController::_recalcChildItems(void)
{
1513
    VisualMissionItem* currentParentItem = qobject_cast<VisualMissionItem*>(_visualItems->get(0));
1514 1515 1516

    currentParentItem->childItems()->clear();

1517 1518
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1519 1520 1521 1522 1523

        // Set up non-coordinate item child hierarchy
        if (item->specifiesCoordinate()) {
            item->childItems()->clear();
            currentParentItem = item;
1524
        } else if (item->isSimpleItem()) {
1525 1526 1527 1528 1529
            currentParentItem->childItems()->append(item);
        }
    }
}

1530
void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoordinate& clickCoordinate)
1531
{
1532 1533
    QGeoCoordinate firstCoordinate;

1534 1535 1536 1537
    if (_settingsItem->coordinate().isValid()) {
        return;
    }

1538
    // Set the planned home position to be a delta from first coordinate
1539 1540 1541 1542
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = _visualItems->value<VisualMissionItem*>(i);

        if (item->specifiesCoordinate()) {
1543 1544
            firstCoordinate = item->coordinate();
            break;
1545 1546 1547
        }
    }

1548 1549 1550 1551
    // No item specifying a coordinate was found, in this case it we have a clickCoordinate use that
    if (!firstCoordinate.isValid()) {
        firstCoordinate = clickCoordinate;
    }
1552

1553 1554 1555 1556 1557 1558 1559 1560 1561
    if (firstCoordinate.isValid()) {
        QGeoCoordinate plannedHomeCoord = firstCoordinate.atDistanceAndAzimuth(30, 0);
        plannedHomeCoord.setAltitude(0);
        _settingsItem->setCoordinate(plannedHomeCoord);
    }
}

/// @param clickCoordinate The location of the user click when inserting a new item
void MissionController::_recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate)
1562
{
1563
    if (!_flyView) {
1564
        _setPlannedHomePositionFromFirstCoordinate(clickCoordinate);
1565
    }
1566
    _recalcSequence();
1567 1568
    _recalcChildItems();
    _recalcWaypointLines();
1569
    _updateTimer.start(UPDATE_TIMEOUT);
1570 1571
}

1572 1573 1574 1575 1576 1577
void MissionController::_recalcAll(void)
{
    QGeoCoordinate emptyCoord;
    _recalcAllWithClickCoordinate(emptyCoord);
}

1578
/// Initializes a new set of mission items
1579
void MissionController::_initAllVisualItems(void)
1580
{
1581 1582
    // Setup home position at index 0

1583 1584 1585
    _settingsItem = qobject_cast<MissionSettingsItem*>(_visualItems->get(0));
    if (!_settingsItem) {
        qWarning() << "First item not MissionSettingsItem";
1586 1587
        return;
    }
1588
    if (!_flyView) {
1589 1590
        _settingsItem->setIsCurrentItem(true);
    }
1591

1592
    if (_managerVehicle->homePosition().isValid()) {
Don Gagne's avatar
Don Gagne committed
1593
        _settingsItem->setCoordinate(_managerVehicle->homePosition());
1594
    }
1595

1596 1597 1598
    connect(_settingsItem, &MissionSettingsItem::coordinateChanged,     this, &MissionController::_recalcAll);
    connect(_settingsItem, &MissionSettingsItem::missionEndRTLChanged,  this, &MissionController::_recalcAll);
    connect(_settingsItem, &MissionSettingsItem::coordinateChanged,     this, &MissionController::plannedHomePositionChanged);
1599

1600 1601 1602 1603
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
        _initVisualItem(item);
    }
1604

1605
    _recalcAll();
1606

1607
    connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_visualItemsDirtyChanged);
1608 1609 1610
    connect(_visualItems, &QmlObjectListModel::countChanged, this, &MissionController::_updateContainsItems);

    emit visualItemsChanged();
1611
    emit containsItemsChanged(containsItems());
1612
    emit plannedHomePositionChanged(plannedHomePosition());
1613

1614
    setDirty(false);
1615 1616
}

1617
void MissionController::_deinitAllVisualItems(void)
1618
{
1619 1620 1621
    disconnect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &MissionController::_recalcAll);
    disconnect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &MissionController::plannedHomePositionChanged);

1622 1623
    for (int i=0; i<_visualItems->count(); i++) {
        _deinitVisualItem(qobject_cast<VisualMissionItem*>(_visualItems->get(i)));
1624 1625
    }

1626
    disconnect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged);
1627
    disconnect(_visualItems, &QmlObjectListModel::countChanged, this, &MissionController::_updateContainsItems);
1628 1629
}

1630
void MissionController::_initVisualItem(VisualMissionItem* visualItem)
1631
{
1632
    setDirty(false);
1633

1634
    connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,                 this, &MissionController::_recalcWaypointLines);
1635 1636
    connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,       this, &MissionController::_recalcWaypointLines);
    connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,   this, &MissionController::_recalcWaypointLines);
DonLakeFlyer's avatar
DonLakeFlyer committed
1637 1638
    connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged,                this, &MissionController::_recalcMissionFlightStatus);
    connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged,                  this, &MissionController::_recalcMissionFlightStatus);
1639
    connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged,                this, &MissionController::_recalcMissionFlightStatus);
1640
    connect(visualItem, &VisualMissionItem::terrainAltitudeChanged,                     this, &MissionController::_recalcMissionFlightStatus);
1641
    connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged,                 this, &MissionController::_recalcMissionFlightStatus);
1642
    connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged,                  this, &MissionController::_recalcSequence);
1643

1644 1645 1646 1647 1648 1649 1650 1651
    if (visualItem->isSimpleItem()) {
        // We need to track commandChanged on simple item since recalc has special handling for takeoff command
        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
        if (simpleItem) {
            connect(&simpleItem->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
        } else {
            qWarning() << "isSimpleItem == true, yet not SimpleMissionItem";
        }
1652 1653
    } else {
        ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
1654
        if (complexItem) {
1655
            connect(complexItem, &ComplexMissionItem::complexDistanceChanged,       this, &MissionController::_recalcMissionFlightStatus);
1656
            connect(complexItem, &ComplexMissionItem::greatestDistanceToChanged,    this, &MissionController::_recalcMissionFlightStatus);
1657 1658 1659
        } else {
            qWarning() << "ComplexMissionItem not found";
        }
1660
    }
1661 1662
}

1663
void MissionController::_deinitVisualItem(VisualMissionItem* visualItem)
1664
{
1665 1666
    // Disconnect all signals
    disconnect(visualItem, 0, 0, 0);
1667 1668
}

1669
void MissionController::_itemCommandChanged(void)
1670
{
1671 1672
    _recalcChildItems();
    _recalcWaypointLines();
1673 1674
}

1675
void MissionController::managerVehicleChanged(Vehicle* managerVehicle)
1676
{
1677 1678 1679 1680 1681 1682
    if (_managerVehicle) {
        _missionManager->disconnect(this);
        _managerVehicle->disconnect(this);
        _managerVehicle = NULL;
        _missionManager = NULL;
    }
1683

1684 1685
    _managerVehicle = managerVehicle;
    if (!_managerVehicle) {
1686
        qWarning() << "MissionController::managerVehicleChanged managerVehicle=NULL";
1687 1688
        return;
    }
1689

1690 1691
    _missionManager = _managerVehicle->missionManager();
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle);
DonLakeFlyer's avatar
DonLakeFlyer committed
1692 1693
    connect(_missionManager, &MissionManager::sendComplete,             this, &MissionController::_managerSendComplete);
    connect(_missionManager, &MissionManager::removeAllComplete,        this, &MissionController::_managerRemoveAllComplete);
1694 1695 1696 1697 1698
    connect(_missionManager, &MissionManager::inProgressChanged,        this, &MissionController::_inProgressChanged);
    connect(_missionManager, &MissionManager::progressPct,              this, &MissionController::_progressPctChanged);
    connect(_missionManager, &MissionManager::currentIndexChanged,      this, &MissionController::_currentMissionIndexChanged);
    connect(_missionManager, &MissionManager::lastCurrentIndexChanged,  this, &MissionController::resumeMissionIndexChanged);
    connect(_missionManager, &MissionManager::resumeMissionReady,       this, &MissionController::resumeMissionReady);
1699
    connect(_missionManager, &MissionManager::resumeMissionUploadFail,  this, &MissionController::resumeMissionUploadFail);
1700 1701 1702 1703
    connect(_managerVehicle, &Vehicle::homePositionChanged,             this, &MissionController::_managerVehicleHomePositionChanged);
    connect(_managerVehicle, &Vehicle::defaultCruiseSpeedChanged,       this, &MissionController::_recalcMissionFlightStatus);
    connect(_managerVehicle, &Vehicle::defaultHoverSpeedChanged,        this, &MissionController::_recalcMissionFlightStatus);
    connect(_managerVehicle, &Vehicle::vehicleTypeChanged,              this, &MissionController::complexMissionItemNamesChanged);
1704

1705 1706
    if (!_masterController->offline()) {
        _managerVehicleHomePositionChanged(_managerVehicle->homePosition());
1707
    }
1708

1709
    emit complexMissionItemNamesChanged();
1710
    emit resumeMissionIndexChanged();
1711 1712
}

1713
void MissionController::_managerVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
1714
{
1715
    if (_visualItems) {
1716 1717
        bool currentDirtyBit = dirty();

1718
        MissionSettingsItem* settingsItem = qobject_cast<MissionSettingsItem*>(_visualItems->get(0));
1719
        if (settingsItem) {
1720
            settingsItem->setHomePositionFromVehicle(homePosition);
1721
        } else {
1722
            qWarning() << "First item is not MissionSettingsItem";
1723
        }
1724 1725 1726 1727 1728 1729

        if (!currentDirtyBit) {
            // Don't let this trip the dirty bit. Otherwise plan will keep getting marked dirty if vehicle home
            // changes.
            _visualItems->setDirty(false);
        }
1730
    }
1731 1732 1733
}

void MissionController::_inProgressChanged(bool inProgress)
1734
{
1735
    emit syncInProgressChanged(inProgress);
1736
}
1737

1738
bool MissionController::_findPreviousAltitude(int newIndex, double* prevAltitude, int* prevAltitudeMode)
1739
{
1740 1741
    bool        found = false;
    double      foundAltitude;
1742
    int         foundAltitudeMode;
1743

1744 1745 1746 1747 1748 1749
    if (newIndex > _visualItems->count()) {
        return false;
    }
    newIndex--;

    for (int i=newIndex; i>0; i--) {
1750
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1751

1752 1753 1754
        if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) {
            if (visualItem->isSimpleItem()) {
                SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
1755 1756 1757
                if (simpleItem->specifiesAltitude()) {
                    foundAltitude = simpleItem->altitude()->rawValue().toDouble();
                    foundAltitudeMode = simpleItem->altitudeMode();
1758
                    found = true;
1759
                    break;
1760 1761
                }
            }
1762 1763 1764
        }
    }

1765
    if (found) {
1766
        *prevAltitude = foundAltitude;
1767
        *prevAltitudeMode = foundAltitudeMode;
1768 1769 1770
    }

    return found;
1771
}
1772 1773 1774 1775 1776 1777 1778 1779 1780 1781 1782 1783 1784

double MissionController::_normalizeLat(double lat)
{
    // Normalize latitude to range: 0 to 180, S to N
    return lat + 90.0;
}

double MissionController::_normalizeLon(double lon)
{
    // Normalize longitude to range: 0 to 360, W to E
    return lon  + 180.0;
}

1785
/// Add the Mission Settings complex item to the front of the items
DonLakeFlyer's avatar
DonLakeFlyer committed
1786
void MissionController::_addMissionSettings(QmlObjectListModel* visualItems, bool addToCenter)
1787
{
1788
    MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems);
1789

Don Gagne's avatar
Don Gagne committed
1790 1791
    qCDebug(MissionControllerLog) << "_addMissionSettings addToCenter" << addToCenter;

1792
    visualItems->insert(0, settingsItem);
1793

1794 1795 1796 1797 1798 1799 1800 1801 1802 1803 1804 1805 1806 1807 1808 1809 1810 1811 1812 1813 1814 1815 1816 1817 1818
    if (addToCenter) {
        if (visualItems->count() > 1) {
            double north = 0.0;
            double south = 0.0;
            double east  = 0.0;
            double west  = 0.0;
            bool firstCoordSet = false;

            for (int i=1; i<visualItems->count(); i++) {
                VisualMissionItem* item = qobject_cast<VisualMissionItem*>(visualItems->get(i));
                if (item->specifiesCoordinate()) {
                    if (firstCoordSet) {
                        double lat = _normalizeLat(item->coordinate().latitude());
                        double lon = _normalizeLon(item->coordinate().longitude());
                        north = fmax(north, lat);
                        south = fmin(south, lat);
                        east  = fmax(east, lon);
                        west  = fmin(west, lon);
                    } else {
                        firstCoordSet = true;
                        north = _normalizeLat(item->coordinate().latitude());
                        south = north;
                        east  = _normalizeLon(item->coordinate().longitude());
                        west  = east;
                    }
1819 1820
                }
            }
1821

1822 1823 1824
            if (firstCoordSet) {
                settingsItem->setCoordinate(QGeoCoordinate((south + ((north - south) / 2)) - 90.0, (west + ((east - west) / 2)) - 180.0, 0.0));
            }
1825
        }
Don Gagne's avatar
Don Gagne committed
1826 1827
    } else if (_managerVehicle->homePosition().isValid()) {
        settingsItem->setCoordinate(_managerVehicle->homePosition());
1828 1829
    }
}
1830

1831
int MissionController::resumeMissionIndex(void) const
1832
{
1833

1834
    int resumeIndex = 0;
1835

1836
    if (_flyView) {
1837
        resumeIndex = _missionManager->lastCurrentIndex() + (_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 0 : 1);
1838
        if (resumeIndex > 1 && resumeIndex != _visualItems->value<VisualMissionItem*>(_visualItems->count() - 1)->sequenceNumber()) {
1839 1840
            // Resume at the item previous to the item we were heading towards
            resumeIndex--;
1841 1842
        } else {
            resumeIndex = 0;
1843 1844 1845 1846 1847
        }
    }

    return resumeIndex;
}
1848

1849 1850
int MissionController::currentMissionIndex(void) const
{
1851
    if (!_flyView) {
1852 1853 1854 1855 1856 1857 1858 1859 1860 1861
        return -1;
    } else {
        int currentIndex = _missionManager->currentIndex();
        if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
            currentIndex++;
        }
        return currentIndex;
    }
}

1862
void MissionController::_currentMissionIndexChanged(int sequenceNumber)
1863
{
1864
    if (_flyView) {
1865
        if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
1866 1867 1868
            sequenceNumber++;
        }

1869 1870
        for (int i=0; i<_visualItems->count(); i++) {
            VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1871 1872
            item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber);
        }
1873
        emit currentMissionIndexChanged(currentMissionIndex());
1874 1875
    }
}
1876

1877
bool MissionController::syncInProgress(void) const
1878
{
1879
    return _missionManager->inProgress();
1880 1881 1882 1883 1884 1885 1886 1887 1888 1889 1890 1891
}

bool MissionController::dirty(void) const
{
    return _visualItems ? _visualItems->dirty() : false;
}


void MissionController::setDirty(bool dirty)
{
    if (_visualItems) {
        _visualItems->setDirty(dirty);
1892
    }
1893
}
1894

1895 1896
void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle)
{
1897 1898
    // First we look for a Fixed Wing Landing Pattern which is at the end
    FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, vehicle);
1899

1900 1901 1902 1903 1904 1905
    int scanIndex = 0;
    while (scanIndex < visualItems->count()) {
        VisualMissionItem* visualItem = visualItems->value<VisualMissionItem*>(scanIndex);

        qCDebug(MissionControllerLog) << "MissionController::_scanForAdditionalSettings count:scanIndex" << visualItems->count() << scanIndex;

1906
        if (!_flyView) {
1907 1908 1909 1910 1911 1912
            MissionSettingsItem* settingsItem = qobject_cast<MissionSettingsItem*>(visualItem);
            if (settingsItem) {
                scanIndex++;
                settingsItem->scanForMissionSettings(visualItems, scanIndex);
                continue;
            }
1913 1914 1915
        }

        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
1916
        if (simpleItem) {
1917 1918 1919 1920 1921
            scanIndex++;
            simpleItem->scanForSections(visualItems, scanIndex, vehicle);
        } else {
            // Complex item, can't have sections
            scanIndex++;
1922 1923 1924
        }
    }
}
1925 1926 1927 1928 1929 1930 1931 1932 1933 1934 1935 1936 1937

void MissionController::_updateContainsItems(void)
{
    emit containsItemsChanged(containsItems());
}

bool MissionController::containsItems(void) const
{
    return _visualItems ? _visualItems->count() > 1 : false;
}

void MissionController::removeAllFromVehicle(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1938 1939 1940 1941 1942 1943 1944 1945
    if (_masterController->offline()) {
        qCWarning(MissionControllerLog) << "MissionControllerLog::removeAllFromVehicle called while offline";
    } else if (syncInProgress()) {
        qCWarning(MissionControllerLog) << "MissionControllerLog::removeAllFromVehicle called while syncInProgress";
    } else {
        _itemsRequested = true;
        _missionManager->removeAll();
    }
1946
}
1947 1948 1949 1950 1951 1952

QStringList MissionController::complexMissionItemNames(void) const
{
    QStringList complexItems;

    complexItems.append(_surveyMissionItemName);
1953
    complexItems.append(patternCorridorScanName);
1954
    if (_controllerVehicle->fixedWing()) {
1955
        complexItems.append(patternFWLandingName);
1956
    }
1957
    if (_controllerVehicle->multiRotor() || _controllerVehicle->vtol()) {
1958
        complexItems.append(patternStructureScanName);
1959
    }
1960

1961
    return qgcApp()->toolbox()->corePlugin()->complexMissionItemNames(_controllerVehicle, complexItems);
1962
}
1963

1964 1965
void MissionController::resumeMission(int resumeIndex)
{
1966
    if (!_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
1967 1968
        resumeIndex--;
    }
1969
    _missionManager->generateResumeMission(resumeIndex);
1970
}
1971 1972 1973 1974 1975 1976 1977 1978 1979

QGeoCoordinate MissionController::plannedHomePosition(void) const
{
    if (_settingsItem) {
        return _settingsItem->coordinate();
    } else {
        return QGeoCoordinate();
    }
}
DonLakeFlyer's avatar
DonLakeFlyer committed
1980 1981 1982 1983 1984 1985 1986 1987 1988 1989

void MissionController::applyDefaultMissionAltitude(void)
{
    double defaultAltitude = _appSettings->defaultMissionItemAltitude()->rawValue().toDouble();

    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = _visualItems->value<VisualMissionItem*>(i);
        item->applyNewAltitude(defaultAltitude);
    }
}
1990

1991 1992 1993 1994 1995 1996 1997
void MissionController::_progressPctChanged(double progressPct)
{
    if (!qFuzzyCompare(progressPct, _progressPct)) {
        _progressPct = progressPct;
        emit progressPctChanged(progressPct);
    }
}
1998 1999 2000 2001 2002 2003

void MissionController::_visualItemsDirtyChanged(bool dirty)
{
    // We could connect signal to signal and not need this but this is handy for setting a breakpoint on
    emit dirtyChanged(dirty);
}
DonLakeFlyer's avatar
DonLakeFlyer committed
2004 2005 2006

bool MissionController::showPlanFromManagerVehicle (void)
{
2007
    qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle _flyView" << _flyView;
DonLakeFlyer's avatar
DonLakeFlyer committed
2008 2009
    if (_masterController->offline()) {
        qCWarning(MissionControllerLog) << "MissionController::showPlanFromManagerVehicle called while offline";
Patrick José Pereira's avatar
Patrick José Pereira committed
2010
        return true;    // stops further propagation of showPlanFromManagerVehicle due to error
DonLakeFlyer's avatar
DonLakeFlyer committed
2011 2012 2013 2014 2015 2016 2017 2018 2019 2020 2021 2022 2023 2024 2025 2026 2027 2028 2029
    } else {
        if (!_managerVehicle->initialPlanRequestComplete()) {
            // The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically
            qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal";
            return true;
        } else if (syncInProgress()) {
            // If the sync is already in progress, newMissionItemsAvailable will be signalled automatically when it is done. So no need to do anything.
            qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal";
            return true;
        } else {
            // Fake a _newMissionItemsAvailable with the current items
            qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
            _itemsRequested = true;
            _newMissionItemsAvailableFromVehicle(false /* removeAllRequested */);
            return false;
        }
    }
}

2030
void MissionController::_managerSendComplete(bool error)
DonLakeFlyer's avatar
DonLakeFlyer committed
2031
{
2032
    // Fly view always reloads on send complete
2033
    if (!error && _flyView) {
DonLakeFlyer's avatar
DonLakeFlyer committed
2034 2035 2036 2037
        showPlanFromManagerVehicle();
    }
}

2038
void MissionController::_managerRemoveAllComplete(bool error)
DonLakeFlyer's avatar
DonLakeFlyer committed
2039
{
2040 2041 2042 2043
    if (!error) {
        // Remove all from vehicle so we always update
        showPlanFromManagerVehicle();
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
2044
}
2045 2046 2047 2048 2049 2050 2051 2052 2053 2054 2055 2056 2057 2058 2059 2060 2061 2062 2063 2064 2065 2066 2067 2068 2069 2070 2071 2072 2073 2074

int MissionController::currentPlanViewIndex(void) const
{
    return _currentPlanViewIndex;
}

VisualMissionItem* MissionController::currentPlanViewItem(void) const
{
    return _currentPlanViewItem;
}

void MissionController::setCurrentPlanViewIndex(int sequenceNumber, bool force)
{
    if(_visualItems && (force || sequenceNumber != _currentPlanViewIndex)) {
        _currentPlanViewItem  = NULL;
        _currentPlanViewIndex = -1;
        for (int i = 0; i < _visualItems->count(); i++) {
            VisualMissionItem* pVI = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
            if (pVI && pVI->sequenceNumber() == sequenceNumber) {
                pVI->setIsCurrentItem(true);
                _currentPlanViewItem  = pVI;
                _currentPlanViewIndex = sequenceNumber;
            } else {
                pVI->setIsCurrentItem(false);
            }
        }
        emit currentPlanViewIndexChanged();
        emit currentPlanViewItemChanged();
    }
}
2075 2076 2077

void MissionController::_updateTimeout()
{
Gus Grubba's avatar
Gus Grubba committed
2078
    QGeoCoordinate firstCoordinate;
2079
    QGeoCoordinate takeoffCoordinate;
2080
    QGCGeoBoundingCube boundingCube;
2081 2082 2083 2084
    double north = 0.0;
    double south = 180.0;
    double east  = 0.0;
    double west  = 360.0;
2085 2086
    double minAlt = QGCGeoBoundingCube::MaxAlt;
    double maxAlt = QGCGeoBoundingCube::MinAlt;
2087 2088 2089 2090 2091 2092
    for (int i = 1; i < _visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
        if(item->isSimpleItem()) {
            SimpleMissionItem* pSimpleItem = qobject_cast<SimpleMissionItem*>(item);
            if(pSimpleItem) {
                switch(pSimpleItem->command()) {
2093
                case MAV_CMD_NAV_TAKEOFF:
2094 2095 2096
                case MAV_CMD_NAV_WAYPOINT:
                case MAV_CMD_NAV_LAND:
                if(pSimpleItem->coordinate().isValid()) {
2097
                    if((MAV_CMD)pSimpleItem->command() == MAV_CMD_NAV_TAKEOFF) {
Gus Grubba's avatar
Gus Grubba committed
2098 2099 2100 2101
                        takeoffCoordinate = pSimpleItem->coordinate();
                    } else if(!firstCoordinate.isValid()) {
                        firstCoordinate = pSimpleItem->coordinate();
                    }
2102 2103
                    double lat = pSimpleItem->coordinate().latitude()  + 90.0;
                    double lon = pSimpleItem->coordinate().longitude() + 180.0;
2104
                    double alt = pSimpleItem->coordinate().altitude();
2105 2106 2107 2108
                    north  = fmax(north, lat);
                    south  = fmin(south, lat);
                    east   = fmax(east,  lon);
                    west   = fmin(west,  lon);
2109 2110
                    minAlt = fmin(minAlt, alt);
                    maxAlt = fmax(maxAlt, alt);
2111 2112 2113 2114 2115 2116 2117 2118 2119
                }
                break;
                default:
                    break;
                }
            }
        } else {
            ComplexMissionItem* pComplexItem = qobject_cast<ComplexMissionItem*>(item);
            if(pComplexItem) {
2120 2121
                QGCGeoBoundingCube bc = pComplexItem->boundingCube();
                if(bc.isValid()) {
Gus Grubba's avatar
Gus Grubba committed
2122 2123 2124
                    if(!firstCoordinate.isValid() && pComplexItem->coordinate().isValid()) {
                        firstCoordinate = pComplexItem->coordinate();
                    }
2125 2126 2127 2128 2129 2130
                    north  = fmax(north, bc.pointNW.latitude()  + 90.0);
                    south  = fmin(south, bc.pointSE.latitude()  + 90.0);
                    east   = fmax(east,  bc.pointNW.longitude() + 180.0);
                    west   = fmin(west,  bc.pointSE.longitude() + 180.0);
                    minAlt = fmin(minAlt, bc.pointNW.altitude());
                    maxAlt = fmax(maxAlt, bc.pointSE.altitude());
2131 2132 2133 2134
                }
            }
        }
    }
Gus Grubba's avatar
Gus Grubba committed
2135 2136 2137 2138 2139 2140 2141 2142 2143
    //-- Figure out where this thing is taking off from
    if(!takeoffCoordinate.isValid()) {
        if(firstCoordinate.isValid()) {
            takeoffCoordinate = firstCoordinate;
        } else {
            takeoffCoordinate = plannedHomePosition();
        }
    }
    //-- Build bounding "cube"
2144 2145 2146
    boundingCube = QGCGeoBoundingCube(
        QGeoCoordinate(north - 90.0, west - 180.0, minAlt),
        QGeoCoordinate(south - 90.0, east - 180.0, maxAlt));
2147 2148
    if(_travelBoundingCube != boundingCube || _takeoffCoordinate != takeoffCoordinate) {
        _takeoffCoordinate  = takeoffCoordinate;
2149
        _travelBoundingCube = boundingCube;
2150
        emit missionBoundingCubeChanged();
2151
        qCDebug(MissionControllerLog) << "Bounding cube:" << _travelBoundingCube.pointNW << _travelBoundingCube.pointSE;
2152 2153 2154 2155 2156 2157 2158
    }
}

void MissionController::_complexBoundingBoxChanged()
{
    _updateTimer.start(UPDATE_TIMEOUT);
}