From ade4b92448ad5c543afd1da1ac47f5370d386ef4 Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Wed, 25 Oct 2017 10:04:15 -0700
Subject: [PATCH] Fix MISSION_CURRENT handling
Also fixed bug with ArduPilot guided waypoint write which would trash
existing mission.
---
src/MissionManager/MissionManager.cc | 56 ++++++++++++++++++++++++++-
src/MissionManager/MissionManager.h | 21 ++++++++++-
src/MissionManager/PlanManager.cc | 73 +++++++++++++-----------------------
src/MissionManager/PlanManager.h | 3 +-
4 files changed, 102 insertions(+), 51 deletions(-)
diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc
index c61eb2c..0bf4501 100644
--- a/src/MissionManager/MissionManager.cc
+++ b/src/MissionManager/MissionManager.cc
@@ -22,9 +22,10 @@
QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
MissionManager::MissionManager(Vehicle* vehicle)
- : PlanManager(vehicle, MAV_MISSION_TYPE_MISSION)
+ : PlanManager (vehicle, MAV_MISSION_TYPE_MISSION)
+ , _cachedLastCurrentIndex (-1)
{
-
+ connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
}
MissionManager::~MissionManager()
@@ -219,3 +220,54 @@ void MissionManager::generateResumeMission(int resumeIndex)
resumeMission[i]->deleteLater();
}
}
+
+/// Called when a new mavlink message for out vehicle is received
+void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
+{
+ switch (message.msgid) {
+ case MAVLINK_MSG_ID_MISSION_CURRENT:
+ _handleMissionCurrent(message);
+ break;
+
+ case MAVLINK_MSG_ID_HEARTBEAT:
+ _handleHeartbeat(message);
+ break;
+ }
+}
+
+void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
+{
+ 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;
+ emit currentIndexChanged(_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);
+ }
+}
+
diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h
index bf6936c..e34d6e3 100644
--- a/src/MissionManager/MissionManager.h
+++ b/src/MissionManager/MissionManager.h
@@ -22,7 +22,13 @@ class MissionManager : public PlanManager
public:
MissionManager(Vehicle* vehicle);
~MissionManager();
-
+
+ /// Current mission item as reported by MISSION_CURRENT
+ int currentIndex(void) const { return _currentMissionIndex; }
+
+ /// Last current mission item reported while in Mission flight mode
+ int lastCurrentIndex(void) const { return _lastCurrentIndex; }
+
/// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
/// @param gotoCoord Coordinate to move to
/// @param altChangeOnly true: only altitude change, false: lat/lon/alt change
@@ -31,6 +37,19 @@ public:
/// Generates a new mission which starts from the specified index. It will include all the CMD_DO items
/// from mission start to resumeIndex in the generate mission.
void generateResumeMission(int resumeIndex);
+
+signals:
+ void currentIndexChanged (int currentIndex);
+ void lastCurrentIndexChanged (int lastCurrentIndex);
+
+private slots:
+ void _mavlinkMessageReceived(const mavlink_message_t& message);
+
+private:
+ void _handleMissionCurrent(const mavlink_message_t& message);
+ void _handleHeartbeat(const mavlink_message_t& message);
+
+ int _cachedLastCurrentIndex;
};
#endif
diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc
index 53c95ed..6ad7fbf 100644
--- a/src/MissionManager/PlanManager.cc
+++ b/src/MissionManager/PlanManager.cc
@@ -634,6 +634,7 @@ void PlanManager::_handleMissionAck(const mavlink_message_t& message)
break;
}
}
+
/// Called when a new mavlink message for out vehicle is received
void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
@@ -661,14 +662,6 @@ void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message)
case MAVLINK_MSG_ID_MISSION_ACK:
_handleMissionAck(message);
break;
-
- case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
- // FIXME: NYI
- break;
-
- case MAVLINK_MSG_ID_MISSION_CURRENT:
- _handleMissionCurrent(message);
- break;
}
}
@@ -800,7 +793,7 @@ QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result)
return resultString + lastRequestString;
}
-void PlanManager::_finishTransaction(bool success)
+void PlanManager::_finishTransaction(bool success, bool apmGuidedItemWrite)
{
emit progressPct(1);
_disconnectFromMavlink();
@@ -826,22 +819,27 @@ void PlanManager::_finishTransaction(bool success)
emit newMissionItemsAvailable(false);
break;
case TransactionWrite:
- if (success) {
- // Write succeeded, update internal list to be current
- _currentMissionIndex = -1;
- _lastCurrentIndex = -1;
- emit currentIndexChanged(-1);
- emit lastCurrentIndexChanged(-1);
- _clearAndDeleteMissionItems();
- for (int i=0; i<_writeMissionItems.count(); i++) {
- _missionItems.append(_writeMissionItems[i]);
+ // No need to do anything for ArduPilot guided go to waypoint write
+ if (!apmGuidedItemWrite) {
+ if (success) {
+ // Write succeeded, update internal list to be current
+ if (_planType == MAV_MISSION_TYPE_MISSION) {
+ _currentMissionIndex = -1;
+ _lastCurrentIndex = -1;
+ emit currentIndexChanged(-1);
+ emit lastCurrentIndexChanged(-1);
+ }
+ _clearAndDeleteMissionItems();
+ for (int i=0; i<_writeMissionItems.count(); i++) {
+ _missionItems.append(_writeMissionItems[i]);
+ }
+ _writeMissionItems.clear();
+ } else {
+ // Write failed, throw out the write list
+ _clearAndDeleteWriteMissionItems();
}
- _writeMissionItems.clear();
- } else {
- // Write failed, throw out the write list
- _clearAndDeleteWriteMissionItems();
+ emit sendComplete(!success /* error */);
}
- emit sendComplete(!success /* error */);
break;
case TransactionRemoveAll:
emit removeAllComplete(!success /* error */);
@@ -865,25 +863,6 @@ bool PlanManager::inProgress(void) const
return _transactionInProgress != TransactionNone;
}
-void PlanManager::_handleMissionCurrent(const mavlink_message_t& message)
-{
- mavlink_mission_current_t missionCurrent;
-
- mavlink_msg_mission_current_decode(&message, &missionCurrent);
-
- if (missionCurrent.seq != _currentMissionIndex) {
- qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 currentIndex:").arg(_planTypeString()) << missionCurrent.seq;
- _currentMissionIndex = missionCurrent.seq;
- emit currentIndexChanged(_currentMissionIndex);
- }
-
- if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
- qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 lastCurrentIndex:").arg(_planTypeString()) << _currentMissionIndex;
- _lastCurrentIndex = _currentMissionIndex;
- emit lastCurrentIndexChanged(_lastCurrentIndex);
- }
-}
-
void PlanManager::_removeAllWorker(void)
{
mavlink_message_t message;
@@ -915,10 +894,12 @@ void PlanManager::removeAll(void)
_clearAndDeleteMissionItems();
- _currentMissionIndex = -1;
- _lastCurrentIndex = -1;
- emit currentIndexChanged(-1);
- emit lastCurrentIndexChanged(-1);
+ if (_planType == MAV_MISSION_TYPE_MISSION) {
+ _currentMissionIndex = -1;
+ _lastCurrentIndex = -1;
+ emit currentIndexChanged(-1);
+ emit lastCurrentIndexChanged(-1);
+ }
_transactionInProgress = TransactionRemoveAll;
_retryCount = 0;
diff --git a/src/MissionManager/PlanManager.h b/src/MissionManager/PlanManager.h
index 43f696c..aeda03a 100644
--- a/src/MissionManager/PlanManager.h
+++ b/src/MissionManager/PlanManager.h
@@ -112,13 +112,12 @@ protected:
void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
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);
QString _ackTypeToString(AckType_t ackType);
QString _missionResultToString(MAV_MISSION_RESULT result);
- void _finishTransaction(bool success);
+ void _finishTransaction(bool success, bool apmGuidedItemWrite = false);
void _requestList(void);
void _writeMissionCount(void);
void _writeMissionItemsWorker(void);