diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index c61eb2c2ab2620eb204158a35ceff6049073f44b..0bf4501872909906dc004fbf5b6d2cf39fdc347e 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -22,9 +22,10 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") MissionManager::MissionManager(Vehicle* vehicle) - : PlanManager(vehicle, MAV_MISSION_TYPE_MISSION) + : PlanManager (vehicle, MAV_MISSION_TYPE_MISSION) + , _cachedLastCurrentIndex (-1) { - + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived); } MissionManager::~MissionManager() @@ -219,3 +220,54 @@ void MissionManager::generateResumeMission(int resumeIndex) resumeMission[i]->deleteLater(); } } + +/// Called when a new mavlink message for out vehicle is received +void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_MISSION_CURRENT: + _handleMissionCurrent(message); + break; + + case MAVLINK_MSG_ID_HEARTBEAT: + _handleHeartbeat(message); + break; + } +} + +void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) +{ + mavlink_mission_current_t missionCurrent; + + mavlink_msg_mission_current_decode(&message, &missionCurrent); + + if (missionCurrent.seq != _currentMissionIndex) { + qCDebug(MissionManagerLog) << "_handleMissionCurrent currentIndex:" << missionCurrent.seq; + _currentMissionIndex = missionCurrent.seq; + emit currentIndexChanged(_currentMissionIndex); + } + + if (_currentMissionIndex != _lastCurrentIndex) { + // We have to be careful of an RTL sequence causing a change of index to the DO_LAND_START sequence. This also triggers + // a flight mode change away from mission flight mode. So we only update _lastCurrentIndex when the flight mode is mission. + // But we can run into problems where we may get the MISSION_CURRENT message for the RTL/DO_LAND_START sequenc change prior + // to the HEARTBEAT message which contains the flight mode change which will cause things to work incorrectly. To fix this + // We force the sequencing of HEARTBEAT following by MISSION_CURRENT by caching the possible _lastCurrentIndex update until + // the next HEARTBEAT comes through. + qCDebug(MissionManagerLog) << "_handleMissionCurrent caching _lastCurrentIndex for possible update:" << _currentMissionIndex; + _cachedLastCurrentIndex = _currentMissionIndex; + } +} + +void MissionManager::_handleHeartbeat(const mavlink_message_t& message) +{ + Q_UNUSED(message); + + if (_cachedLastCurrentIndex != -1 && _vehicle->flightMode() == _vehicle->missionFlightMode()) { + qCDebug(MissionManagerLog) << "_handleHeartbeat updating lastCurrentIndex from cached value:" << _cachedLastCurrentIndex; + _lastCurrentIndex = _cachedLastCurrentIndex; + _cachedLastCurrentIndex = -1; + emit lastCurrentIndexChanged(_lastCurrentIndex); + } +} + diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index bf6936c6aba9fefc7b5c41e964091f25be46060c..e34d6e37dc69239ff7f09882fdc69bf10a35c6b0 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -22,7 +22,13 @@ class MissionManager : public PlanManager public: MissionManager(Vehicle* vehicle); ~MissionManager(); - + + /// Current mission item as reported by MISSION_CURRENT + int currentIndex(void) const { return _currentMissionIndex; } + + /// Last current mission item reported while in Mission flight mode + int lastCurrentIndex(void) const { return _lastCurrentIndex; } + /// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item. /// @param gotoCoord Coordinate to move to /// @param altChangeOnly true: only altitude change, false: lat/lon/alt change @@ -31,6 +37,19 @@ public: /// 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); + +signals: + void currentIndexChanged (int currentIndex); + void lastCurrentIndexChanged (int lastCurrentIndex); + +private slots: + void _mavlinkMessageReceived(const mavlink_message_t& message); + +private: + void _handleMissionCurrent(const mavlink_message_t& message); + void _handleHeartbeat(const mavlink_message_t& message); + + int _cachedLastCurrentIndex; }; #endif diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 53c95ed492d56688be8947bcda309c0088bac130..6ad7fbfd0d8c1bcd91b482d6b000e75c8c5d0d45 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -634,6 +634,7 @@ void PlanManager::_handleMissionAck(const mavlink_message_t& message) break; } } + /// Called when a new mavlink message for out vehicle is received void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message) { @@ -661,14 +662,6 @@ void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message) 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; } } @@ -800,7 +793,7 @@ QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result) return resultString + lastRequestString; } -void PlanManager::_finishTransaction(bool success) +void PlanManager::_finishTransaction(bool success, bool apmGuidedItemWrite) { emit progressPct(1); _disconnectFromMavlink(); @@ -826,22 +819,27 @@ void PlanManager::_finishTransaction(bool success) emit newMissionItemsAvailable(false); break; case TransactionWrite: - if (success) { - // Write succeeded, update internal list to be current - _currentMissionIndex = -1; - _lastCurrentIndex = -1; - emit currentIndexChanged(-1); - emit lastCurrentIndexChanged(-1); - _clearAndDeleteMissionItems(); - for (int i=0; i<_writeMissionItems.count(); i++) { - _missionItems.append(_writeMissionItems[i]); + // No need to do anything for ArduPilot guided go to waypoint write + if (!apmGuidedItemWrite) { + if (success) { + // Write succeeded, update internal list to be current + if (_planType == MAV_MISSION_TYPE_MISSION) { + _currentMissionIndex = -1; + _lastCurrentIndex = -1; + emit currentIndexChanged(-1); + emit lastCurrentIndexChanged(-1); + } + _clearAndDeleteMissionItems(); + for (int i=0; i<_writeMissionItems.count(); i++) { + _missionItems.append(_writeMissionItems[i]); + } + _writeMissionItems.clear(); + } else { + // Write failed, throw out the write list + _clearAndDeleteWriteMissionItems(); } - _writeMissionItems.clear(); - } else { - // Write failed, throw out the write list - _clearAndDeleteWriteMissionItems(); + emit sendComplete(!success /* error */); } - emit sendComplete(!success /* error */); break; case TransactionRemoveAll: emit removeAllComplete(!success /* error */); @@ -865,25 +863,6 @@ bool PlanManager::inProgress(void) const return _transactionInProgress != TransactionNone; } -void PlanManager::_handleMissionCurrent(const mavlink_message_t& message) -{ - mavlink_mission_current_t missionCurrent; - - mavlink_msg_mission_current_decode(&message, &missionCurrent); - - if (missionCurrent.seq != _currentMissionIndex) { - qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 currentIndex:").arg(_planTypeString()) << missionCurrent.seq; - _currentMissionIndex = missionCurrent.seq; - emit currentIndexChanged(_currentMissionIndex); - } - - if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) { - qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 lastCurrentIndex:").arg(_planTypeString()) << _currentMissionIndex; - _lastCurrentIndex = _currentMissionIndex; - emit lastCurrentIndexChanged(_lastCurrentIndex); - } -} - void PlanManager::_removeAllWorker(void) { mavlink_message_t message; @@ -915,10 +894,12 @@ void PlanManager::removeAll(void) _clearAndDeleteMissionItems(); - _currentMissionIndex = -1; - _lastCurrentIndex = -1; - emit currentIndexChanged(-1); - emit lastCurrentIndexChanged(-1); + if (_planType == MAV_MISSION_TYPE_MISSION) { + _currentMissionIndex = -1; + _lastCurrentIndex = -1; + emit currentIndexChanged(-1); + emit lastCurrentIndexChanged(-1); + } _transactionInProgress = TransactionRemoveAll; _retryCount = 0; diff --git a/src/MissionManager/PlanManager.h b/src/MissionManager/PlanManager.h index 43f696c14ed9f7f6a8d5834c5cb311070a89e747..aeda03a50e735beb2a2f5b9f0ebeca06c513965b 100644 --- a/src/MissionManager/PlanManager.h +++ b/src/MissionManager/PlanManager.h @@ -112,13 +112,12 @@ protected: void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt); void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt); void _handleMissionAck(const mavlink_message_t& message); - void _handleMissionCurrent(const mavlink_message_t& message); void _requestNextMissionItem(void); void _clearMissionItems(void); void _sendError(ErrorCode_t errorCode, const QString& errorMsg); QString _ackTypeToString(AckType_t ackType); QString _missionResultToString(MAV_MISSION_RESULT result); - void _finishTransaction(bool success); + void _finishTransaction(bool success, bool apmGuidedItemWrite = false); void _requestList(void); void _writeMissionCount(void); void _writeMissionItemsWorker(void);