Merge pull request #2777 from DonLakeFlyer/CurrentMissionItem

Track current mission item while flying

src/MissionManager/MissionController.cc

@@ -637,6 +637,7 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
 
         disconnect(missionManager, &MissionManager::newMissionItemsAvailable,   this, &MissionController::_newMissionItemsAvailableFromVehicle);
         disconnect(missionManager, &MissionManager::inProgressChanged,          this, &MissionController::_inProgressChanged);
+        disconnect(missionManager, &MissionManager::currentItemChanged,         this, &MissionController::_currentMissionItemChanged);
         disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged,      this, &MissionController::_activeVehicleHomePositionAvailableChanged);
         disconnect(_activeVehicle, &Vehicle::homePositionChanged,               this, &MissionController::_activeVehicleHomePositionChanged);
         _activeVehicle = NULL;
@@ -649,6 +650,7 @@ void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
 
         connect(missionManager, &MissionManager::newMissionItemsAvailable,  this, &MissionController::_newMissionItemsAvailableFromVehicle);
         connect(missionManager, &MissionManager::inProgressChanged,         this, &MissionController::_inProgressChanged);
+        connect(missionManager, &MissionManager::currentItemChanged,        this, &MissionController::_currentMissionItemChanged);
         connect(_activeVehicle, &Vehicle::homePositionAvailableChanged,     this, &MissionController::_activeVehicleHomePositionAvailableChanged);
         connect(_activeVehicle, &Vehicle::homePositionChanged,              this, &MissionController::_activeVehicleHomePositionChanged);
 
@@ -814,3 +816,17 @@ void MissionController::_addPlannedHomePosition(bool addToCenter)
         homeItem->setCoordinate(qgcApp()->lastKnownHomePosition());
     }
 }
+
+void MissionController::_currentMissionItemChanged(int sequenceNumber)
+{
+    if (!_editMode) {
+        if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
+            sequenceNumber++;
+        }
+
+        for (int i=0; i<_missionItems->count(); i++) {
+            MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
+            item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber);
+        }
+    }
+}

src/MissionManager/MissionController.h

@@ -78,6 +78,7 @@ private slots:
     void _activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition);
     void _dirtyChanged(bool dirty);
     void _inProgressChanged(bool inProgress);
+    void _currentMissionItemChanged(int sequenceNumber);
 
 private:
     void _recalcSequence(void);

src/MissionManager/MissionManager.cc

@@ -39,6 +39,7 @@ MissionManager::MissionManager(Vehicle* vehicle)
     , _retryAck(AckNone)
     , _readTransactionInProgress(false)
     , _writeTransactionInProgress(false)
+    , _currentMissionItem(-1)
 {
     connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
     
@@ -421,7 +422,7 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
             break;
             
         case MAVLINK_MSG_ID_MISSION_CURRENT:
-            // FIXME: NYI
+            _handleMissionCurrent(message);
             break;
     }
 }
@@ -535,3 +536,16 @@ bool MissionManager::inProgress(void)
 {
     return _readTransactionInProgress || _writeTransactionInProgress;
 }
+
+void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
+{
+    mavlink_mission_current_t missionCurrent;
+
+    mavlink_msg_mission_current_decode(&message, &missionCurrent);
+
+    qCDebug(MissionManagerLog) << "_handleMissionCurrent seq:" << missionCurrent.seq;
+    if (missionCurrent.seq != _currentMissionItem) {
+        _currentMissionItem = missionCurrent.seq;
+        emit currentItemChanged(_currentMissionItem);
+    }
+}

src/MissionManager/MissionManager.h

@@ -50,11 +50,13 @@ public:
     
     Q_PROPERTY(bool                 inProgress      READ inProgress     NOTIFY inProgressChanged)
     Q_PROPERTY(QmlObjectListModel*  missionItems    READ missionItems   CONSTANT)
+    Q_PROPERTY(int                  currentItem     READ currentItem    NOTIFY currentItemChanged)
     
     // Property accessors
     
     bool inProgress(void);
     QmlObjectListModel* missionItems(void) { return &_missionItems; }
+    int currentItem(void) { return _currentMissionItem; }
     
     // C++ methods
     
@@ -88,6 +90,7 @@ signals:
     void newMissionItemsAvailable(void);
     void inProgressChanged(bool inProgress);
     void error(int errorCode, const QString& errorMsg);
+    void currentItemChanged(int currentItem);
     
 private slots:
     void _mavlinkMessageReceived(const mavlink_message_t& message);
@@ -108,6 +111,7 @@ private:
     void _handleMissionItem(const mavlink_message_t& message);
     void _handleMissionRequest(const mavlink_message_t& message);
     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);
@@ -131,6 +135,7 @@ private:
     QMutex _dataMutex;
     
     QmlObjectListModel  _missionItems;
+    int                 _currentMissionItem;
 };
 
 #endif