MissionController.cc 38.6 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

#include "MissionController.h"
#include "MultiVehicleManager.h"
#include "MissionManager.h"
#include "CoordinateVector.h"
28
#include "FirmwarePlugin.h"
29
#include "QGCApplication.h"
30 31
#include "SimpleMissionItem.h"
#include "ComplexMissionItem.h"
32
#include "JsonHelper.h"
33

34 35 36 37
#ifndef __mobile__
#include "QGCFileDialog.h"
#endif

38 39
QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog")

40 41
const char* MissionController::jsonSimpleItemsKey = "items";

42 43 44 45
const char* MissionController::_settingsGroup =                 "MissionController";
const char* MissionController::_jsonVersionKey =                "version";
const char* MissionController::_jsonGroundStationKey =          "groundStation";
const char* MissionController::_jsonMavAutopilotKey =           "MAV_AUTOPILOT";
46
const char* MissionController::_jsonComplexItemsKey =           "complexItems";
47
const char* MissionController::_jsonPlannedHomePositionKey =    "plannedHomePosition";
48 49 50

MissionController::MissionController(QObject *parent)
    : QObject(parent)
51
    , _editMode(false)
52 53
    , _visualItems(NULL)
    , _complexItems(NULL)
54
    , _activeVehicle(NULL)
55
    , _autoSync(false)
56
    , _firstItemsFromVehicle(false)
57 58
    , _missionItemsRequested(false)
    , _queuedSend(false)
59
{
60 61 62 63 64

}

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

66 67 68 69
}

void MissionController::start(bool editMode)
{
70 71
    qCDebug(MissionControllerLog) << "start editMode" << editMode;

72 73
    _editMode = editMode;

74
    MultiVehicleManager* multiVehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
75

76
    connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionController::_activeVehicleChanged);
77
    _activeVehicleChanged(multiVehicleMgr->activeVehicle());
78

79
    // We start with an empty mission
80 81 82
    _visualItems = new QmlObjectListModel(this);
    _addPlannedHomePosition(_visualItems, false /* addToCenter */);
    _initAllVisualItems();
83 84
}

85
// Called when new mission items have completed downloading from Vehicle
86
void MissionController::_newMissionItemsAvailableFromVehicle(void)
87
{
88 89
    qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle";

90
    if (!_editMode || _missionItemsRequested || _visualItems->count() == 1) {
91 92 93 94 95
        // Fly Mode:
        //      - Always accepts new items fromthe vehicle so Fly view is kept up to date
        // Edit Mode:
        //      - Either a load from vehicle was manually requested or
        //      - The initial automatic load from a vehicle completed and the current editor it empty
96

97 98
        QmlObjectListModel* newControllerMissionItems = new QmlObjectListModel(this);
        const QList<MissionItem*>& newMissionItems = _activeVehicle->missionManager()->missionItems();
99

100 101 102 103 104 105 106 107 108 109 110 111
        qCDebug(MissionControllerLog) << "loading from vehicle: count"<< _visualItems->count();
        foreach(const MissionItem* missionItem, newMissionItems) {
            newControllerMissionItems->append(new SimpleMissionItem(_activeVehicle, *missionItem, this));
        }

        _deinitAllVisualItems();

        _visualItems->deleteListAndContents();
        _visualItems = newControllerMissionItems;

        if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) {
            _addPlannedHomePosition(_visualItems,true /* addToCenter */);
112 113
        }

114
        _missionItemsRequested = false;
115

116
        _initAllVisualItems();
117
        emit newItemsFromVehicle();
118 119 120
    }
}

121
void MissionController::getMissionItems(void)
122
{
123
    Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
124

125 126 127 128 129 130 131 132
    if (activeVehicle) {
        _missionItemsRequested = true;
        activeVehicle->missionManager()->requestMissionItems();
    }
}

void MissionController::sendMissionItems(void)
{
133 134 135 136 137 138 139
    if (_activeVehicle) {
        // Convert to MissionItems so we can send to vehicle
        QList<MissionItem*> missionItems;

        for (int i=0; i<_visualItems->count(); i++) {
            VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
            if (visualItem->isSimpleItem()) {
140
                missionItems.append(new MissionItem(qobject_cast<SimpleMissionItem*>(visualItem)->missionItem()));
141
            } else {
142
                ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
143 144 145
                QmlObjectListModel* complexMissionItems = complexItem->getMissionItems();
                for (int j=0; j<complexMissionItems->count(); j++) {
                    missionItems.append(new MissionItem(*qobject_cast<MissionItem*>(complexMissionItems->get(j))));
146
                }
147
                delete complexMissionItems;
148 149 150 151 152
            }
        }

        _activeVehicle->missionManager()->writeMissionItems(missionItems);
        _visualItems->setDirty(false);
153 154 155 156

        for (int i=0; i<missionItems.count(); i++) {
            delete missionItems[i];
        }
157 158
    }
}
159

160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175
int MissionController::_nextSequenceNumber(void)
{
    if (_visualItems->count() == 0) {
        qWarning() << "Internal error: Empty visual item list";
        return 0;
    } else {
        VisualMissionItem* lastItem = qobject_cast<VisualMissionItem*>(_visualItems->get(_visualItems->count() - 1));

        if (lastItem->isSimpleItem()) {
            return lastItem->sequenceNumber() + 1;
        } else {
            return qobject_cast<ComplexMissionItem*>(lastItem)->lastSequenceNumber() + 1;
        }
    }
}

176
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
177
{
178
    int sequenceNumber = _nextSequenceNumber();
179
    SimpleMissionItem * newItem = new SimpleMissionItem(_activeVehicle, this);
180
    newItem->setSequenceNumber(sequenceNumber);
181
    newItem->setCoordinate(coordinate);
182 183 184
    newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
    _initVisualItem(newItem);
    if (_visualItems->count() == 1) {
185 186
        newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
    }
187
    newItem->setDefaultsForCommand();
Don Gagne's avatar
Don Gagne committed
188
    if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) {
189 190 191
        double lastValue;

        if (_findLastAcceptanceRadius(&lastValue)) {
192
            newItem->missionItem().setParam2(lastValue);
193 194
        }
        if (_findLastAltitude(&lastValue)) {
195
            newItem->missionItem().setParam7(lastValue);
196
        }
197
    }
198
    _visualItems->insert(i, newItem);
199 200 201

    _recalcAll();

202
    return sequenceNumber;
203 204
}

205 206
int MissionController::insertComplexMissionItem(QGeoCoordinate coordinate, int i)
{
207
    int sequenceNumber = _nextSequenceNumber();
208
    ComplexMissionItem* newItem = new ComplexMissionItem(_activeVehicle, this);
209
    newItem->setSequenceNumber(sequenceNumber);
210
    newItem->setCoordinate(coordinate);
211
    _initVisualItem(newItem);
212

213 214
    _visualItems->insert(i, newItem);
    _complexItems->append(newItem);
215 216 217

    _recalcAll();

218
    return sequenceNumber;
219 220
}

221 222
void MissionController::removeMissionItem(int index)
{
223
    VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));
224

225 226 227 228 229 230 231 232 233
    _deinitVisualItem(item);
    if (!item->isSimpleItem()) {
        ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(_complexItems->removeOne(item));
        if (complexItem) {
            complexItem->deleteLater();
        } else {
            qWarning() << "Complex item missing";
        }
    }
234
    item->deleteLater();
235 236 237 238 239

    _recalcAll();

    // Set the new current item

240
    if (index >= _visualItems->count()) {
241 242
        index--;
    }
243
    for (int i=0; i<_visualItems->count(); i++) {
Don Gagne's avatar
Don Gagne committed
244
        VisualMissionItem* item =  qobject_cast<VisualMissionItem*>(_visualItems->get(i));
245 246
        item->setIsCurrentItem(i == index);
    }
247
    _visualItems->setDirty(true);
248 249
}

250 251
void MissionController::removeAllMissionItems(void)
{
252 253 254 255 256 257
    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteListAndContents();
        _visualItems = new QmlObjectListModel(this);
        _addPlannedHomePosition(_visualItems, false /* addToCenter */);
        _initAllVisualItems();
Don Gagne's avatar
Don Gagne committed
258
        _visualItems->setDirty(true);
259
    }
260
}
261

262
bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString)
263 264 265 266 267 268 269 270 271 272 273
{
    QJsonParseError jsonParseError;
    QJsonDocument   jsonDoc(QJsonDocument::fromJson(bytes, &jsonParseError));

    if (jsonParseError.error != QJsonParseError::NoError) {
        errorString = jsonParseError.errorString();
        return false;
    }

    QJsonObject json = jsonDoc.object();

274 275 276 277 278 279 280 281 282 283 284 285 286
    // Check for required keys
    QStringList requiredKeys;
    requiredKeys << _jsonVersionKey << _jsonPlannedHomePositionKey;
    if (!JsonHelper::validateRequiredKeys(json, requiredKeys, errorString)) {
        return false;
    }

    // Validate base key types
    QStringList             keyList;
    QList<QJsonValue::Type> typeList;
    keyList << jsonSimpleItemsKey << _jsonVersionKey << _jsonGroundStationKey << _jsonMavAutopilotKey << _jsonComplexItemsKey << _jsonPlannedHomePositionKey;
    typeList << QJsonValue::Array << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Array << QJsonValue::Object;
    if (!JsonHelper::validateKeyTypes(json, keyList, typeList, errorString)) {
287 288
        return false;
    }
289 290

    // Version check
291 292 293 294 295
    if (json[_jsonVersionKey].toString() != "1.0") {
        errorString = QStringLiteral("QGroundControl does not support this file version");
        return false;
    }

296 297 298 299 300
    // Read complex items
    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];
301

302 303 304 305 306 307 308 309 310 311 312
        if (!itemValue.isObject()) {
            errorString = QStringLiteral("Mission item is not an object");
            return false;
        }

        ComplexMissionItem* item = new ComplexMissionItem(_activeVehicle, this);
        if (item->load(itemValue.toObject(), errorString)) {
            qCDebug(MissionControllerLog) << "Json load: complex item start:stop" << item->sequenceNumber() << item->lastSequenceNumber();
            complexItems->append(item);
        } else {
            return false;
313
        }
314
    }
315

316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343
    // 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
    QJsonArray itemArray(json[jsonSimpleItemsKey].toArray());

    qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << complexItems->count();
    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
        if (nextComplexItemIndex < complexItems->count()) {
            ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(complexItems->get(nextComplexItemIndex));

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

344 345 346 347 348
            if (!itemValue.isObject()) {
                errorString = QStringLiteral("Mission item is not an object");
                return false;
            }

349
            SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this);
350
            if (item->load(itemValue.toObject(), errorString)) {
351 352
                qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber();
                visualItems->append(item);
353 354 355
            } else {
                return false;
            }
356 357

            nextSequenceNumber++;
358
        }
Don Gagne's avatar
Don Gagne committed
359
    } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < complexItems->count());
360 361

    if (json.contains(_jsonPlannedHomePositionKey)) {
362
        SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this);
363 364

        if (item->load(json[_jsonPlannedHomePositionKey].toObject(), errorString)) {
365
            visualItems->insert(0, item);
366 367 368 369
        } else {
            return false;
        }
    } else {
370
        _addPlannedHomePosition(visualItems, true /* addToCenter */);
371 372 373 374 375
    }

    return true;
}

376
bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString)
377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396
{
    bool addPlannedHomePosition = false;

    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;
        } else if (version[2] == "120") {
            // Old QGC file, no planned home position
            versionOk = true;
            addPlannedHomePosition = true;
        }
    }

    if (versionOk) {
        while (!stream.atEnd()) {
397
            SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this);
398 399

            if (item->load(stream)) {
400
                visualItems->append(item);
401 402 403 404 405 406 407 408 409 410
            } else {
                errorString = QStringLiteral("The mission file is corrupted.");
                return false;
            }
        }
    } else {
        errorString = QStringLiteral("The mission file is not compatible with this version of QGroundControl.");
        return false;
    }

411 412
    if (addPlannedHomePosition || visualItems->count() == 0) {
        _addPlannedHomePosition(visualItems, true /* addToCenter */);
Don Gagne's avatar
Don Gagne committed
413

414 415 416 417 418
        // Update sequence numbers in DO_JUMP commands to take into account added home position in index 0
        for (int i=1; i<visualItems->count(); i++) {
            SimpleMissionItem* item = qobject_cast<SimpleMissionItem*>(visualItems->get(i));
            if (item && item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) {
                item->missionItem().setParam1((int)item->missionItem().param1() + 1);
Don Gagne's avatar
Don Gagne committed
419 420
            }
        }
421 422 423
    }

    return true;
424 425
}

Don Gagne's avatar
Don Gagne committed
426
void MissionController::loadMissionFromFile(const QString& filename)
427 428 429 430 431 432 433
{
    QString errorString;

    if (filename.isEmpty()) {
        return;
    }

434 435
    QmlObjectListModel* newVisualItems = new QmlObjectListModel(this);
    QmlObjectListModel* newComplexItems = new QmlObjectListModel(this);
436 437 438 439 440 441

    QFile file(filename);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        errorString = file.errorString();
    } else {
442 443
        QByteArray  bytes = file.readAll();
        QTextStream stream(&bytes);
444

445 446 447
        QString firstLine = stream.readLine();
        if (firstLine.contains(QRegExp("QGC.*WPL"))) {
            stream.seek(0);
448
            _loadTextMissionFile(stream, newVisualItems, errorString);
449
        } else {
450
            _loadJsonMissionFile(bytes, newVisualItems, newComplexItems, errorString);
451 452
        }
    }
453

454
    if (!errorString.isEmpty()) {
455 456 457 458 459 460 461 462 463
        for (int i=0; i<newVisualItems->count(); i++) {
            newVisualItems->get(i)->deleteLater();
        }
        for (int i=0; i<newComplexItems->count(); i++) {
            newComplexItems->get(i)->deleteLater();
        }
        delete newVisualItems;
        delete newComplexItems;

464
        qgcApp()->showMessage(errorString);
465
        return;
466 467
    }

468 469 470
    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteListAndContents();
471
    }
472 473 474 475 476 477 478
    if (_complexItems) {
        _complexItems->deleteLater();
    }

    _visualItems = newVisualItems;
    _complexItems = newComplexItems;

479 480
    if (_visualItems->count() == 0) {
        _addPlannedHomePosition(_visualItems, true /* addToCenter */);
481 482
    }

483
    _initAllVisualItems();
484 485
}

Don Gagne's avatar
Don Gagne committed
486
void MissionController::loadMissionFromFilePicker(void)
487
{
488
#ifndef __mobile__
489 490 491 492 493
    QString filename = QGCFileDialog::getOpenFileName(NULL, "Select Mission File to load", QString(), "Mission file (*.mission);;All Files (*.*)");

    if (filename.isEmpty()) {
        return;
    }
Don Gagne's avatar
Don Gagne committed
494
    loadMissionFromFile(filename);
495 496 497
#endif
}

Don Gagne's avatar
Don Gagne committed
498
void MissionController::saveMissionToFile(const QString& filename)
499 500
{
    qDebug() << filename;
501 502 503 504

    if (filename.isEmpty()) {
        return;
    }
505

506
    QString missionFilename = filename;
507
    if (!QFileInfo(filename).fileName().contains(".")) {
Don Gagne's avatar
Don Gagne committed
508
        missionFilename += QString(".%1").arg(QGCApplication::missionFileExtension);
509 510
    }

511
    QFile file(missionFilename);
512

513
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
514
        qgcApp()->showMessage(file.errorString());
515
    } else {
516 517 518
        QJsonObject missionFileObject;      // top level json object
        QJsonArray  simpleItemsObject;
        QJsonArray  complexItemsObject;
519

520 521
        missionFileObject[_jsonVersionKey] =        "1.0";
        missionFileObject[_jsonGroundStationKey] =  "QGroundControl";
522

523 524 525 526 527 528 529 530 531 532
        MAV_AUTOPILOT firmwareType = MAV_AUTOPILOT_GENERIC;
        if (_activeVehicle) {
            firmwareType = _activeVehicle->firmwareType();
        } else {
            // FIXME: Hack duplicated code from QGroundControlQmlGlobal. Had to do this for now since
            // QGroundControlQmlGlobal is not available from C++ side.

            QSettings settings;
            firmwareType = (MAV_AUTOPILOT)settings.value("OfflineEditingFirmwareType", MAV_AUTOPILOT_ARDUPILOTMEGA).toInt();
        }
533
        missionFileObject[_jsonMavAutopilotKey] = firmwareType;
534

535
        // Save planned home position
536
        QJsonObject homePositionObject;
537 538 539 540 541 542 543
        SimpleMissionItem* homeItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));
        if (homeItem) {
            homeItem->missionItem().save(homePositionObject);
        } else {
            qgcApp()->showMessage(QStringLiteral("Internal error: VisualMissionItem at index 0 not SimpleMissionItem"));
            return;
        }
544
        missionFileObject[_jsonPlannedHomePositionKey] = homePositionObject;
545

546
        // Save the visual items
547
        for (int i=1; i<_visualItems->count(); i++) {
548 549
            QJsonObject itemObject;

550
            VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
551 552 553 554 555 556
            visualItem->save(itemObject);

            if (visualItem->isSimpleItem()) {
                simpleItemsObject.append(itemObject);
            } else {
                complexItemsObject.append(itemObject);
557
            }
558
        }
559

560 561 562 563
        missionFileObject[jsonSimpleItemsKey] = simpleItemsObject;
        missionFileObject[_jsonComplexItemsKey] = complexItemsObject;

        QJsonDocument saveDoc(missionFileObject);
564
        file.write(saveDoc.toJson());
565 566
    }

567
    _visualItems->setDirty(false);
568 569
}

Don Gagne's avatar
Don Gagne committed
570
void MissionController::saveMissionToFilePicker(void)
571 572 573 574 575 576 577
{
#ifndef __mobile__
    QString filename = QGCFileDialog::getSaveFileName(NULL, "Select file to save mission to", QString(), "Mission file (*.mission);;All Files (*.*)");

    if (filename.isEmpty()) {
        return;
    }
Don Gagne's avatar
Don Gagne committed
578
    saveMissionToFile(filename);
579
#endif
580 581
}

582
void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
583
{
Don Gagne's avatar
Don Gagne committed
584
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
585
    QGeoCoordinate  prevCoord =     prevItem->exitCoordinate();
586 587 588 589
    bool            distanceOk =    false;

    // Convert to fixed altitudes

590
    qCDebug(MissionControllerLog) << homeAlt
591 592
                                  << currentItem->coordinateHasRelativeAltitude() << currentItem->coordinate().altitude()
                                  << prevItem->exitCoordinateHasRelativeAltitude() << prevItem->exitCoordinate().altitude();
593

594
    distanceOk = true;
595
    if (currentItem->coordinateHasRelativeAltitude()) {
596 597
        currentCoord.setAltitude(homeAlt + currentCoord.altitude());
    }
598
    if (prevItem->exitCoordinateHasRelativeAltitude()) {
599
        prevCoord.setAltitude(homeAlt + prevCoord.altitude());
600 601 602 603 604
    }

    qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;

    if (distanceOk) {
Don Gagne's avatar
Don Gagne committed
605 606 607
        *altDifference = currentCoord.altitude() - prevCoord.altitude();
        *distance = prevCoord.distanceTo(currentCoord);
        *azimuth = prevCoord.azimuthTo(currentCoord);
608
    } else {
Don Gagne's avatar
Don Gagne committed
609
        *altDifference = 0.0;
610
        *azimuth = 0.0;
Don Gagne's avatar
Don Gagne committed
611
        *distance = -1.0;   // Signals no values
612 613 614
    }
}

615 616
void MissionController::_recalcWaypointLines(void)
{
617 618 619 620 621 622 623 624 625 626 627
    bool                firstCoordinateItem =   true;
    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));

    SimpleMissionItem*  homeItem = qobject_cast<SimpleMissionItem*>(lastCoordinateItem);

    if (!homeItem) {
        qWarning() << "Home item is not SimpleMissionItem";
    }

    bool    showHomePosition =  homeItem->showHomePosition();
    double  homeAlt =           homeItem->coordinate().altitude();
628 629 630 631 632 633

    qCDebug(MissionControllerLog) << "_recalcWaypointLines";

    // 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.
634

635
    // No values for first item
636
    lastCoordinateItem->setAltDifference(0.0);
637
    lastCoordinateItem->setAzimuth(0.0);
638 639
    lastCoordinateItem->setDistance(-1.0);

640 641 642 643 644
    double minAltSeen = 0.0;
    double maxAltSeen = 0.0;
    double homePositionAltitude = homeItem->coordinate().altitude();
    minAltSeen = maxAltSeen = homeItem->coordinate().altitude();

645 646
    _waypointLines.clear();

Don Gagne's avatar
Don Gagne committed
647
    bool linkBackToHome = false;
648 649
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
650

651 652 653
        // Assume the worst
        item->setAzimuth(0.0);
        item->setDistance(-1.0);
654

655 656 657 658
        // If we still haven't found the first coordinate item and we hit a a takeoff command link back to home
        if (firstCoordinateItem &&
                item->isSimpleItem() &&
                qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
Don Gagne's avatar
Don Gagne committed
659 660 661
            linkBackToHome = true;
        }

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

665
            double absoluteAltitude = item->coordinate().altitude();
666
            if (item->coordinateHasRelativeAltitude()) {
667 668 669 670 671
                absoluteAltitude += homePositionAltitude;
            }
            minAltSeen = std::min(minAltSeen, absoluteAltitude);
            maxAltSeen = std::max(maxAltSeen, absoluteAltitude);

672 673 674 675 676 677 678 679 680 681
            if (!item->exitCoordinateSameAsEntry()) {
                absoluteAltitude = item->exitCoordinate().altitude();
                if (item->exitCoordinateHasRelativeAltitude()) {
                    absoluteAltitude += homePositionAltitude;
                }
                minAltSeen = std::min(minAltSeen, absoluteAltitude);
                maxAltSeen = std::max(maxAltSeen, absoluteAltitude);
            }

            if (!item->isStandaloneCoordinate()) {
Don Gagne's avatar
Don Gagne committed
682
                firstCoordinateItem = false;
683
                if (lastCoordinateItem != homeItem || (showHomePosition && linkBackToHome)) {
684 685 686 687
                    double azimuth, distance, altDifference;

                    // Subsequent coordinate items link to last coordinate item. If the last coordinate item
                    // is an invalid home position we skip the line
688
                    _calcPrevWaypointValues(homeAlt, item, lastCoordinateItem, &azimuth, &distance, &altDifference);
689 690 691
                    item->setAltDifference(altDifference);
                    item->setAzimuth(azimuth);
                    item->setDistance(distance);
692
                    _waypointLines.append(new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate()));
693
                }
694 695 696 697 698 699 700
                lastCoordinateItem = item;
            }
        }
    }

    // Walk the list again calculating altitude percentages
    double altRange = maxAltSeen - minAltSeen;
701 702
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
703 704 705

        if (item->specifiesCoordinate()) {
            double absoluteAltitude = item->coordinate().altitude();
706
            if (item->coordinateHasRelativeAltitude()) {
707 708 709 710 711 712
                absoluteAltitude += homePositionAltitude;
            }
            if (altRange == 0.0) {
                item->setAltPercent(0.0);
            } else {
                item->setAltPercent((absoluteAltitude - minAltSeen) / altRange);
713
            }
714 715
        }
    }
716 717

    emit waypointLinesChanged();
718 719
}

720 721 722
// This will update the sequence numbers to be sequential starting from 0
void MissionController::_recalcSequence(void)
{
723 724 725 726 727 728 729 730 731
    // Setup ascending sequence numbers for all visual items

    int sequenceNumber = 0;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

        item->setSequenceNumber(sequenceNumber++);
        if (!item->isSimpleItem()) {
            ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item);
732

733
            if (complexItem) {
734
                 sequenceNumber = complexItem->lastSequenceNumber() + 1;
735 736 737 738
            } else {
                qWarning() << "isSimpleItem == false, yet not ComplexMissionItem";
            }
        }
739 740 741
    }
}

742 743 744
// This will update the child item hierarchy
void MissionController::_recalcChildItems(void)
{
745
    VisualMissionItem* currentParentItem = qobject_cast<VisualMissionItem*>(_visualItems->get(0));
746 747 748

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

749 750
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
751 752 753 754 755

        // Set up non-coordinate item child hierarchy
        if (item->specifiesCoordinate()) {
            item->childItems()->clear();
            currentParentItem = item;
756
        } else if (item->isSimpleItem()) {
757 758 759 760 761
            currentParentItem->childItems()->append(item);
        }
    }
}

762 763
void MissionController::_recalcAll(void)
{
764
    _recalcSequence();
765 766 767 768
    _recalcChildItems();
    _recalcWaypointLines();
}

769
/// Initializes a new set of mission items
770
void MissionController::_initAllVisualItems(void)
771
{
772 773 774 775 776 777 778 779 780
    SimpleMissionItem* homeItem = NULL;

    // Setup home position at index 0

    homeItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));
    if (!homeItem) {
        qWarning() << "homeItem not SimpleMissionItem";
        return;
    }
781

Don Gagne's avatar
Don Gagne committed
782
    homeItem->setHomePositionSpecialCase(true);
783
    homeItem->setShowHomePosition(_editMode);
784 785
    homeItem->missionItem().setCommand(MAV_CMD_NAV_WAYPOINT);
    homeItem->missionItem().setFrame(MAV_FRAME_GLOBAL);
786 787 788 789 790
    homeItem->setIsCurrentItem(true);

    if (!_editMode && _activeVehicle && _activeVehicle->homePositionAvailable()) {
        homeItem->setCoordinate(_activeVehicle->homePosition());
        homeItem->setShowHomePosition(true);
791
    }
792

793
    QmlObjectListModel* newComplexItems = new QmlObjectListModel(this);
794 795 796
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
        _initVisualItem(item);
797

798 799 800
        // Set up complex item list
        if (!item->isSimpleItem()) {
            ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item);
801

802 803 804 805 806
            if (complexItem) {
                newComplexItems->append(item);
            } else {
                qWarning() << "isSimpleItem == false, but not ComplexMissionItem";
            }
807
        }
808 809
    }

810 811 812 813
    if (_complexItems) {
        _complexItems->deleteLater();
    }
    _complexItems = newComplexItems;
814

815
    _recalcAll();
816

817 818
    emit visualItemsChanged();
    emit complexVisualItemsChanged();
819

820
    _visualItems->setDirty(false);
821

822
    connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged);
823 824
}

825
void MissionController::_deinitAllVisualItems(void)
826
{
827 828
    for (int i=0; i<_visualItems->count(); i++) {
        _deinitVisualItem(qobject_cast<VisualMissionItem*>(_visualItems->get(i)));
829 830
    }

831
    connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged);
832 833
}

834
void MissionController::_initVisualItem(VisualMissionItem* visualItem)
835
{
836 837 838
    _visualItems->setDirty(false);

    connect(visualItem, &VisualMissionItem::coordinateChanged,                          this, &MissionController::_itemCoordinateChanged);
839
    connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,                 this, &MissionController::_recalcWaypointLines);
840 841
    connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,       this, &MissionController::_recalcWaypointLines);
    connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,   this, &MissionController::_recalcWaypointLines);
842

843 844 845 846 847 848 849 850
    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";
        }
851 852 853 854
    } else {
        // We need to track changes of lastSequenceNumber so we can recalc sequence numbers for subsequence items
        ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
        connect(complexItem, &ComplexMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence);
855
    }
856 857
}

858
void MissionController::_deinitVisualItem(VisualMissionItem* visualItem)
859
{
860 861
    // Disconnect all signals
    disconnect(visualItem, 0, 0, 0);
862 863 864 865 866 867 868 869
}

void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
{
    Q_UNUSED(coordinate);
    _recalcWaypointLines();
}

870
void MissionController::_itemCommandChanged(void)
871
{
872 873
    _recalcChildItems();
    _recalcWaypointLines();
874 875 876 877
}

void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
{
878 879
    qCDebug(MissionControllerLog) << "_activeVehicleChanged activeVehicle" << activeVehicle;

880 881
    if (_activeVehicle) {
        MissionManager* missionManager = _activeVehicle->missionManager();
882

883
        disconnect(missionManager, &MissionManager::newMissionItemsAvailable,   this, &MissionController::_newMissionItemsAvailableFromVehicle);
884
        disconnect(missionManager, &MissionManager::inProgressChanged,          this, &MissionController::_inProgressChanged);
885
        disconnect(missionManager, &MissionManager::currentItemChanged,         this, &MissionController::_currentMissionItemChanged);
886 887
        disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged,      this, &MissionController::_activeVehicleHomePositionAvailableChanged);
        disconnect(_activeVehicle, &Vehicle::homePositionChanged,               this, &MissionController::_activeVehicleHomePositionChanged);
888
        _activeVehicle = NULL;
Don Gagne's avatar
Don Gagne committed
889 890
    }

891
    _activeVehicle = activeVehicle;
892

893 894
    if (_activeVehicle) {
        MissionManager* missionManager = activeVehicle->missionManager();
895

896
        connect(missionManager, &MissionManager::newMissionItemsAvailable,  this, &MissionController::_newMissionItemsAvailableFromVehicle);
897
        connect(missionManager, &MissionManager::inProgressChanged,         this, &MissionController::_inProgressChanged);
898
        connect(missionManager, &MissionManager::currentItemChanged,        this, &MissionController::_currentMissionItemChanged);
899 900 901
        connect(_activeVehicle, &Vehicle::homePositionAvailableChanged,     this, &MissionController::_activeVehicleHomePositionAvailableChanged);
        connect(_activeVehicle, &Vehicle::homePositionChanged,              this, &MissionController::_activeVehicleHomePositionChanged);

902 903 904
        if (!_editMode) {
            removeAllMissionItems();
        }
905 906 907 908

        _activeVehicleHomePositionChanged(_activeVehicle->homePosition());
        _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable());
    }
909 910 911

    // Whenever vehicle changes we need to update syncInProgress
    emit syncInProgressChanged(syncInProgress());
912 913 914 915
}

void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable)
{
916 917 918 919 920 921 922 923 924
    if (!_editMode && _visualItems) {
        SimpleMissionItem* homeItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));

        if (homeItem) {
            homeItem->setShowHomePosition(homePositionAvailable);
            _recalcWaypointLines();
        } else {
            qWarning() << "Unabled to cast home item to SimpleMissionItem";
        }
925
    }
926 927 928 929
}

void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
{
930 931
    if (!_editMode && _visualItems) {
        qobject_cast<VisualMissionItem*>(_visualItems->get(0))->setCoordinate(homePosition);
932 933
        _recalcWaypointLines();
    }
934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953
}

void MissionController::setAutoSync(bool autoSync)
{
    // FIXME: AutoSync temporarily turned off
#if 0
    _autoSync = autoSync;
    emit autoSyncChanged(_autoSync);

    if (_autoSync) {
        _dirtyChanged(true);
    }
#else
    Q_UNUSED(autoSync)
#endif
}

void MissionController::_dirtyChanged(bool dirty)
{
    if (dirty && _autoSync) {
954
        Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
955 956 957 958 959 960 961 962 963 964 965 966 967 968

        if (activeVehicle && !activeVehicle->armed()) {
            if (_activeVehicle->missionManager()->inProgress()) {
                _queuedSend = true;
            } else {
                _autoSyncSend();
            }
        }
    }
}

void MissionController::_autoSyncSend(void)
{
    _queuedSend = false;
969
    if (_visualItems) {
970
        sendMissionItems();
971
        _visualItems->setDirty(false);
972 973
    }
}
974

975
void MissionController::_inProgressChanged(bool inProgress)
976
{
977
    emit syncInProgressChanged(inProgress);
978 979 980 981
    if (!inProgress && _queuedSend) {
        _autoSyncSend();
    }
}
982

983
bool MissionController::_findLastAltitude(double* lastAltitude)
984
{
985 986
    bool found = false;
    double foundAltitude;
987

988
    // Don't use home position
989 990
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
991

992 993
        if (item->specifiesCoordinate() && !item->isStandaloneCoordinate()) {
            foundAltitude = item->exitCoordinate().altitude();
994
            found = true;
995 996 997
        }
    }

998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009
    if (found) {
        *lastAltitude = foundAltitude;
    }

    return found;
}

bool MissionController::_findLastAcceptanceRadius(double* lastAcceptanceRadius)
{
    bool found = false;
    double foundAcceptanceRadius;

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

1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023
        if (visualItem->isSimpleItem()) {
            SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);

            if (simpleItem) {
                if ((MAV_CMD)simpleItem->command() == MAV_CMD_NAV_WAYPOINT) {
                    foundAcceptanceRadius = simpleItem->missionItem().param2();
                    found = true;
                }
            } else {
                qWarning() << "isSimpleItem == true, yet not SimpleMissionItem";
            }
1024 1025 1026 1027 1028 1029 1030 1031
        }
    }

    if (found) {
        *lastAcceptanceRadius = foundAcceptanceRadius;
    }

    return found;
1032
}
1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046

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

/// Add the home position item to the front of the list
1047
void MissionController::_addPlannedHomePosition(QmlObjectListModel* visualItems, bool addToCenter)
1048
{
1049 1050
    SimpleMissionItem* homeItem = new SimpleMissionItem(_activeVehicle, this);
    visualItems->insert(0, homeItem);
1051

1052 1053
    if (visualItems->count() > 1  && addToCenter) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(visualItems->get(1));
1054 1055 1056 1057 1058 1059

        double north = _normalizeLat(item->coordinate().latitude());
        double south = north;
        double east = _normalizeLon(item->coordinate().longitude());
        double west = east;

1060 1061
        for (int i=2; i<visualItems->count(); i++) {
            item = qobject_cast<VisualMissionItem*>(visualItems->get(i));
1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076

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

        homeItem->setCoordinate(QGeoCoordinate((south + ((north - south) / 2)) - 90.0, (west + ((east - west) / 2)) - 180.0, 0.0));
    } else {
        homeItem->setCoordinate(qgcApp()->lastKnownHomePosition());
    }
}
1077 1078 1079 1080 1081 1082 1083 1084

void MissionController::_currentMissionItemChanged(int sequenceNumber)
{
    if (!_editMode) {
        if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
            sequenceNumber++;
        }

1085 1086
        for (int i=0; i<_visualItems->count(); i++) {
            VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
1087 1088 1089 1090
            item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber);
        }
    }
}
1091

1092 1093
bool MissionController::syncInProgress(void)
{
1094 1095 1096 1097 1098
    if (_activeVehicle) {
        return _activeVehicle->missionManager()->inProgress();
    } else {
        return false;
    }
1099
}