diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 01a52584d693dc4ee3dda64778bed0a31d43e00e..a8c67ba94c5cf5b8f39bb72ad3190fc055f31a8a 100644 --- a/src/MissionManager/MissionController.cc +++ b/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(_missionItems->get(i)); + item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber); + } + } +} diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index f8b2c1fb32ce91ab921f1697fc9d859e337192d9..8f6b860f9435d987e817b41521d8f1ba1cef2b86 100644 --- a/src/MissionManager/MissionController.h +++ b/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); diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 27b745553ee9820a75c90f4481a861199e910ffb..25616206a6b21fd067cade960c5839e7f8ff0931 100644 --- a/src/MissionManager/MissionManager.cc +++ b/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); + } +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 3c80333b47f17678311c13b3fc45d7a64e708e2f..e43e9a590e8189e23e0ed41d68f2d8cda9ce3373 100644 --- a/src/MissionManager/MissionManager.h +++ b/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