From 6454b0f085a6935775d31c5ef8f893806b11ef60 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 30 Mar 2017 10:00:09 -0700 Subject: [PATCH] Add resume mission support --- .../FlightDisplayViewWidgets.qml | 52 ++- src/MissionManager/MissionController.cc | 46 ++- src/MissionManager/MissionController.h | 12 +- src/MissionManager/MissionManager.cc | 325 +++++++++++------- src/MissionManager/MissionManager.h | 19 +- 5 files changed, 299 insertions(+), 155 deletions(-) diff --git a/src/FlightDisplay/FlightDisplayViewWidgets.qml b/src/FlightDisplay/FlightDisplayViewWidgets.qml index 6c85db413..10de92bd4 100644 --- a/src/FlightDisplay/FlightDisplayViewWidgets.qml +++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml @@ -38,7 +38,7 @@ Item { // Guided bar properties property bool _missionAvailable: missionController.containsItems property bool _missionActive: _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.missionFlightMode : false - property bool _missionInProgress: missionController.missionInProgress + property int _resumeMissionItem: missionController.resumeMissionItem property bool _showEmergenyStop: QGroundControl.corePlugin.options.guidedBarShowEmergencyStop property bool _showOrbit: QGroundControl.corePlugin.options.guidedBarShowOrbit @@ -92,6 +92,11 @@ Item { onValueChanged: _setInstrumentWidget() } + Connections { + target: missionController + onResumeMissionReady: _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMissionReady) + } + Component.onCompleted: { _setInstrumentWidget() } @@ -219,19 +224,20 @@ Item { } } - readonly property int confirmHome: 1 - readonly property int confirmLand: 2 - readonly property int confirmTakeoff: 3 - readonly property int confirmArm: 4 - readonly property int confirmDisarm: 5 - readonly property int confirmEmergencyStop: 6 - readonly property int confirmChangeAlt: 7 - readonly property int confirmGoTo: 8 - readonly property int confirmRetask: 9 - readonly property int confirmOrbit: 10 - readonly property int confirmAbort: 11 - readonly property int confirmStartMission: 12 - readonly property int confirmResumeMission: 13 + readonly property int confirmHome: 1 + readonly property int confirmLand: 2 + readonly property int confirmTakeoff: 3 + readonly property int confirmArm: 4 + readonly property int confirmDisarm: 5 + readonly property int confirmEmergencyStop: 6 + readonly property int confirmChangeAlt: 7 + readonly property int confirmGoTo: 8 + readonly property int confirmRetask: 9 + readonly property int confirmOrbit: 10 + readonly property int confirmAbort: 11 + readonly property int confirmStartMission: 12 + readonly property int confirmResumeMission: 13 + readonly property int confirmResumeMissionReady: 14 property int confirmActionCode property real _showMargin: _margins @@ -250,6 +256,11 @@ Item { _activeVehicle.guidedModeTakeoff() break; case confirmResumeMission: + missionController.resumeMission(missionController.resumeMissionItem) + break; + case confirmResumeMissionReady: + _activeVehicle.startMission() + break; case confirmStartMission: _activeVehicle.startMission() break; @@ -318,6 +329,9 @@ Item { case confirmResumeMission: guidedModeConfirm.confirmText = qsTr("resume mission") break; + case confirmResumeMissionReady: + guidedModeConfirm.confirmText = qsTr("resume modified mission after review") + break; case confirmLand: guidedModeConfirm.confirmText = qsTr("land") break; @@ -400,15 +414,15 @@ Item { QGCButton { pointSize: _guidedModeBar._fontPointSize - text: qsTr("Start Mission") + text: _resumeMissionItem !== -1 ? qsTr("Restart Mission") : qsTr("Start Mission") visible: _activeVehicle && !_activeVehicle.flying && _missionAvailable onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmStartMission) } QGCButton { pointSize: _guidedModeBar._fontPointSize - text: qsTr("Resume Mission") - visible: _activeVehicle && _activeVehicle.guidedModeSupported && !_activeVehicle.flying && _missionAvailable && _missionInProgress + text: qsTr("Resume Mission (%1)").arg(_resumeMissionItem) + visible: _activeVehicle && !_activeVehicle.flying && _missionAvailable && _resumeMissionItem !== -1 onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMission) } @@ -422,14 +436,14 @@ Item { QGCButton { pointSize: _guidedModeBar._fontPointSize text: qsTr("Change Altitude") - visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed + visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed && !_missionActive onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmChangeAlt) } QGCButton { pointSize: _guidedModeBar._fontPointSize text: qsTr("Orbit") - visible: _showOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed + visible: _showOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed && !_missionActive onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit) } diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 2ddaaaba7..295822b4e 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -53,7 +53,6 @@ MissionController::MissionController(QObject *parent) , _settingsItem(NULL) , _firstItemsFromVehicle(false) , _missionItemsRequested(false) - , _queuedSend(false) , _surveyMissionItemName(tr("Survey")) , _fwLandingMissionItemName(tr("Fixed Wing Landing")) , _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings()) @@ -647,7 +646,6 @@ void MissionController::loadFromFile(const QString& filename) MissionController::_scanForAdditionalSettings(_visualItems, _activeVehicle); _initAllVisualItems(); - sendToVehicle(); } bool MissionController::loadItemsFromFile(Vehicle* vehicle, const QString& filename, QmlObjectListModel** visualItems) @@ -1280,6 +1278,8 @@ void MissionController::_activeVehicleBeingRemoved(void) disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); disconnect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); + disconnect(missionManager, &MissionManager::lastCurrentItemChanged, this, &MissionController::resumeMissionItemChanged); + disconnect(missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady); disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); // We always remove all items on vehicle change. This leaves a user model hole: @@ -1298,6 +1298,8 @@ void MissionController::_activeVehicleSet(void) connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); connect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); connect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); + connect(missionManager, &MissionManager::lastCurrentItemChanged, this, &MissionController::resumeMissionItemChanged); + connect(missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady); connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); connect(_activeVehicle, &Vehicle::defaultCruiseSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); connect(_activeVehicle, &Vehicle::defaultHoverSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); @@ -1312,6 +1314,7 @@ void MissionController::_activeVehicleSet(void) _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); emit complexMissionItemNamesChanged(); + emit resumeMissionItemChanged(); } void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) @@ -1478,11 +1481,29 @@ void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel } } -void MissionController::_currentMissionItemChanged(int sequenceNumber) +int MissionController::resumeMissionItem(void) const { + + int resumeIndex = -1; + if (!_editMode) { - bool prevMissionInProgress = missionInProgress(); + int firstTrueItemIndex = _activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 1 : 0; + resumeIndex = _activeVehicle->missionManager()->lastCurrentItem(); + if (resumeIndex > firstTrueItemIndex) { + if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { + resumeIndex++; + } + // Resume at the item previous to the item we were heading towards + resumeIndex--; + } + } + + return resumeIndex; +} +void MissionController::_currentMissionItemChanged(int sequenceNumber) +{ + if (!_editMode) { if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { sequenceNumber++; } @@ -1491,10 +1512,6 @@ void MissionController::_currentMissionItemChanged(int sequenceNumber) VisualMissionItem* item = qobject_cast(_visualItems->get(i)); item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber); } - - if (prevMissionInProgress != missionInProgress()) { - emit missionInProgressChanged(); - } } } @@ -1576,11 +1593,6 @@ QStringList MissionController::complexMissionItemNames(void) const return complexItems; } -bool MissionController::missionInProgress(void) const -{ - return _visualItems && _visualItems->count() > 1 && (!_visualItems->value(0)->isCurrentItem() && !_visualItems->value(1)->isCurrentItem()); -} - void MissionController::_visualItemsDirtyChanged(bool dirty) { if (dirty) { @@ -1594,3 +1606,11 @@ void MissionController::_visualItemsDirtyChanged(bool dirty) emit dirtyChanged(false); } } + +void MissionController::resumeMission(int resumeIndex) +{ + if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { + resumeIndex--; + } + _activeVehicle->missionManager()->generateResumeMission(resumeIndex); +} diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 5de84f9dc..1a893ab64 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -55,7 +55,7 @@ public: Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) Q_PROPERTY(QStringList complexMissionItemNames READ complexMissionItemNames NOTIFY complexMissionItemNamesChanged) - Q_PROPERTY(bool missionInProgress READ missionInProgress NOTIFY missionInProgressChanged) ///< true: Mission sequence is beyond first item + Q_PROPERTY(int resumeMissionItem READ resumeMissionItem NOTIFY resumeMissionItemChanged) Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged) Q_PROPERTY(double missionTime READ missionTime NOTIFY missionTimeChanged) @@ -79,6 +79,8 @@ public: /// @return Sequence number for new item Q_INVOKABLE int insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i); + Q_INVOKABLE void resumeMission(int resumeIndex); + /// Loads the mission items from the specified file /// @param[in] vehicle Vehicle we are loading items for /// @param[in] filename File to load from @@ -110,7 +112,9 @@ public: QmlObjectListModel* visualItems (void) { return _visualItems; } QmlObjectListModel* waypointLines (void) { return &_waypointLines; } QStringList complexMissionItemNames (void) const; - bool missionInProgress (void) const; + + /// Returns the item index two which a mission should be resumed. -1 indicates resume mission not available. + int resumeMissionItem(void) const; double missionDistance (void) const { return _missionFlightStatus.totalDistance; } double missionTime (void) const { return _missionFlightStatus.totalTime; } @@ -132,7 +136,8 @@ signals: void missionCruiseTimeChanged(void); void missionMaxTelemetryChanged(double missionMaxTelemetry); void complexMissionItemNamesChanged(void); - bool missionInProgressChanged(void); + void resumeMissionItemChanged(void); + void resumeMissionReady(void); private slots: void _newMissionItemsAvailableFromVehicle(bool removeAllRequested); @@ -188,7 +193,6 @@ private: CoordVectHashTable _linesTable; bool _firstItemsFromVehicle; bool _missionItemsRequested; - bool _queuedSend; MissionFlightStatus_t _missionFlightStatus; QString _surveyMissionItemName; QString _fwLandingMissionItemName; diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 85ca185f4..b32d5804e 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -26,7 +26,9 @@ MissionManager::MissionManager(Vehicle* vehicle) , _expectedAck(AckNone) , _readTransactionInProgress(false) , _writeTransactionInProgress(false) + , _resumeMission(false) , _currentMissionItem(-1) + , _lastCurrentItem(-1) { connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived); @@ -42,6 +44,29 @@ MissionManager::~MissionManager() } +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(); + + _currentMissionItem = -1; + _lastCurrentItem = -1; + emit currentItemChanged(-1); + emit lastCurrentItemChanged(-1); +} + + void MissionManager::writeMissionItems(const QList& missionItems) { if (_vehicle->isOfflineEditingVehicle()) { @@ -55,10 +80,10 @@ void MissionManager::writeMissionItems(const QList& missionItems) bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); - _missionItems.clear(); + _clearAndDeleteMissionItems(); int firstIndex = skipFirstItem ? 1 : 0; - + for (int i=firstIndex; i& missionItems) } } } - 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(); + _writeMissionItemsWorker(); } /// This begins the write sequence with the vehicle. This may be called during a retry. @@ -398,8 +412,8 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m mavlink_msg_mission_item_int_decode(&message, &missionItem); command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; param2 = missionItem.param2; param3 = missionItem.param3; param4 = missionItem.param4; @@ -414,8 +428,8 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m mavlink_msg_mission_item_decode(&message, &missionItem); command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; param2 = missionItem.param2; param3 = missionItem.param3; param4 = missionItem.param4; @@ -470,7 +484,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m void MissionManager::_clearMissionItems(void) { _itemIndicesToRead.clear(); - _missionItems.clear(); + _clearAndDeleteMissionItems(); } void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt) @@ -572,45 +586,45 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); switch (savedExpectedAck) { - 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"; - _finishTransaction(true); - } else { - _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())); - _finishTransaction(false); - } - } 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"; + 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"; _finishTransaction(true); } else { - _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _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())); _finishTransaction(false); } - break; + } 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; } } @@ -662,73 +676,73 @@ void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) QString MissionManager::_ackTypeToString(AckType_t ackType) { switch (ackType) { - 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"); - } + 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"); + } } QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result) { switch (result) { - 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"); + 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"); } } @@ -736,7 +750,7 @@ void MissionManager::_finishTransaction(bool success) { if (!success && _readTransactionInProgress) { // Read from vehicle failed, clear partial list - _missionItems.clear(); + _clearAndDeleteMissionItems(); emit newMissionItemsAvailable(false); } @@ -748,6 +762,11 @@ void MissionManager::_finishTransaction(bool success) _writeTransactionInProgress = false; emit inProgressChanged(false); } + + if (_resumeMission) { + _resumeMission = false; + emit resumeMissionReady(); + } } bool MissionManager::inProgress(void) @@ -766,6 +785,11 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) _currentMissionItem = missionCurrent.seq; emit currentItemChanged(_currentMissionItem); } + + if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionItem != _lastCurrentItem) { + _lastCurrentItem = _currentMissionItem; + emit lastCurrentItemChanged(_lastCurrentItem); + } } void MissionManager::removeAll(void) @@ -774,3 +798,70 @@ void MissionManager::removeAll(void) writeMissionItems(emptyList); } + +void MissionManager::generateResumeMission(int resumeIndex) +{ + if (_vehicle->isOfflineEditingVehicle()) { + return; + } + + if (inProgress()) { + qCDebug(MissionManagerLog) << "generateResumeMission called while transaction in progress"; + return; + } + + int seqNum = 0; + QList resumeMission; + + QList 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; + + for (int i=0; i<_missionItems.count(); i++) { + MissionItem* oldItem = _missionItems[i]; + if ((i == 0 && addHomePosition) || i >= resumeIndex || includedResumeCommands.contains(oldItem->command())) { + MissionItem* newItem = new MissionItem(*oldItem, this); + newItem->setIsCurrentItem( i == setCurrentIndex); + newItem->setSequenceNumber(seqNum++); + resumeMission.append(newItem); + } + } + + // Handle DO_JUMP seq num update + + // Send to vehicle + _clearAndDeleteMissionItems(); + for (int i=0; ideleteLater(); + } +} + +void MissionManager::_clearAndDeleteMissionItems(void) +{ + for (int i=0; i<_missionItems.count(); i++) { + _missionItems[i]->deleteLater(); + } + _missionItems.clear(); +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 583d8f858..3cbbafab7 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -36,7 +36,12 @@ public: bool inProgress(void); const QList& missionItems(void) { return _missionItems; } - int currentItem(void) { return _currentMissionItem; } + + /// Current mission item as reported by MISSION_CURRENT + int currentItem(void) const { return _currentMissionItem; } + + /// Last current mission item reported while in Mission flight mode + int lastCurrentItem(void) const { return _lastCurrentItem; } void requestMissionItems(void); @@ -52,6 +57,10 @@ public: /// Removes all mission items from vehicle void removeAll(void); + /// Generates a new mission which starts from the specified index. It will include all the CMD_DO items + /// from mission start to resumeIndex in the generate mission. + void generateResumeMission(int resumeIndex); + /// Error codes returned in error signal typedef enum { InternalError, @@ -73,7 +82,9 @@ signals: void inProgressChanged(bool inProgress); void error(int errorCode, const QString& errorMsg); void currentItemChanged(int currentItem); - + void lastCurrentItemChanged(int lastCurrentMissionItem); + void resumeMissionReady(void); + private slots: void _mavlinkMessageReceived(const mavlink_message_t& message); void _ackTimeout(void); @@ -103,6 +114,8 @@ private: void _finishTransaction(bool success); void _requestList(void); void _writeMissionCount(void); + void _writeMissionItemsWorker(void); + void _clearAndDeleteMissionItems(void); private: Vehicle* _vehicle; @@ -114,6 +127,7 @@ private: bool _readTransactionInProgress; bool _writeTransactionInProgress; + bool _resumeMission; QList _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle QList _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle @@ -121,6 +135,7 @@ private: QList _missionItems; int _currentMissionItem; + int _lastCurrentItem; }; #endif -- 2.22.0