MissionManager: get currentMissionIndex from High_lat messages as well:

High latency messages provide wp_num field, which
is meant to be used instead of MISSION_CURRENT when
we are under high latency link. This commit also
refactors the way of updating _lastCurrentIndex
from the 3 possible types of mavlink messages.

src/MissionManager/MissionManager.cc

@@ -221,6 +221,14 @@ void MissionManager::generateResumeMission(int resumeIndex)
 void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
 {
     switch (message.msgid) {
+    case MAVLINK_MSG_ID_HIGH_LATENCY:
+        _handleHighLatency(message);
+        break;
+
+    case MAVLINK_MSG_ID_HIGH_LATENCY2:
+        _handleHighLatency2(message);
+        break;
+
     case MAVLINK_MSG_ID_MISSION_CURRENT:
         _handleMissionCurrent(message);
         break;
@@ -231,15 +239,11 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
     }
 }
 
-void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
+void MissionManager::_updateMissionIndex(int index)
 {
-    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;
+    if (index != _currentMissionIndex) {
+        qCDebug(MissionManagerLog) << "_updateMissionIndex currentIndex:" << index;
+        _currentMissionIndex = index;
         emit currentIndexChanged(_currentMissionIndex);
     }
 
@@ -250,11 +254,32 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
         // 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;
+        qCDebug(MissionManagerLog) << "_updateMissionIndex caching _lastCurrentIndex for possible update:" << _currentMissionIndex;
         _cachedLastCurrentIndex = _currentMissionIndex;
     }
 }
 
+void MissionManager::_handleHighLatency(const mavlink_message_t& message) 
+{
+    mavlink_high_latency_t highLatency;
+    mavlink_msg_high_latency_decode(&message, &highLatency);
+    _updateMissionIndex(highLatency.wp_num);
+}
+
+void MissionManager::_handleHighLatency2(const mavlink_message_t& message) 
+{
+    mavlink_high_latency2_t highLatency2;
+    mavlink_msg_high_latency2_decode(&message, &highLatency2);
+    _updateMissionIndex(highLatency2.wp_num);
+}
+
+void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
+{
+    mavlink_mission_current_t missionCurrent;
+    mavlink_msg_mission_current_decode(&message, &missionCurrent);
+    _updateMissionIndex(missionCurrent.seq);
+}
+
 void MissionManager::_handleHeartbeat(const mavlink_message_t& message)
 {
     Q_UNUSED(message);

src/MissionManager/MissionManager.h

@@ -40,7 +40,10 @@ private slots:
     void _mavlinkMessageReceived(const mavlink_message_t& message);
 
 private:
+    void _handleHighLatency(const mavlink_message_t& message);
+    void _handleHighLatency2(const mavlink_message_t& message);
     void _handleMissionCurrent(const mavlink_message_t& message);
+    void _updateMissionIndex(int index);
     void _handleHeartbeat(const mavlink_message_t& message);
 
     int _cachedLastCurrentIndex;