MissionController.cc 26.8 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 32 33 34
#ifndef __mobile__
#include "QGCFileDialog.h"
#endif

35 36
QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog")

37 38 39 40 41 42
const char* MissionController::_settingsGroup =                 "MissionController";
const char* MissionController::_jsonVersionKey =                "version";
const char* MissionController::_jsonGroundStationKey =          "groundStation";
const char* MissionController::_jsonMavAutopilotKey =           "MAV_AUTOPILOT";
const char* MissionController::_jsonItemsKey =                  "items";
const char* MissionController::_jsonPlannedHomePositionKey =    "plannedHomePosition";
43 44 45

MissionController::MissionController(QObject *parent)
    : QObject(parent)
46
    , _editMode(false)
47 48
    , _missionItems(NULL)
    , _activeVehicle(NULL)
49
    , _autoSync(false)
50
    , _firstItemsFromVehicle(false)
51 52
    , _missionItemsRequested(false)
    , _queuedSend(false)
53
{
54 55 56 57 58

}

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

60 61 62 63
}

void MissionController::start(bool editMode)
{
64 65
    qCDebug(MissionControllerLog) << "start editMode" << editMode;

66 67
    _editMode = editMode;

68
    MultiVehicleManager* multiVehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
69

70
    connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionController::_activeVehicleChanged);
71
    _activeVehicleChanged(multiVehicleMgr->activeVehicle());
72

73 74 75 76
    // We start with an empty mission
    _missionItems = new QmlObjectListModel(this);
    _addPlannedHomePosition(false /* addToCenter */);
    _initAllMissionItems();
77 78
}

79
// Called when new mission items have completed downloading from Vehicle
80
void MissionController::_newMissionItemsAvailableFromVehicle(void)
81
{
82 83
    qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle";

84 85 86 87 88 89 90 91
    if (!_editMode || _missionItemsRequested || _missionItems->count() == 1) {
        // 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
        _deinitAllMissionItems();
        _missionItems->deleteLater();
92

93 94
        _missionItems = _activeVehicle->missionManager()->copyMissionItems();
        qCDebug(MissionControllerLog) << "loading from vehicle count"<< _missionItems->count();
95

96 97
        if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _missionItems->count() == 0) {
            _addPlannedHomePosition(true /* addToCenter */);
98 99
        }

100
        _missionItemsRequested = false;
101

102 103
        _initAllMissionItems();
        emit newItemsFromVehicle();
104 105 106
    }
}

107
void MissionController::getMissionItems(void)
108
{
109
    Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
110

111 112 113 114 115 116 117 118
    if (activeVehicle) {
        _missionItemsRequested = true;
        activeVehicle->missionManager()->requestMissionItems();
    }
}

void MissionController::sendMissionItems(void)
{
119
    Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
120

121 122 123 124 125
    if (activeVehicle) {
        activeVehicle->missionManager()->writeMissionItems(*_missionItems);
        _missionItems->setDirty(false);
    }
}
126

127
int MissionController::insertMissionItem(QGeoCoordinate coordinate, int i)
128
{
129
    MissionItem * newItem = new MissionItem(_activeVehicle, this);
130 131 132
    newItem->setSequenceNumber(_missionItems->count());
    newItem->setCoordinate(coordinate);
    newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
133 134 135 136
    _initMissionItem(newItem);
    if (_missionItems->count() == 1) {
        newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
    }
137
    newItem->setDefaultsForCommand();
Don Gagne's avatar
Don Gagne committed
138
    if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) {
139 140 141 142 143 144 145 146
        double lastValue;

        if (_findLastAcceptanceRadius(&lastValue)) {
            newItem->setParam2(lastValue);
        }
        if (_findLastAltitude(&lastValue)) {
            newItem->setParam7(lastValue);
        }
147
    }
148
    _missionItems->insert(i, newItem);
149 150 151 152 153 154 155 156 157 158 159

    _recalcAll();

    return _missionItems->count() - 1;
}

void MissionController::removeMissionItem(int index)
{
    MissionItem* item = qobject_cast<MissionItem*>(_missionItems->removeAt(index));

    _deinitMissionItem(item);
160
    item->deleteLater();
161 162 163 164 165 166 167 168 169 170 171 172

    _recalcAll();

    // Set the new current item

    if (index >= _missionItems->count()) {
        index--;
    }
    for (int i=0; i<_missionItems->count(); i++) {
        MissionItem* item =  qobject_cast<MissionItem*>(_missionItems->get(i));
        item->setIsCurrentItem(i == index);
    }
173
    _missionItems->setDirty(true);
174 175
}

176 177 178
void MissionController::removeAllMissionItems(void)
{
    if (_missionItems) {
179 180 181
        while (_missionItems->count() != 1) {
            removeMissionItem(_missionItems->count() - 1);
        }
182
    }
183
}
184

185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285
#ifndef __mobile__
bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QString& errorString)
{
    QJsonParseError jsonParseError;
    QJsonDocument   jsonDoc(QJsonDocument::fromJson(bytes, &jsonParseError));

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

    QJsonObject json = jsonDoc.object();

    if (!json.contains(_jsonVersionKey)) {
        errorString = QStringLiteral("File is missing version key");
        return false;
    }
    if (json[_jsonVersionKey].toString() != "1.0") {
        errorString = QStringLiteral("QGroundControl does not support this file version");
        return false;
    }

    if (json.contains(_jsonItemsKey)) {
        if (!json[_jsonItemsKey].isArray()) {
            errorString = QStringLiteral("items values must be array");
            return false;
        }

        QJsonArray itemArray(json[_jsonItemsKey].toArray());
        foreach (const QJsonValue& itemValue, itemArray) {
            if (!itemValue.isObject()) {
                errorString = QStringLiteral("Mission item is not an object");
                return false;
            }

            MissionItem* item = new MissionItem(_activeVehicle, this);
            if (item->load(itemValue.toObject(), errorString)) {
                _missionItems->append(item);
            } else {
                return false;
            }
        }
    }

    if (json.contains(_jsonPlannedHomePositionKey)) {
        MissionItem* item = new MissionItem(_activeVehicle, this);

        if (item->load(json[_jsonPlannedHomePositionKey].toObject(), errorString)) {
            _missionItems->insert(0, item);
        } else {
            return false;
        }
    } else {
        _addPlannedHomePosition(true /* addToCenter */);
    }

    return true;
}
#endif

#ifndef __mobile__
bool MissionController::_loadTextMissionFile(QTextStream& stream, QString& errorString)
{
    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()) {
            MissionItem* item = new MissionItem(_activeVehicle, this);

            if (item->load(stream)) {
                _missionItems->append(item);
            } 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;
    }

    if (addPlannedHomePosition || _missionItems->count() == 0) {
        _addPlannedHomePosition(true /* addToCenter */);
    }

    return true;
286
}
287
#endif
288

289 290
void MissionController::loadMissionFromFile(void)
{
291
#ifndef __mobile__
292
    QString errorString;
293
    QString filename = QGCFileDialog::getOpenFileName(NULL, "Select Mission File to load", QString(), "Mission file (*.mission);;All Files (*.*)");
294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309

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

    if (_missionItems) {
        _deinitAllMissionItems();
        _missionItems->deleteLater();
    }
    _missionItems = new QmlObjectListModel(this);

    QFile file(filename);

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

313 314 315 316
        QString firstLine = stream.readLine();
        if (firstLine.contains(QRegExp("QGC.*WPL"))) {
            stream.seek(0);
            _loadTextMissionFile(stream, errorString);
317
        } else {
318
            _loadJsonMissionFile(bytes, errorString);
319 320
        }
    }
321

322
    if (!errorString.isEmpty()) {
323
        _missionItems->clear();
324
        qgcApp()->showMessage(errorString);
325 326
    }

327 328 329 330
    if (_missionItems->count() == 0) {
        _addPlannedHomePosition(true /* addToCenter */);
    }

331
    _initAllMissionItems();
332
#endif
333 334
}

335
void MissionController::saveMissionToFile(void)
336
{
337
#ifndef __mobile__
338
    QString filename = QGCFileDialog::getSaveFileName(NULL, "Select file to save mission to", QString(), "Mission file (*.mission);;All Files (*.*)");
339 340 341 342

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

344 345 346 347
    if (!QFileInfo(filename).fileName().contains(".")) {
        filename += ".mission";
    }

348
    QFile file(filename);
349

350
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
351
        qgcApp()->showMessage(file.errorString());
352
    } else {
353
        QJsonObject missionObject;
354

355 356
        missionObject[_jsonVersionKey] = "1.0";
        missionObject[_jsonGroundStationKey] = "QGroundControl";
357

358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374
        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();
        }
        missionObject[_jsonMavAutopilotKey] = firmwareType;

        QJsonObject homePositionObject;
        qobject_cast<MissionItem*>(_missionItems->get(0))->save(homePositionObject);
        missionObject["plannedHomePosition"] = homePositionObject;

        QJsonArray itemArray;
375
        for (int i=1; i<_missionItems->count(); i++) {
376 377 378
            QJsonObject itemObject;
            qobject_cast<MissionItem*>(_missionItems->get(i))->save(itemObject);
            itemArray.append(itemObject);
379
        }
380 381 382 383
        missionObject["items"] = itemArray;

        QJsonDocument saveDoc(missionObject);
        file.write(saveDoc.toJson());
384 385 386
    }

    _missionItems->setDirty(false);
387
#endif
388 389
}

390
void MissionController::_calcPrevWaypointValues(double homeAlt, MissionItem* currentItem, MissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
391
{
Don Gagne's avatar
Don Gagne committed
392 393
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
    QGeoCoordinate  prevCoord =     prevItem->coordinate();
394 395 396 397
    bool            distanceOk =    false;

    // Convert to fixed altitudes

398
    qCDebug(MissionControllerLog) << homeAlt
Don Gagne's avatar
Don Gagne committed
399 400
                                  << currentItem->relativeAltitude() << currentItem->coordinate().altitude()
                                  << prevItem->relativeAltitude() << prevItem->coordinate().altitude();
401

402 403 404 405 406 407
    distanceOk = true;
    if (currentItem->relativeAltitude()) {
        currentCoord.setAltitude(homeAlt + currentCoord.altitude());
    }
    if (prevItem->relativeAltitude()) {
        prevCoord.setAltitude(homeAlt + prevCoord.altitude());
408 409 410 411 412
    }

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

    if (distanceOk) {
Don Gagne's avatar
Don Gagne committed
413 414 415
        *altDifference = currentCoord.altitude() - prevCoord.altitude();
        *distance = prevCoord.distanceTo(currentCoord);
        *azimuth = prevCoord.azimuthTo(currentCoord);
416
    } else {
Don Gagne's avatar
Don Gagne committed
417
        *altDifference = 0.0;
418
        *azimuth = 0.0;
Don Gagne's avatar
Don Gagne committed
419
        *distance = -1.0;   // Signals no values
420 421 422
    }
}

423 424 425
void MissionController::_recalcWaypointLines(void)
{
    MissionItem*    lastCoordinateItem =    qobject_cast<MissionItem*>(_missionItems->get(0));
426
    MissionItem*    homeItem =              lastCoordinateItem;
427
    bool            firstCoordinateItem =   true;
428
    bool            showHomePosition =      homeItem->showHomePosition();
429 430 431 432 433 434 435
    double          homeAlt =               homeItem->coordinate().altitude();

    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.
436

437
    // No values for first item
438
    lastCoordinateItem->setAltDifference(0.0);
439
    lastCoordinateItem->setAzimuth(0.0);
440 441
    lastCoordinateItem->setDistance(-1.0);

442 443 444 445 446
    double minAltSeen = 0.0;
    double maxAltSeen = 0.0;
    double homePositionAltitude = homeItem->coordinate().altitude();
    minAltSeen = maxAltSeen = homeItem->coordinate().altitude();

447 448 449 450 451
    _waypointLines.clear();

    for (int i=1; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));

452 453 454
        // Assume the worst
        item->setAzimuth(0.0);
        item->setDistance(-1.0);
455

456 457
        if (item->specifiesCoordinate()) {
            double absoluteAltitude = item->coordinate().altitude();
458
            if (item->relativeAltitude()) {
459 460 461 462 463 464 465 466
                absoluteAltitude += homePositionAltitude;
            }
            minAltSeen = std::min(minAltSeen, absoluteAltitude);
            maxAltSeen = std::max(maxAltSeen, absoluteAltitude);

            if (!item->standaloneCoordinate()) {
                if (firstCoordinateItem) {
                    if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
467 468
                        // The first coordinate we hit is a takeoff command so link back to home position
                        if (showHomePosition) {
469 470 471
                            double azimuth, distance, altDifference;

                            _waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate()));
472
                            _calcPrevWaypointValues(homeAlt, item, homeItem, &azimuth, &distance, &altDifference);
473 474 475 476 477 478
                            item->setAltDifference(altDifference);
                            item->setAzimuth(azimuth);
                            item->setDistance(distance);
                        }
                    } else {
                        // First coordiante is not a takeoff command, it does not link backwards to anything
479
                    }
480
                    firstCoordinateItem = false;
481
                } else if (!lastCoordinateItem->homePosition() || showHomePosition) {
482 483 484 485
                    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
486
                    _calcPrevWaypointValues(homeAlt, item, lastCoordinateItem, &azimuth, &distance, &altDifference);
487 488 489 490
                    item->setAltDifference(altDifference);
                    item->setAzimuth(azimuth);
                    item->setDistance(distance);
                    _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
491
                }
492 493 494 495 496 497 498 499 500 501 502 503
                lastCoordinateItem = item;
            }
        }
    }

    // Walk the list again calculating altitude percentages
    double altRange = maxAltSeen - minAltSeen;
    for (int i=0; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));

        if (item->specifiesCoordinate()) {
            double absoluteAltitude = item->coordinate().altitude();
504
            if (item->relativeAltitude()) {
505 506 507 508 509 510
                absoluteAltitude += homePositionAltitude;
            }
            if (altRange == 0.0) {
                item->setAltPercent(0.0);
            } else {
                item->setAltPercent((absoluteAltitude - minAltSeen) / altRange);
511
            }
512 513
        }
    }
514 515

    emit waypointLinesChanged();
516 517
}

518 519 520 521 522 523 524 525 526 527 528
// This will update the sequence numbers to be sequential starting from 0
void MissionController::_recalcSequence(void)
{
    for (int i=0; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));

        // Setup ascending sequence numbers
        item->setSequenceNumber(i);
    }
}

529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548
// This will update the child item hierarchy
void MissionController::_recalcChildItems(void)
{
    MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0));

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

    for (int i=1; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));

        // Set up non-coordinate item child hierarchy
        if (item->specifiesCoordinate()) {
            item->childItems()->clear();
            currentParentItem = item;
        } else {
            currentParentItem->childItems()->append(item);
        }
    }
}

549 550
void MissionController::_recalcAll(void)
{
551
    _recalcSequence();
552 553 554 555
    _recalcChildItems();
    _recalcWaypointLines();
}

556
/// Initializes a new set of mission items
557 558
void MissionController::_initAllMissionItems(void)
{
559 560
    MissionItem* homeItem = NULL;

561
    homeItem = qobject_cast<MissionItem*>(_missionItems->get(0));
Don Gagne's avatar
Don Gagne committed
562
    homeItem->setHomePositionSpecialCase(true);
563
    homeItem->setShowHomePosition(_editMode);
Don Gagne's avatar
Don Gagne committed
564 565
    homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
    homeItem->setFrame(MAV_FRAME_GLOBAL);
566 567 568 569 570
    homeItem->setIsCurrentItem(true);

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

573
    qDebug() << "home item" << homeItem->coordinate();
DonLakeFlyer's avatar
DonLakeFlyer committed
574

575 576 577 578
    for (int i=0; i<_missionItems->count(); i++) {
        _initMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));
    }

579
    _recalcAll();
580

581
    emit missionItemsChanged();
582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602

    _missionItems->setDirty(false);

    connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged);
}

void MissionController::_deinitAllMissionItems(void)
{
    for (int i=0; i<_missionItems->count(); i++) {
        _deinitMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));
    }

    connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged);
}

void MissionController::_initMissionItem(MissionItem* item)
{
    _missionItems->setDirty(false);

    connect(item, &MissionItem::commandChanged,     this, &MissionController::_itemCommandChanged);
    connect(item, &MissionItem::coordinateChanged,  this, &MissionController::_itemCoordinateChanged);
603
    connect(item, &MissionItem::frameChanged,       this, &MissionController::_itemFrameChanged);
604 605 606 607 608 609
}

void MissionController::_deinitMissionItem(MissionItem* item)
{
    disconnect(item, &MissionItem::commandChanged,     this, &MissionController::_itemCommandChanged);
    disconnect(item, &MissionItem::coordinateChanged,  this, &MissionController::_itemCoordinateChanged);
610
    disconnect(item, &MissionItem::frameChanged,       this, &MissionController::_itemFrameChanged);
611 612 613 614 615 616 617 618
}

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

619 620 621 622 623 624
void MissionController::_itemFrameChanged(int frame)
{
    Q_UNUSED(frame);
    _recalcWaypointLines();
}

625 626 627 628 629
void MissionController::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command)
{
    Q_UNUSED(command);;
    _recalcChildItems();
    _recalcWaypointLines();
630 631 632 633
}

void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
{
634 635
    qCDebug(MissionControllerLog) << "_activeVehicleChanged activeVehicle" << activeVehicle;

636 637
    if (_activeVehicle) {
        MissionManager* missionManager = _activeVehicle->missionManager();
638

639
        disconnect(missionManager, &MissionManager::newMissionItemsAvailable,   this, &MissionController::_newMissionItemsAvailableFromVehicle);
640 641 642
        disconnect(missionManager, &MissionManager::inProgressChanged,          this, &MissionController::_inProgressChanged);
        disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged,      this, &MissionController::_activeVehicleHomePositionAvailableChanged);
        disconnect(_activeVehicle, &Vehicle::homePositionChanged,               this, &MissionController::_activeVehicleHomePositionChanged);
643
        _activeVehicle = NULL;
Don Gagne's avatar
Don Gagne committed
644 645
    }

646
    _activeVehicle = activeVehicle;
647

648 649
    if (_activeVehicle) {
        MissionManager* missionManager = activeVehicle->missionManager();
650

651
        connect(missionManager, &MissionManager::newMissionItemsAvailable,  this, &MissionController::_newMissionItemsAvailableFromVehicle);
652
        connect(missionManager, &MissionManager::inProgressChanged,         this, &MissionController::_inProgressChanged);
653 654 655
        connect(_activeVehicle, &Vehicle::homePositionAvailableChanged,     this, &MissionController::_activeVehicleHomePositionAvailableChanged);
        connect(_activeVehicle, &Vehicle::homePositionChanged,              this, &MissionController::_activeVehicleHomePositionChanged);

656 657 658
        if (!_editMode) {
            removeAllMissionItems();
        }
659 660 661 662 663 664 665 666

        _activeVehicleHomePositionChanged(_activeVehicle->homePosition());
        _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable());
    }
}

void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable)
{
667 668 669 670
    if (!_editMode && _missionItems) {
        qobject_cast<MissionItem*>(_missionItems->get(0))->setShowHomePosition(homePositionAvailable);
        _recalcWaypointLines();
    }
671 672 673 674
}

void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
{
675 676 677 678
    if (!_editMode && _missionItems) {
        qobject_cast<MissionItem*>(_missionItems->get(0))->setCoordinate(homePosition);
        _recalcWaypointLines();
    }
679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698
}

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) {
699
        Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717

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

void MissionController::_autoSyncSend(void)
{
    qDebug() << "Auto-syncing with vehicle";
    _queuedSend = false;
    if (_missionItems) {
        sendMissionItems();
        _missionItems->setDirty(false);
718 719
    }
}
720

721
void MissionController::_inProgressChanged(bool inProgress)
722
{
723 724 725 726
    if (!inProgress && _queuedSend) {
        _autoSyncSend();
    }
}
727

728 729 730
QmlObjectListModel* MissionController::missionItems(void)
{
    return _missionItems;
731
}
732

733
bool MissionController::_findLastAltitude(double* lastAltitude)
734
{
735 736
    bool found = false;
    double foundAltitude;
737 738 739 740

    for (int i=0; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));

741
        if (item->specifiesCoordinate() && !item->standaloneCoordinate()) {
742 743
            foundAltitude = item->param7();
            found = true;
744 745 746
        }
    }

747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772
    if (found) {
        *lastAltitude = foundAltitude;
    }

    return found;
}

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

    for (int i=0; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));

        if ((MAV_CMD)item->command() == MAV_CMD_NAV_WAYPOINT) {
            foundAcceptanceRadius = item->param2();
            found = true;
        }
    }

    if (found) {
        *lastAcceptanceRadius = foundAcceptanceRadius;
    }

    return found;
773
}
774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817

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
void MissionController::_addPlannedHomePosition(bool addToCenter)
{
    MissionItem* homeItem = new MissionItem(_activeVehicle, this);
    _missionItems->insert(0, homeItem);

    if (_missionItems->count() > 1  && addToCenter) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(1));

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

        for (int i=2; i<_missionItems->count(); i++) {
            item = qobject_cast<MissionItem*>(_missionItems->get(i));

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