MissionManager.cc 36.1 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14 15

/// @file
///     @author Don Gagne <don@thegagnes.com>

#include "MissionManager.h"
#include "Vehicle.h"
16
#include "FirmwarePlugin.h"
Don Gagne's avatar
Don Gagne committed
17
#include "MAVLinkProtocol.h"
18
#include "QGCApplication.h"
Don Gagne's avatar
Don Gagne committed
19 20 21 22

QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")

MissionManager::MissionManager(Vehicle* vehicle)
23
    : _vehicle(vehicle)
24
    , _dedicatedLink(NULL)
Don Gagne's avatar
Don Gagne committed
25
    , _ackTimeoutTimer(NULL)
26
    , _expectedAck(AckNone)
27 28
    , _readTransactionInProgress(false)
    , _writeTransactionInProgress(false)
29
    , _resumeMission(false)
30 31
    , _currentMissionIndex(-1)
    , _lastCurrentIndex(-1)
Don Gagne's avatar
Don Gagne committed
32 33 34 35 36 37 38 39 40 41
{
    connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
    
    _ackTimeoutTimer = new QTimer(this);
    _ackTimeoutTimer->setSingleShot(true);
    _ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds);
    
    connect(_ackTimeoutTimer, &QTimer::timeout, this, &MissionManager::_ackTimeout);
}

42
MissionManager::~MissionManager()
Don Gagne's avatar
Don Gagne committed
43
{
44

Don Gagne's avatar
Don Gagne committed
45 46
}

47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
void MissionManager::_writeMissionItemsWorker(void)
{
    emit newMissionItemsAvailable(_missionItems.count() == 0);

    qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();

    // Prime write list
    for (int i=0; i<_missionItems.count(); i++) {
        _itemIndicesToWrite << i;
    }

    _writeTransactionInProgress = true;
    _retryCount = 0;
    emit inProgressChanged(true);
    _writeMissionCount();

63 64 65 66
    _currentMissionIndex = -1;
    _lastCurrentIndex = -1;
    emit currentIndexChanged(-1);
    emit lastCurrentIndexChanged(-1);
67 68 69
}


70
void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
Don Gagne's avatar
Don Gagne committed
71
{
72 73 74 75
    if (_vehicle->isOfflineEditingVehicle()) {
        return;
    }

76 77 78 79 80
    if (inProgress()) {
        qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress";
        return;
    }

81 82
    bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();

83
    _clearAndDeleteMissionItems();
84 85

    int firstIndex = skipFirstItem ? 1 : 0;
86

87
    for (int i=firstIndex; i<missionItems.count(); i++) {
88 89
        MissionItem* item = new MissionItem(*missionItems[i]);
        _missionItems.append(item);
90

91
        item->setIsCurrentItem(i == firstIndex);
92

93
        if (skipFirstItem) {
94
            // Home is in sequence 0, remainder of items start at sequence 1
95
            item->setSequenceNumber(item->sequenceNumber() - 1);
Don Gagne's avatar
Don Gagne committed
96
            if (item->command() == MAV_CMD_DO_JUMP) {
97 98
                item->setParam1((int)item->param1() - 1);
            }
Don Gagne's avatar
Don Gagne committed
99 100
        }
    }
101

102
    _writeMissionItemsWorker();
103 104 105 106 107
}

/// This begins the write sequence with the vehicle. This may be called during a retry.
void MissionManager::_writeMissionCount(void)
{
108
    qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _missionItems.count() << _retryCount;
109

110 111
    mavlink_message_t       message;
    mavlink_mission_count_t missionCount;
112

113 114 115
    missionCount.target_system = _vehicle->id();
    missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
    missionCount.count = _missionItems.count();
116

117
    _dedicatedLink = _vehicle->priorityLink();
118 119 120 121 122 123
    mavlink_msg_mission_count_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                          qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                          _dedicatedLink->mavlinkChannel(),
                                          &message,
                                          &missionCount);

124
    _vehicle->sendMessageOnLink(_dedicatedLink, message);
125 126 127
    _startAckTimeout(AckMissionRequest);
}

Don Gagne's avatar
Don Gagne committed
128 129 130 131 132 133 134 135 136 137 138 139 140
void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly)
{
    if (inProgress()) {
        qCDebug(MissionManagerLog) << "writeArduPilotGuidedMissionItem called while transaction in progress";
        return;
    }

    _writeTransactionInProgress = true;

    mavlink_message_t       messageOut;
    mavlink_mission_item_t  missionItem;

    missionItem.target_system =     _vehicle->id();
141
    missionItem.target_component =  _vehicle->defaultComponentId();
Don Gagne's avatar
Don Gagne committed
142 143 144 145 146 147 148 149 150 151 152 153 154 155
    missionItem.seq =               0;
    missionItem.command =           MAV_CMD_NAV_WAYPOINT;
    missionItem.param1 =            0;
    missionItem.param2 =            0;
    missionItem.param3 =            0;
    missionItem.param4 =            0;
    missionItem.x =                 gotoCoord.latitude();
    missionItem.y =                 gotoCoord.longitude();
    missionItem.z =                 gotoCoord.altitude();
    missionItem.frame =             MAV_FRAME_GLOBAL_RELATIVE_ALT;
    missionItem.current =           altChangeOnly ? 3 : 2;
    missionItem.autocontinue =      true;

    _dedicatedLink = _vehicle->priorityLink();
156 157 158 159 160 161
    mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                         qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                         _dedicatedLink->mavlinkChannel(),
                                         &messageOut,
                                         &missionItem);

Don Gagne's avatar
Don Gagne committed
162 163 164 165 166
    _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
    _startAckTimeout(AckGuidedItem);
    emit inProgressChanged(true);
}

167
void MissionManager::requestMissionItems(void)
Don Gagne's avatar
Don Gagne committed
168
{
169 170 171 172
    if (_vehicle->isOfflineEditingVehicle()) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
173
    qCDebug(MissionManagerLog) << "requestMissionItems read sequence";
174 175 176 177 178

    if (inProgress()) {
        qCDebug(MissionManagerLog) << "requestMissionItems called while transaction in progress";
        return;
    }
179 180

    _retryCount = 0;
Don Gagne's avatar
Don Gagne committed
181
    _readTransactionInProgress = true;
182 183 184 185 186 187 188 189 190
    emit inProgressChanged(true);
    _requestList();
}

/// Internal call to request list of mission items. May be called during a retry sequence.
void MissionManager::_requestList(void)
{
    qCDebug(MissionManagerLog) << "_requestList retry count" << _retryCount;

Don Gagne's avatar
Don Gagne committed
191 192
    mavlink_message_t               message;
    mavlink_mission_request_list_t  request;
193

194
    _itemIndicesToRead.clear();
Don Gagne's avatar
Don Gagne committed
195
    _clearMissionItems();
196

Don Gagne's avatar
Don Gagne committed
197 198
    request.target_system = _vehicle->id();
    request.target_component = MAV_COMP_ID_MISSIONPLANNER;
199

200
    _dedicatedLink = _vehicle->priorityLink();
201 202 203 204 205
    mavlink_msg_mission_request_list_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                                 qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                                 _dedicatedLink->mavlinkChannel(),
                                                 &message,
                                                 &request);
206

207
    _vehicle->sendMessageOnLink(_dedicatedLink, message);
208 209 210
    _startAckTimeout(AckMissionCount);
}

Don Gagne's avatar
Don Gagne committed
211 212
void MissionManager::_ackTimeout(void)
{
213
    if (_expectedAck == AckNone) {
Don Gagne's avatar
Don Gagne committed
214 215
        return;
    }
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

    switch (_expectedAck) {
    case AckNone:
        qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
        _sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone");
        break;
    case AckMissionCount:
        // MISSION_COUNT message expected
        if (_retryCount > _maxRetryCount) {
            _sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded."));
            _finishTransaction(false);
        } else {
            _retryCount++;
            qCDebug(MissionManagerLog) << "Retrying REQUEST_LIST retry Count" << _retryCount;
            _requestList();
        }
        break;
    case AckMissionItem:
        // MISSION_ITEM expected
        if (_retryCount > _maxRetryCount) {
            _sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded."));
            _finishTransaction(false);
        } else {
            _retryCount++;
            qCDebug(MissionManagerLog) << "Retrying MISSION_REQUEST retry Count" << _retryCount;
            _requestNextMissionItem();
        }
        break;
    case AckMissionRequest:
        // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
        if (_itemIndicesToWrite.count() == 0) {
            // Vehicle did not send final MISSION_ACK at end of sequence
            _sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack."));
            _finishTransaction(false);
        } else if (_itemIndicesToWrite[0] == 0) {
            // Vehicle did not respond to MISSION_COUNT, try again
            if (_retryCount > _maxRetryCount) {
                _sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded."));
                _finishTransaction(false);
            } else {
                _retryCount++;
                qCDebug(MissionManagerLog) << "Retrying MISSION_COUNT retry Count" << _retryCount;
                _writeMissionCount();
            }
        } else {
            // Vehicle did not request all items from ground station
            _sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck)));
            _expectedAck = AckNone;
            _finishTransaction(false);
        }
        break;
    case AckGuidedItem:
        // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
    default:
        _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck)));
        _expectedAck = AckNone;
        _finishTransaction(false);
    }
Don Gagne's avatar
Don Gagne committed
274 275
}

276
void MissionManager::_startAckTimeout(AckType_t ack)
Don Gagne's avatar
Don Gagne committed
277
{
278
    _expectedAck = ack;
Don Gagne's avatar
Don Gagne committed
279 280 281
    _ackTimeoutTimer->start();
}

282 283 284
/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped.
/// @return true: received ack matches expected ack
bool MissionManager::_checkForExpectedAck(AckType_t receivedAck)
Don Gagne's avatar
Don Gagne committed
285
{
286 287 288 289 290 291 292
    if (receivedAck == _expectedAck) {
        _expectedAck = AckNone;
        _ackTimeoutTimer->stop();
        return true;
    } else {
        if (_expectedAck == AckNone) {
            // Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using
293 294
            // spurious MISSION_ITEMs.
        } else {
295 296 297
            // We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers.
            // Whatever it is we let the ack timeout handle any error output to the user.
            qCDebug(MissionManagerLog) << QString("Out of sequence ack expected:received %1:%2").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck));
298
        }
299
        return false;
Don Gagne's avatar
Don Gagne committed
300 301 302
    }
}

303
void MissionManager::_readTransactionComplete(void)
Don Gagne's avatar
Don Gagne committed
304
{
305
    qCDebug(MissionManagerLog) << "_readTransactionComplete read sequence complete";
Don Gagne's avatar
Don Gagne committed
306 307 308 309 310 311 312 313
    
    mavlink_message_t       message;
    mavlink_mission_ack_t   missionAck;
    
    missionAck.target_system =      _vehicle->id();
    missionAck.target_component =   MAV_COMP_ID_MISSIONPLANNER;
    missionAck.type =               MAV_MISSION_ACCEPTED;
    
314 315 316 317 318
    mavlink_msg_mission_ack_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                        qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                        _dedicatedLink->mavlinkChannel(),
                                        &message,
                                        &missionAck);
Don Gagne's avatar
Don Gagne committed
319
    
320 321
    _vehicle->sendMessageOnLink(_dedicatedLink, message);

322
    _finishTransaction(true);
323
    emit newMissionItemsAvailable(false);
Don Gagne's avatar
Don Gagne committed
324 325 326 327 328 329
}

void MissionManager::_handleMissionCount(const mavlink_message_t& message)
{
    mavlink_mission_count_t missionCount;
    
330
    if (!_checkForExpectedAck(AckMissionCount)) {
Don Gagne's avatar
Don Gagne committed
331 332
        return;
    }
333 334

    _retryCount = 0;
Don Gagne's avatar
Don Gagne committed
335 336
    
    mavlink_msg_mission_count_decode(&message, &missionCount);
337 338 339
    qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;

    if (missionCount.count == 0) {
340
        _readTransactionComplete();
Don Gagne's avatar
Don Gagne committed
341
    } else {
342 343 344 345 346
        // Prime read list
        for (int i=0; i<missionCount.count; i++) {
            _itemIndicesToRead << i;
        }
        _requestNextMissionItem();
Don Gagne's avatar
Don Gagne committed
347 348 349
    }
}

350
void MissionManager::_requestNextMissionItem(void)
Don Gagne's avatar
Don Gagne committed
351
{
352 353
    if (_itemIndicesToRead.count() == 0) {
        _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
Don Gagne's avatar
Don Gagne committed
354 355
        return;
    }
356 357 358

    qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount;

359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384
    mavlink_message_t message;
    if (_vehicle->supportsMissionItemInt()) {
        mavlink_mission_request_int_t missionRequest;

        missionRequest.target_system =      _vehicle->id();
        missionRequest.target_component =   MAV_COMP_ID_MISSIONPLANNER;
        missionRequest.seq =                _itemIndicesToRead[0];

        mavlink_msg_mission_request_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                                    qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                                    _dedicatedLink->mavlinkChannel(),
                                                    &message,
                                                    &missionRequest);
    } else {
        mavlink_mission_request_t missionRequest;

        missionRequest.target_system =      _vehicle->id();
        missionRequest.target_component =   MAV_COMP_ID_MISSIONPLANNER;
        missionRequest.seq =                _itemIndicesToRead[0];

        mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                                qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                                _dedicatedLink->mavlinkChannel(),
                                                &message,
                                                &missionRequest);
    }
Don Gagne's avatar
Don Gagne committed
385
    
386
    _vehicle->sendMessageOnLink(_dedicatedLink, message);
387
    _startAckTimeout(AckMissionItem);
Don Gagne's avatar
Don Gagne committed
388 389
}

390
void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
Don Gagne's avatar
Don Gagne committed
391 392
{
    
393
    if (!_checkForExpectedAck(AckMissionItem)) {
Don Gagne's avatar
Don Gagne committed
394 395
        return;
    }
396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414

    MAV_CMD     command;
    MAV_FRAME   frame;
    double      param1;
    double      param2;
    double      param3;
    double      param4;
    double      param5;
    double      param6;
    double      param7;
    bool        autoContinue;
    bool        isCurrentItem;
    int         seq;

    if (missionItemInt) {
        mavlink_mission_item_int_t missionItem;
        mavlink_msg_mission_item_int_decode(&message, &missionItem);

        command =       (MAV_CMD)missionItem.command,
415 416
                frame =         (MAV_FRAME)missionItem.frame,
                param1 =        missionItem.param1;
417 418 419 420 421
        param2 =        missionItem.param2;
        param3 =        missionItem.param3;
        param4 =        missionItem.param4;
        param5 =        (double)missionItem.x / qPow(10.0, 7.0);
        param6 =        (double)missionItem.y / qPow(10.0, 7.0);
422
        param7 =        (double)missionItem.z;
423 424 425 426 427 428 429 430
        autoContinue =  missionItem.autocontinue;
        isCurrentItem = missionItem.current;
        seq =           missionItem.seq;
    } else {
        mavlink_mission_item_t missionItem;
        mavlink_msg_mission_item_decode(&message, &missionItem);

        command =       (MAV_CMD)missionItem.command,
431 432
                frame =         (MAV_FRAME)missionItem.frame,
                param1 =        missionItem.param1;
433 434 435 436 437 438 439 440 441 442
        param2 =        missionItem.param2;
        param3 =        missionItem.param3;
        param4 =        missionItem.param4;
        param5 =        missionItem.x;
        param6 =        missionItem.y;
        param7 =        missionItem.z;
        autoContinue =  missionItem.autocontinue;
        isCurrentItem = missionItem.current;
        seq =           missionItem.seq;
    }
443 444 445 446 447 448 449

    // We don't support editing ALT_INT frames so change on the way in.
    if (frame == MAV_FRAME_GLOBAL_INT) {
        frame = MAV_FRAME_GLOBAL;
    } else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
        frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
    }
Don Gagne's avatar
Don Gagne committed
450
    
451
    qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current" << seq << command << isCurrentItem;
Don Gagne's avatar
Don Gagne committed
452
    
453 454 455 456 457 458 459 460 461 462 463 464 465 466 467
    if (_itemIndicesToRead.contains(seq)) {
        _itemIndicesToRead.removeOne(seq);

        MissionItem* item = new MissionItem(seq,
                                            command,
                                            frame,
                                            param1,
                                            param2,
                                            param3,
                                            param4,
                                            param5,
                                            param6,
                                            param7,
                                            autoContinue,
                                            isCurrentItem,
468
                                            this);
Don Gagne's avatar
Don Gagne committed
469

Don Gagne's avatar
Don Gagne committed
470
        if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
Don Gagne's avatar
Don Gagne committed
471 472 473 474
            // Home is in position 0
            item->setParam1((int)item->param1() + 1);
        }

475 476
        _missionItems.append(item);
    } else {
477
        qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << seq;
478 479 480
        // We have to put the ack timeout back since it was removed above
        _startAckTimeout(AckMissionItem);
        return;
Don Gagne's avatar
Don Gagne committed
481
    }
482
    
483
    _retryCount = 0;
484
    if (_itemIndicesToRead.count() == 0) {
485
        _readTransactionComplete();
Don Gagne's avatar
Don Gagne committed
486
    } else {
487
        _requestNextMissionItem();
Don Gagne's avatar
Don Gagne committed
488 489 490 491 492
    }
}

void MissionManager::_clearMissionItems(void)
{
493
    _itemIndicesToRead.clear();
494
    _clearAndDeleteMissionItems();
Don Gagne's avatar
Don Gagne committed
495 496
}

497
void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
Don Gagne's avatar
Don Gagne committed
498 499 500
{
    mavlink_mission_request_t missionRequest;
    
501
    if (!_checkForExpectedAck(AckMissionRequest)) {
Don Gagne's avatar
Don Gagne committed
502 503 504 505 506
        return;
    }
    
    mavlink_msg_mission_request_decode(&message, &missionRequest);
    
507 508 509 510 511 512 513
    if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
        if (missionRequest.seq > _missionItems.count()) {
            _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq));
            _finishTransaction(false);
            return;
        } else {
            qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
514
        }
515 516
    } else {
        _itemIndicesToWrite.removeOne(missionRequest.seq);
Don Gagne's avatar
Don Gagne committed
517 518
    }
    
519
    MissionItem* item = _missionItems[missionRequest.seq];
520
    qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command();
521 522

    mavlink_message_t   messageOut;
523 524 525 526 527 528 529 530 531 532 533 534 535 536
    if (missionItemInt) {
        mavlink_mission_item_int_t missionItem;


        missionItem.target_system =     _vehicle->id();
        missionItem.target_component =  MAV_COMP_ID_MISSIONPLANNER;
        missionItem.seq =               missionRequest.seq;
        missionItem.command =           item->command();
        missionItem.param1 =            item->param1();
        missionItem.param2 =            item->param2();
        missionItem.param3 =            item->param3();
        missionItem.param4 =            item->param4();
        missionItem.x =                 item->param5() * qPow(10.0, 7.0);
        missionItem.y =                 item->param6() * qPow(10.0, 7.0);
537
        missionItem.z =                 item->param7();
538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570
        missionItem.frame =             item->frame();
        missionItem.current =           missionRequest.seq == 0;
        missionItem.autocontinue =      item->autoContinue();

        mavlink_msg_mission_item_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                                 qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                                 _dedicatedLink->mavlinkChannel(),
                                                 &messageOut,
                                                 &missionItem);
    } else {
        mavlink_mission_item_t missionItem;

        missionItem.target_system =     _vehicle->id();
        missionItem.target_component =  MAV_COMP_ID_MISSIONPLANNER;
        missionItem.seq =               missionRequest.seq;
        missionItem.command =           item->command();
        missionItem.param1 =            item->param1();
        missionItem.param2 =            item->param2();
        missionItem.param3 =            item->param3();
        missionItem.param4 =            item->param4();
        missionItem.x =                 item->param5();
        missionItem.y =                 item->param6();
        missionItem.z =                 item->param7();
        missionItem.frame =             item->frame();
        missionItem.current =           missionRequest.seq == 0;
        missionItem.autocontinue =      item->autoContinue();

        mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                             qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                             _dedicatedLink->mavlinkChannel(),
                                             &messageOut,
                                             &missionItem);
    }
Don Gagne's avatar
Don Gagne committed
571
    
572
    _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
573
    _startAckTimeout(AckMissionRequest);
Don Gagne's avatar
Don Gagne committed
574 575 576 577 578 579
}

void MissionManager::_handleMissionAck(const mavlink_message_t& message)
{
    mavlink_mission_ack_t missionAck;
    
580
    // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
581
    // type of a protocol sequence we are in.
582
    AckType_t savedExpectedAck = _expectedAck;
583 584
    
    // We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
585
    // a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
586
    // matter what.
587
    if (!_checkForExpectedAck(_expectedAck)) {
Don Gagne's avatar
Don Gagne committed
588 589 590 591 592
        return;
    }
    
    mavlink_msg_mission_ack_decode(&message, &missionAck);
    
Don Gagne's avatar
Don Gagne committed
593
    qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
594

595
    switch (savedExpectedAck) {
596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614
    case AckNone:
        // State machine is idle. Vehicle is confused.
        _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
        break;
    case AckMissionCount:
        // MISSION_COUNT message expected
        _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
        _finishTransaction(false);
        break;
    case AckMissionItem:
        // MISSION_ITEM expected
        _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
        _finishTransaction(false);
        break;
    case AckMissionRequest:
        // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
        if (missionAck.type == MAV_MISSION_ACCEPTED) {
            if (_itemIndicesToWrite.count() == 0) {
                qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
Don Gagne's avatar
Don Gagne committed
615 616
                _finishTransaction(true);
            } else {
617
                _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1. Vehicle only has partial list of mission items.").arg(_itemIndicesToWrite.count()));
Don Gagne's avatar
Don Gagne committed
618 619
                _finishTransaction(false);
            }
620 621 622 623 624 625 626 627 628 629 630 631 632 633 634
        } else {
            _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
            _finishTransaction(false);
        }
        break;
    case AckGuidedItem:
        // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
        if (missionAck.type == MAV_MISSION_ACCEPTED) {
            qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted";
            _finishTransaction(true);
        } else {
            _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
            _finishTransaction(false);
        }
        break;
Don Gagne's avatar
Don Gagne committed
635 636 637 638 639 640 641
    }
}

/// Called when a new mavlink message for out vehicle is received
void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
    switch (message.msgid) {
642 643 644
    case MAVLINK_MSG_ID_MISSION_COUNT:
        _handleMissionCount(message);
        break;
Don Gagne's avatar
Don Gagne committed
645

646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672
    case MAVLINK_MSG_ID_MISSION_ITEM:
        _handleMissionItem(message, false /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        _handleMissionItem(message, true /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_REQUEST:
        _handleMissionRequest(message, false /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        _handleMissionRequest(message, true /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_ACK:
        _handleMissionAck(message);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
        // FIXME: NYI
        break;

    case MAVLINK_MSG_ID_MISSION_CURRENT:
        _handleMissionCurrent(message);
        break;
Don Gagne's avatar
Don Gagne committed
673 674 675
    }
}

676 677
void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
{
678
    qCDebug(MissionManagerLog) << "Sending error" << errorCode << errorMsg;
679

680
    emit error(errorCode, errorMsg);
681
}
Don Gagne's avatar
Don Gagne committed
682 683 684 685

QString MissionManager::_ackTypeToString(AckType_t ackType)
{
    switch (ackType) {
686 687 688 689 690 691 692 693 694 695 696 697 698 699
    case AckNone:
        return QString("No Ack");
    case AckMissionCount:
        return QString("MISSION_COUNT");
    case AckMissionItem:
        return QString("MISSION_ITEM");
    case AckMissionRequest:
        return QString("MISSION_REQUEST");
    case AckGuidedItem:
        return QString("Guided Mode Item");
    default:
        qWarning(MissionManagerLog) << "Fell off end of switch statement";
        return QString("QGC Internal Error");
    }
Don Gagne's avatar
Don Gagne committed
700
}
Don Gagne's avatar
Don Gagne committed
701 702 703 704

QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result)
{
    switch (result) {
705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752
    case MAV_MISSION_ACCEPTED:
        return QString("Mission accepted (MAV_MISSION_ACCEPTED)");
        break;
    case MAV_MISSION_ERROR:
        return QString("Unspecified error (MAV_MISSION_ERROR)");
        break;
    case MAV_MISSION_UNSUPPORTED_FRAME:
        return QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)");
        break;
    case MAV_MISSION_UNSUPPORTED:
        return QString("Command is not supported (MAV_MISSION_UNSUPPORTED)");
        break;
    case MAV_MISSION_NO_SPACE:
        return QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)");
        break;
    case MAV_MISSION_INVALID:
        return QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)");
        break;
    case MAV_MISSION_INVALID_PARAM1:
        return QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)");
        break;
    case MAV_MISSION_INVALID_PARAM2:
        return QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)");
        break;
    case MAV_MISSION_INVALID_PARAM3:
        return QString("param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)");
        break;
    case MAV_MISSION_INVALID_PARAM4:
        return QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)");
        break;
    case MAV_MISSION_INVALID_PARAM5_X:
        return QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)");
        break;
    case MAV_MISSION_INVALID_PARAM6_Y:
        return QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)");
        break;
    case MAV_MISSION_INVALID_PARAM7:
        return QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)");
        break;
    case MAV_MISSION_INVALID_SEQUENCE:
        return QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)");
        break;
    case MAV_MISSION_DENIED:
        return QString("Not accepting any mission commands (MAV_MISSION_DENIED)");
        break;
    default:
        qWarning(MissionManagerLog) << "Fell off end of switch statement";
        return QString("QGC Internal Error");
Don Gagne's avatar
Don Gagne committed
753
    }
754
}
755 756 757 758 759

void MissionManager::_finishTransaction(bool success)
{
    if (!success && _readTransactionInProgress) {
        // Read from vehicle failed, clear partial list
760
        _clearAndDeleteMissionItems();
761
        emit newMissionItemsAvailable(false);
762 763 764 765 766
    }

    _itemIndicesToRead.clear();
    _itemIndicesToWrite.clear();

767 768 769 770 771
    if (_readTransactionInProgress || _writeTransactionInProgress) {
        _readTransactionInProgress = false;
        _writeTransactionInProgress = false;
        emit inProgressChanged(false);
    }
772 773 774 775 776

    if (_resumeMission) {
        _resumeMission = false;
        emit resumeMissionReady();
    }
777 778 779 780 781 782
}

bool MissionManager::inProgress(void)
{
    return _readTransactionInProgress || _writeTransactionInProgress;
}
783 784 785 786 787 788 789

void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
{
    mavlink_mission_current_t missionCurrent;

    mavlink_msg_mission_current_decode(&message, &missionCurrent);

790 791 792 793
    if (missionCurrent.seq != _currentMissionIndex) {
        qCDebug(MissionManagerLog) << "_handleMissionCurrent currentIndex:" << missionCurrent.seq;
        _currentMissionIndex = missionCurrent.seq;
        emit currentIndexChanged(_currentMissionIndex);
794
    }
795

796 797 798 799
    if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
        qCDebug(MissionManagerLog) << "_handleMissionCurrent lastCurrentIndex:" << _currentMissionIndex;
        _lastCurrentIndex = _currentMissionIndex;
        emit lastCurrentIndexChanged(_lastCurrentIndex);
800
    }
801
}
802 803 804 805 806 807 808

void MissionManager::removeAll(void)
{
    QList<MissionItem*> emptyList;

    writeMissionItems(emptyList);
}
809 810 811 812 813 814 815 816 817 818 819 820

void MissionManager::generateResumeMission(int resumeIndex)
{
    if (_vehicle->isOfflineEditingVehicle()) {
        return;
    }

    if (inProgress()) {
        qCDebug(MissionManagerLog) << "generateResumeMission called while transaction in progress";
        return;
    }

821 822 823 824 825 826 827 828
    for (int i=0; i<_missionItems.count(); i++) {
        MissionItem* item = _missionItems[i];
        if (item->command() == MAV_CMD_DO_JUMP) {
            qgcApp()->showMessage(tr("Unable to generate resume mission due to MAV_CMD_DO_JUMP command."));
            return;
        }
    }

829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850
    int seqNum = 0;
    QList<MissionItem*> resumeMission;

    QList<MAV_CMD> includedResumeCommands;

    // If any command in this list occurs before the resumeIndex it will be added to the front of the mission
    includedResumeCommands << MAV_CMD_DO_CONTROL_VIDEO
                           << MAV_CMD_DO_SET_ROI
                           << MAV_CMD_DO_DIGICAM_CONFIGURE
                           << MAV_CMD_DO_DIGICAM_CONTROL
                           << MAV_CMD_DO_MOUNT_CONFIGURE
                           << MAV_CMD_DO_MOUNT_CONTROL
                           << MAV_CMD_DO_SET_CAM_TRIGG_DIST
                           << MAV_CMD_DO_FENCE_ENABLE
                           << MAV_CMD_IMAGE_START_CAPTURE
                           << MAV_CMD_IMAGE_STOP_CAPTURE
                           << MAV_CMD_VIDEO_START_CAPTURE
                           << MAV_CMD_VIDEO_STOP_CAPTURE;

    bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle();
    int setCurrentIndex = addHomePosition ? 1 : 0;

851
    int resumeCommandCount = 0;
852 853 854
    for (int i=0; i<_missionItems.count(); i++) {
        MissionItem* oldItem = _missionItems[i];
        if ((i == 0 && addHomePosition) || i >= resumeIndex || includedResumeCommands.contains(oldItem->command())) {
855 856 857
            if (i < resumeIndex) {
                resumeCommandCount++;
            }
858 859 860 861 862 863 864
            MissionItem* newItem = new MissionItem(*oldItem, this);
            newItem->setIsCurrentItem( i == setCurrentIndex);
            newItem->setSequenceNumber(seqNum++);
            resumeMission.append(newItem);
        }
    }

865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949
    // De-dup and remove no-ops from the commands which were added to the front of the mission
    bool foundROI = false;
    bool foundCamTrigDist = false;
    QList<int> imageStartCameraIds;
    QList<int> imageStopCameraIds;
    QList<int> videoStartCameraIds;
    QList<int> videoStopCameraIds;
    while (resumeIndex >= 0) {
        MissionItem* resumeItem = resumeMission[resumeIndex];
        switch (resumeItem->command()) {
        case MAV_CMD_DO_SET_ROI:
            // Only keep the last one
            if (foundROI) {
                resumeMission.removeAt(resumeIndex);
            }
            foundROI = true;
            break;
        case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
            // Only keep the last one
            if (foundCamTrigDist) {
                resumeMission.removeAt(resumeIndex);
            }
            foundCamTrigDist = true;
            break;
        case MAV_CMD_IMAGE_START_CAPTURE:
        {
            // FIXME: Handle single image capture
            int cameraId = resumeItem->param6();

            if (resumeItem->param1() == 0) {
                // This is an individual image capture command, remove it
                resumeMission.removeAt(resumeIndex);
                break;
            }
            // If we already found an image stop, then all image start/stop commands are useless
            // De-dup repeated image start commands
            // Otherwise keep only the last image start
            if (imageStopCameraIds.contains(cameraId) || imageStartCameraIds.contains(cameraId)) {
                resumeMission.removeAt(resumeIndex);
            }
            if (!imageStopCameraIds.contains(cameraId)) {
                imageStopCameraIds.append(cameraId);
            }
        }
            break;
        case MAV_CMD_IMAGE_STOP_CAPTURE:
        {
            int cameraId = resumeItem->param1();
            // Image stop only matters to kill all previous image starts
            if (!imageStopCameraIds.contains(cameraId)) {
                imageStopCameraIds.append(cameraId);
            }
            resumeMission.removeAt(resumeIndex);
        }
            break;
        case MAV_CMD_VIDEO_START_CAPTURE:
        {
            int cameraId = resumeItem->param1();
            // If we already found an video stop, then all video start/stop commands are useless
            // De-dup repeated video start commands
            // Otherwise keep only the last video start
            if (videoStopCameraIds.contains(cameraId) || videoStopCameraIds.contains(cameraId)) {
                resumeMission.removeAt(resumeIndex);
            }
            if (!videoStopCameraIds.contains(cameraId)) {
                videoStopCameraIds.append(cameraId);
            }
        }
            break;
        case MAV_CMD_VIDEO_STOP_CAPTURE:
        {
            int cameraId = resumeItem->param1();
            // Video stop only matters to kill all previous video starts
            if (!videoStopCameraIds.contains(cameraId)) {
                videoStopCameraIds.append(cameraId);
            }
            resumeMission.removeAt(resumeIndex);
        }
            break;
        default:
            break;
        }

        resumeIndex--;
    }
950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971

    // Send to vehicle
    _clearAndDeleteMissionItems();
    for (int i=0; i<resumeMission.count(); i++) {
        _missionItems.append(new MissionItem(*resumeMission[i], this));
    }
    _resumeMission = true;
    _writeMissionItemsWorker();

    // Clean up no longer needed resume items
    for (int i=0; i<resumeMission.count(); i++) {
        resumeMission[i]->deleteLater();
    }
}

void MissionManager::_clearAndDeleteMissionItems(void)
{
    for (int i=0; i<_missionItems.count(); i++) {
        _missionItems[i]->deleteLater();
    }
    _missionItems.clear();
}