Fix MISSION_CURRENT handling

Also fixed bug with ArduPilot guided waypoint write which would trash
existing mission.

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

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

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;

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