diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 2fa4cd40c7b41cfd58247add2d49097cdfd6ba59..29222dec349858dd3367476f578bca4ed2b4f3dd 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 6c6a792a7c7433bbe2daf525d1c447d28734a4ed..56993fd01b3c2d774837264d9ffa0dd0d0600c12 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 _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 0fc2454db918ef983aa5daa93d16fd7b809d37d8..db64f1a95427d643a316def4b1c332131d912468 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);