Commit 638464bd authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #5786 from DonLakeFlyer/MissionItemCurrent

Fix MISSION_CURRENT handling
parents ba940e91 ade4b924
......@@ -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);
}
}
......@@ -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
......@@ -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;
......
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment