From de8dc0d958bee403c361ceee9bad1d9e18056a08 Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Wed, 18 Oct 2017 11:15:58 -0700
Subject: [PATCH] Fix Resume Mission race condition

---
 src/MissionManager/MissionManager.cc | 30 +++++++++++++++++++++++++---
 src/MissionManager/MissionManager.h  |  2 ++
 src/Vehicle/Vehicle.cc               |  2 ++
 3 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc
index 2fa4cd40c..29222dec3 100644
--- a/src/MissionManager/MissionManager.cc
+++ b/src/MissionManager/MissionManager.cc
@@ -31,6 +31,7 @@ MissionManager::MissionManager(Vehicle* vehicle)
     , _lastMissionRequest(-1)
     , _currentMissionIndex(-1)
     , _lastCurrentIndex(-1)
+    , _cachedLastCurrentIndex(-1)
 {
     connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
     
@@ -713,6 +714,10 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
     case MAVLINK_MSG_ID_MISSION_CURRENT:
         _handleMissionCurrent(message);
         break;
+
+    case MAVLINK_MSG_ID_HEARTBEAT:
+        _handleHeartbeat(message);
+        break;
     }
 }
 
@@ -873,6 +878,7 @@ void MissionManager::_finishTransaction(bool success)
             // Write succeeded, update internal list to be current            
             _currentMissionIndex = -1;
             _lastCurrentIndex = -1;
+            _cachedLastCurrentIndex = -1;
             emit currentIndexChanged(-1);
             emit lastCurrentIndexChanged(-1);
             _clearAndDeleteMissionItems();
@@ -920,9 +926,26 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
         emit currentIndexChanged(_currentMissionIndex);
     }
 
-    if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
-        qCDebug(MissionManagerLog) << "_handleMissionCurrent lastCurrentIndex:" << _currentMissionIndex;
-        _lastCurrentIndex = _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);
     }
 }
@@ -959,6 +982,7 @@ void MissionManager::removeAll(void)
 
     _currentMissionIndex = -1;
     _lastCurrentIndex = -1;
+    _cachedLastCurrentIndex = -1;
     emit currentIndexChanged(-1);
     emit lastCurrentIndexChanged(-1);
 
diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h
index 6c6a792a7..56993fd01 100644
--- a/src/MissionManager/MissionManager.h
+++ b/src/MissionManager/MissionManager.h
@@ -122,6 +122,7 @@ private:
     void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
     void _handleMissionAck(const mavlink_message_t& message);
     void _handleMissionCurrent(const mavlink_message_t& message);
+    void _handleHeartbeat(const mavlink_message_t& message);
     void _requestNextMissionItem(void);
     void _clearMissionItems(void);
     void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
@@ -156,6 +157,7 @@ private:
     QList<MissionItem*> _writeMissionItems;     ///< Set of mission items currently being written to vehicle
     int                 _currentMissionIndex;
     int                 _lastCurrentIndex;
+    int                 _cachedLastCurrentIndex;
 };
 
 #endif
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 0fc2454db..db64f1a95 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -611,6 +611,8 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         break;
     }
 
+    // This must be emitted after the vehicle processes the message. This way the vehicle state is up to date when anyone else
+    // does processing.
     emit mavlinkMessageReceived(message);
 
     _uas->receiveMessage(message);
-- 
2.22.0