Skip to content

Q
qgroundcontrol
Commit 4131c134 authored by Don Gagne's avatar Don Gagne
Browse files
Options

Mission protocol robustness rework

parent 7f43851e
Show whitespace changes
Inline Side-by-side
Showing with 153 additions and 263 deletions
src/MissionManager/MissionManager.cc
View file @ 4131c134
...@@ -34,9 +34,10 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") ...@@ -34,9 +34,10 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
MissionManager::MissionManager(Vehicle* vehicle) MissionManager::MissionManager(Vehicle* vehicle)
: _vehicle(vehicle) : _vehicle(vehicle)
, _cMissionItems(0)
, _ackTimeoutTimer(NULL) , _ackTimeoutTimer(NULL)
, _retryAck(AckNone) , _retryAck(AckNone)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
{ {
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived); connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
...@@ -56,7 +57,6 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) ...@@ -56,7 +57,6 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
{ {
bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
_retryCount = 0;
_missionItems.clear(); _missionItems.clear();
int firstIndex = skipFirstItem ? 1 : 0; int firstIndex = skipFirstItem ? 1 : 0;
...@@ -83,31 +83,15 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) ...@@ -83,31 +83,15 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
return; return;
} }
mavlink_message_t message; // Prime write list
mavlink_mission_count_t missionCount; for (int i=0; i<_missionItems.count(); i++) {
_itemIndicesToWrite << i;
_expectedSequenceNumber = 0; }
_writeTransactionInProgress = true;
missionCount.target_system = _vehicle->id();
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionCount.count = _missionItems.count();
mavlink_msg_mission_count_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionCount);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
}
void MissionManager::_retryWrite(void)
{
qCDebug(MissionManagerLog) << "_retryWrite";
mavlink_message_t message; mavlink_message_t message;
mavlink_mission_count_t missionCount; mavlink_mission_count_t missionCount;
_expectedSequenceNumber = 0;
missionCount.target_system = _vehicle->id(); missionCount.target_system = _vehicle->id();
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionCount.count = _missionItems.count(); missionCount.count = _missionItems.count();
...@@ -116,6 +100,7 @@ void MissionManager::_retryWrite(void) ...@@ -116,6 +100,7 @@ void MissionManager::_retryWrite(void)
_vehicle->sendMessage(message); _vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest); _startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
} }
void MissionManager::requestMissionItems(void) void MissionManager::requestMissionItems(void)
...@@ -125,7 +110,9 @@ void MissionManager::requestMissionItems(void) ...@@ -125,7 +110,9 @@ void MissionManager::requestMissionItems(void)
mavlink_message_t message; mavlink_message_t message;
mavlink_mission_request_list_t request; mavlink_mission_request_list_t request;
_retryCount = 0; _requestItemRetryCount = 0;
_itemIndicesToRead.clear();
_readTransactionInProgress = true;
_clearMissionItems(); _clearMissionItems();
request.target_system = _vehicle->id(); request.target_system = _vehicle->id();
...@@ -138,24 +125,6 @@ void MissionManager::requestMissionItems(void) ...@@ -138,24 +125,6 @@ void MissionManager::requestMissionItems(void)
emit inProgressChanged(true); emit inProgressChanged(true);
} }
void MissionManager::_retryRead(void)
{
qCDebug(MissionManagerLog) << "_retryRead";
mavlink_message_t message;
mavlink_mission_request_list_t request;
_clearMissionItems();
request.target_system = _vehicle->id();
request.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_msg_mission_request_list_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &request);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionCount);
}
void MissionManager::_ackTimeout(void) void MissionManager::_ackTimeout(void)
{ {
AckType_t timedOutAck = _retryAck; AckType_t timedOutAck = _retryAck;
...@@ -168,10 +137,8 @@ void MissionManager::_ackTimeout(void) ...@@ -168,10 +137,8 @@ void MissionManager::_ackTimeout(void)
return; return;
} }
if (!_retrySequence(timedOutAck)) {
qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << _ackTypeToString(timedOutAck) << _retryCount;
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(timedOutAck))); _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(timedOutAck)));
} _finishTransaction(false);
} }
void MissionManager::_startAckTimeout(AckType_t ack) void MissionManager::_startAckTimeout(AckType_t ack)
...@@ -190,11 +157,8 @@ bool MissionManager::_stopAckTimeout(AckType_t expectedAck) ...@@ -190,11 +157,8 @@ bool MissionManager::_stopAckTimeout(AckType_t expectedAck)
_ackTimeoutTimer->stop(); _ackTimeoutTimer->stop();
if (savedRetryAck != expectedAck) { if (savedRetryAck != expectedAck) {
qCDebug(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << _ackTypeToString(savedRetryAck) << _ackTypeToString(expectedAck);
if (_retrySequence(expectedAck)) {
_sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(_ackTypeToString(savedRetryAck)).arg(_ackTypeToString(expectedAck))); _sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(_ackTypeToString(savedRetryAck)).arg(_ackTypeToString(expectedAck)));
} _finishTransaction(false);
success = false; success = false;
} else { } else {
success = true; success = true;
...@@ -219,7 +183,7 @@ void MissionManager::_readTransactionComplete(void) ...@@ -219,7 +183,7 @@ void MissionManager::_readTransactionComplete(void)
_vehicle->sendMessage(message); _vehicle->sendMessage(message);
emit newMissionItemsAvailable(); emit newMissionItemsAvailable();
emit inProgressChanged(false); _finishTransaction(true);
} }
void MissionManager::_handleMissionCount(const mavlink_message_t& message) void MissionManager::_handleMissionCount(const mavlink_message_t& message)
...@@ -231,24 +195,27 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message) ...@@ -231,24 +195,27 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
} }
mavlink_msg_mission_count_decode(&message, &missionCount); mavlink_msg_mission_count_decode(&message, &missionCount);
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;
_cMissionItems = missionCount.count; if (missionCount.count == 0) {
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << _cMissionItems;
if (_cMissionItems == 0) {
_readTransactionComplete(); _readTransactionComplete();
} else { } else {
_requestNextMissionItem(0); // Prime read list
for (int i=0; i<missionCount.count; i++) {
_itemIndicesToRead << i;
}
_requestNextMissionItem();
} }
} }
void MissionManager::_requestNextMissionItem(int sequenceNumber) void MissionManager::_requestNextMissionItem(void)
{ {
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:" << sequenceNumber; qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:" << _itemIndicesToRead[0];
if (sequenceNumber >= _cMissionItems) { if (_itemIndicesToRead.count() == 0) {
qCWarning(MissionManagerLog) << "_requestNextMissionItem requested seqeuence number > item count sequenceNumber::_cMissionItems" << sequenceNumber << _cMissionItems; _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
_sendError(InternalError, QString("QGroundControl requested mission item outside of range (internal error): %1:%2").arg(sequenceNumber).arg(_cMissionItems));
return; return;
} }
...@@ -257,8 +224,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber) ...@@ -257,8 +224,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber)
missionRequest.target_system = _vehicle->id(); missionRequest.target_system = _vehicle->id();
missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = sequenceNumber; missionRequest.seq = _itemIndicesToRead[0];
_expectedSequenceNumber = sequenceNumber;
mavlink_msg_mission_request_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionRequest); mavlink_msg_mission_request_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionRequest);
...@@ -278,13 +244,9 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) ...@@ -278,13 +244,9 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq; qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq;
if (missionItem.seq != _expectedSequenceNumber) { if (_itemIndicesToRead.contains(missionItem.seq)) {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received out of sequence expected:actual" << _expectedSequenceNumber << missionItem.seq; _requestItemRetryCount = 0;
if (!_retrySequence(AckMissionItem)) { _itemIndicesToRead.removeOne(missionItem.seq);
_sendError(ItemMismatchError, QString("Vehicle returned incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionItem.seq));
}
return;
}
MissionItem* item = new MissionItem(missionItem.seq, MissionItem* item = new MissionItem(missionItem.seq,
(MAV_CMD)missionItem.command, (MAV_CMD)missionItem.command,
...@@ -300,18 +262,25 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) ...@@ -300,18 +262,25 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
missionItem.current, missionItem.current,
this); this);
_missionItems.append(item); _missionItems.append(item);
} else {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq;
if (++_requestItemRetryCount > _maxRetryCount) {
_sendError(RequestRangeError, QString("Vehicle would not send item %1 after max retries. Read from Vehicle failed.").arg(_itemIndicesToRead[0]));
_finishTransaction(false);
return;
}
}
int nextSequenceNumber = missionItem.seq + 1; if (_itemIndicesToRead.count() == 0) {
if (nextSequenceNumber == _cMissionItems) {
_readTransactionComplete(); _readTransactionComplete();
} else { } else {
_requestNextMissionItem(nextSequenceNumber); _requestNextMissionItem();
} }
} }
void MissionManager::_clearMissionItems(void) void MissionManager::_clearMissionItems(void)
{ {
_cMissionItems = 0; _itemIndicesToRead.clear();
_missionItems.clear(); _missionItems.clear();
} }
...@@ -327,16 +296,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) ...@@ -327,16 +296,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq; qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq;
if (missionRequest.seq != _expectedSequenceNumber) { if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested: _expectedSequenceNumber:missionRequest.seq" << _expectedSequenceNumber << missionRequest.seq; if (missionRequest.seq > _missionItems.count()) {
_sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq));
if (!_retrySequence(AckMissionRequest)) { _finishTransaction(false);
_sendError(ItemMismatchError, QString("Vehicle requested incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionRequest.seq));
}
return; return;
} else {
qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
}
} else {
_itemIndicesToWrite.removeOne(missionRequest.seq);
} }
_expectedSequenceNumber++;
mavlink_message_t messageOut; mavlink_message_t messageOut;
mavlink_mission_item_t missionItem; mavlink_mission_item_t missionItem;
...@@ -386,41 +356,31 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) ...@@ -386,41 +356,31 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
switch (savedRetryAck) { switch (savedRetryAck) {
case AckNone: case AckNone:
// State machine is idle. Vehicle is confused. // State machine is idle. Vehicle is confused.
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break; break;
case AckMissionCount: case AckMissionCount:
// MISSION_COUNT message expected // MISSION_COUNT message expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
if (!_retrySequence(AckMissionCount)) { _finishTransaction(false);
_sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
}
break; break;
case AckMissionItem: case AckMissionItem:
// MISSION_ITEM expected // MISSION_ITEM expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionItem)) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
} _finishTransaction(false);
break; break;
case AckMissionRequest: case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) { if (missionAck.type == MAV_MISSION_ACCEPTED) {
if (_expectedSequenceNumber == _missionItems.count()) { if (_itemIndicesToWrite.count() == 0) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
emit inProgressChanged(false); _finishTransaction(true);
} else { } else {
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle did not reqeust all items: _expectedSequenceNumber:_missionItems.count" << _expectedSequenceNumber << _missionItems.count(); _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1. Vehicle only has partial list of mission items.").arg(_itemIndicesToWrite.count()));
if (!_retrySequence(AckMissionRequest)) { _finishTransaction(false);
_sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence %1:%2. Vehicle only has partial list of mission items.").arg(_expectedSequenceNumber).arg(_missionItems.count()));
}
} }
} else { } else {
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
if (!_retrySequence(AckMissionRequest)) { _finishTransaction(false);
_sendError(VehicleError,
QString("Vehicle returned error: %1 on item %2. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)).arg(_expectedSequenceNumber));
}
} }
break; break;
} }
...@@ -469,43 +429,9 @@ QmlObjectListModel* MissionManager::copyMissionItems(void) ...@@ -469,43 +429,9 @@ QmlObjectListModel* MissionManager::copyMissionItems(void)
void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
{ {
emit inProgressChanged(false); qCDebug(MissionManagerLog) << "Sending error" << errorCode << errorMsg;
emit error(errorCode, errorMsg);
}
/// Retry the protocol sequence given the specified ack
/// @return true: sequence retried, false: out of retries
bool MissionManager::_retrySequence(AckType_t ackType)
{
qCDebug(MissionManagerLog) << "_retrySequence ackType:" << _ackTypeToString(ackType) << "_retryCount" << _retryCount;
switch (ackType) { emit error(errorCode, errorMsg);
case AckMissionCount:
case AckMissionItem:
if (++_retryCount <= _maxRetryCount) {
// We are in the middle of a read sequence, start over
_retryRead();
return true;
} else {
// Read sequence failed, signal for what we have up to this point
emit newMissionItemsAvailable();
return false;
}
break;
case AckMissionRequest:
if (++_retryCount <= _maxRetryCount) {
// We are in the middle of a write sequence, start over
_retryWrite();
return true;
} else {
return false;
}
break;
default:
qCWarning(MissionManagerLog) << "_retrySequence fell through switch: ackType:" << _ackTypeToString(ackType);
_sendError(InternalError, QString("Internal error occured during Mission Item communication: _retrySequence fell through switch: ackType:").arg(_ackTypeToString(ackType)));
return false;
}
} }
QString MissionManager::_ackTypeToString(AckType_t ackType) QString MissionManager::_ackTypeToString(AckType_t ackType)
...@@ -578,3 +504,24 @@ QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result) ...@@ -578,3 +504,24 @@ QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result)
return QString("QGC Internal Error"); return QString("QGC Internal Error");
} }
} }
void MissionManager::_finishTransaction(bool success)
{
if (!success && _readTransactionInProgress) {
// Read from vehicle failed, clear partial list
_missionItems.clear();
emit newMissionItemsAvailable();
}
_readTransactionInProgress = false;
_writeTransactionInProgress = false;
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
emit inProgressChanged(false);
}
bool MissionManager::inProgress(void)
{
return _readTransactionInProgress || _writeTransactionInProgress;
}
src/MissionManager/MissionManager.h
View file @ 4131c134
...@@ -52,7 +52,7 @@ public: ...@@ -52,7 +52,7 @@ public:
// Property accessors // Property accessors
bool inProgress(void) { return _retryAck != AckNone; } bool inProgress(void);
QmlObjectListModel* missionItems(void) { return &_missionItems; } QmlObjectListModel* missionItems(void) { return &_missionItems; }
// C++ methods // C++ methods
...@@ -76,6 +76,7 @@ public: ...@@ -76,6 +76,7 @@ public:
ItemMismatchError, ///< Vehicle returned item with seq # different than requested ItemMismatchError, ///< Vehicle returned item with seq # different than requested
VehicleError, ///< Vehicle returned error VehicleError, ///< Vehicle returned error
MissingRequestsError, ///< Vehicle did not request all items during write sequence MissingRequestsError, ///< Vehicle did not request all items during write sequence
MaxRetryExceeded, ///< Retry failed
} ErrorCode_t; } ErrorCode_t;
// These values are public so the unit test can set appropriate signal wait times // These values are public so the unit test can set appropriate signal wait times
...@@ -106,25 +107,24 @@ private: ...@@ -106,25 +107,24 @@ private:
void _handleMissionItem(const mavlink_message_t& message); void _handleMissionItem(const mavlink_message_t& message);
void _handleMissionRequest(const mavlink_message_t& message); void _handleMissionRequest(const mavlink_message_t& message);
void _handleMissionAck(const mavlink_message_t& message); void _handleMissionAck(const mavlink_message_t& message);
void _requestNextMissionItem(int sequenceNumber); void _requestNextMissionItem(void);
void _clearMissionItems(void); void _clearMissionItems(void);
void _sendError(ErrorCode_t errorCode, const QString& errorMsg); void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
void _retryWrite(void);
void _retryRead(void);
bool _retrySequence(AckType_t ackType);
QString _ackTypeToString(AckType_t ackType); QString _ackTypeToString(AckType_t ackType);
QString _missionResultToString(MAV_MISSION_RESULT result); QString _missionResultToString(MAV_MISSION_RESULT result);
void _finishTransaction(bool success);
private: private:
Vehicle* _vehicle; Vehicle* _vehicle;
int _cMissionItems; ///< Mission items on vehicle
QTimer* _ackTimeoutTimer; QTimer* _ackTimeoutTimer;
AckType_t _retryAck; AckType_t _retryAck;
int _retryCount; int _requestItemRetryCount;
int _expectedSequenceNumber; bool _readTransactionInProgress;
bool _writeTransactionInProgress;
QList<int> _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle
QList<int> _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle
QMutex _dataMutex; QMutex _dataMutex;
......
src/MissionManager/MissionManagerTest.cc
View file @ 4131c134
...@@ -42,13 +42,9 @@ MissionManagerTest::MissionManagerTest(void) ...@@ -42,13 +42,9 @@ MissionManagerTest::MissionManagerTest(void)
} }
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly) void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
{ {
_mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly); _mockLink->setMissionItemFailureMode(failureMode);
if (failFirstTimeOnly) {
// Should fail first time, then retry should succed
failureMode = MockLinkMissionItemHandler::FailNone;
}
// Setup our test case data // Setup our test case data
QmlObjectListModel* list = new QmlObjectListModel(); QmlObjectListModel* list = new QmlObjectListModel();
...@@ -131,7 +127,6 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f ...@@ -131,7 +127,6 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
QList<QVariant> signalArgs = spy->takeFirst(); QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 2); QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString(); qDebug() << signalArgs[1].toString();
QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
checkExpectedMessageBox(); checkExpectedMessageBox();
} }
...@@ -141,15 +136,11 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f ...@@ -141,15 +136,11 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
_multiSpyMissionManager->clearAllSignals(); _multiSpyMissionManager->clearAllSignals();
} }
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly) void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
{ {
_writeItems(MockLinkMissionItemHandler::FailNone, MissionManager::InternalError, false); _writeItems(MockLinkMissionItemHandler::FailNone);
_mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly); _mockLink->setMissionItemFailureMode(failureMode);
if (failFirstTimeOnly) {
// Should fail first time, then retry should succed
failureMode = MockLinkMissionItemHandler::FailNone;
}
// Read the items back from the vehicle // Read the items back from the vehicle
_missionManager->requestMissionItems(); _missionManager->requestMissionItems();
...@@ -191,7 +182,6 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode ...@@ -191,7 +182,6 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
QList<QVariant> signalArgs = spy->takeFirst(); QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 2); QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString(); qDebug() << signalArgs[1].toString();
QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
checkExpectedMessageBox(); checkExpectedMessageBox();
} }
...@@ -202,30 +192,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode ...@@ -202,30 +192,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
size_t cMissionItemsExpected; size_t cMissionItemsExpected;
if (failureMode == MockLinkMissionItemHandler::FailNone || failFirstTimeOnly == true) { if (failureMode == MockLinkMissionItemHandler::FailNone) {
cMissionItemsExpected = (int)_cTestCases; cMissionItemsExpected = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle // Home position at position 0 comes from vehicle
cMissionItemsExpected++; cMissionItemsExpected++;
} }
} else { } else {
switch (failureMode) {
case MockLinkMissionItemHandler::FailReadRequestListNoResponse:
case MockLinkMissionItemHandler::FailReadRequest0NoResponse:
case MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence:
case MockLinkMissionItemHandler::FailReadRequest0ErrorAck:
cMissionItemsExpected = 0; cMissionItemsExpected = 0;
break;
case MockLinkMissionItemHandler::FailReadRequest1NoResponse:
case MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence:
case MockLinkMissionItemHandler::FailReadRequest1ErrorAck:
cMissionItemsExpected = 1;
break;
default:
// Internal error
Q_ASSERT(false);
break;
}
} }
QCOMPARE(_missionManager->missionItems()->count(), (int)cMissionItemsExpected); QCOMPARE(_missionManager->missionItems()->count(), (int)cMissionItemsExpected);
...@@ -287,28 +261,24 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void) ...@@ -287,28 +261,24 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void)
typedef struct { typedef struct {
const char* failureText; const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode; MockLinkMissionItemHandler::FailureMode_t failureMode;
MissionManager::ErrorCode_t errorCode;
} TestCase_t; } TestCase_t;
static const TestCase_t rgTestCases[] = { static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, MissionManager::AckTimeoutError }, { "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse, MissionManager::AckTimeoutError }, { "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse, MissionManager::AckTimeoutError }, { "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence, MissionManager::ItemMismatchError }, { "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence, MissionManager::ItemMismatchError }, { "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck, MissionManager::VehicleError }, { "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, MissionManager::VehicleError }, { "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse, MissionManager::AckTimeoutError }, { "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck, MissionManager::VehicleError }, { "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests, MissionManager::MissingRequestsError }, { "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests },
}; };
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) { for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false"; qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false); _writeItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
_writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
_mockLink->resetMissionItemHandler(); _mockLink->resetMissionItemHandler();
} }
} }
...@@ -329,27 +299,22 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void) ...@@ -329,27 +299,22 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void)
typedef struct { typedef struct {
const char* failureText; const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode; MockLinkMissionItemHandler::FailureMode_t failureMode;
MissionManager::ErrorCode_t errorCode;
} TestCase_t; } TestCase_t;
static const TestCase_t rgTestCases[] = { static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, MissionManager::AckTimeoutError }, { "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse, MissionManager::AckTimeoutError }, { "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse, MissionManager::AckTimeoutError }, { "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse, MissionManager::AckTimeoutError }, { "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence, MissionManager::ItemMismatchError }, { "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence, MissionManager::ItemMismatchError }, { "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck, MissionManager::VehicleError }, { "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck, MissionManager::VehicleError }, { "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck },
}; };
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) { for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false"; qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false); _roundTripItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals();
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
_roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
_mockLink->resetMissionItemHandler(); _mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals(); _multiSpyMissionManager->clearAllSignals();
} }
......
src/MissionManager/MissionManagerTest.h
View file @ 4131c134
...@@ -40,14 +40,14 @@ public: ...@@ -40,14 +40,14 @@ public:
MissionManagerTest(void); MissionManagerTest(void);
private slots: private slots:
void _testWriteFailureHandlingAPM(void);
void _testReadFailureHandlingAPM(void);
void _testWriteFailureHandlingPX4(void); void _testWriteFailureHandlingPX4(void);
void _testWriteFailureHandlingAPM(void);
void _testReadFailureHandlingPX4(void); void _testReadFailureHandlingPX4(void);
void _testReadFailureHandlingAPM(void);
private: private:
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly); void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly); void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _testWriteFailureHandlingWorker(void); void _testWriteFailureHandlingWorker(void);
void _testReadFailureHandlingWorker(void); void _testReadFailureHandlingWorker(void);
......
src/comm/MockLink.cc
View file @ 4131c134
...@@ -783,9 +783,9 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) ...@@ -783,9 +783,9 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
} }
} }
void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly) void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)
{ {
_missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly); _missionItemHandler.setMissionItemFailureMode(failureMode);
} }
void MockLink::_sendHomePosition(void) void MockLink::_sendHomePosition(void)
......
src/comm/MockLink.h
View file @ 4131c134
...@@ -131,8 +131,7 @@ public: ...@@ -131,8 +131,7 @@ public:
/// Sets a failure mode for unit testing /// Sets a failure mode for unit testing
/// @param failureMode Type of failure to simulate /// @param failureMode Type of failure to simulate
/// @param firstTimeOnly true: fail first call, success subsequent calls, false: fail all calls void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode);
void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state /// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
......
src/comm/MockLinkMissionItemHandler.cc
View file @ 4131c134
...@@ -119,10 +119,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message ...@@ -119,10 +119,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
} else { } else {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode"; qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
} }
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
} }
void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg) void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
...@@ -181,10 +177,6 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& ...@@ -181,10 +177,6 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
_mockLink->respondWithMavlinkMessage(responseMsg); _mockLink->respondWithMavlinkMessage(responseMsg);
} }
} }
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
} }
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg) void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
...@@ -246,10 +238,6 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) ...@@ -246,10 +238,6 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
_startMissionItemResponseTimer(); _startMissionItemResponseTimer();
} }
} }
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
} }
void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
...@@ -279,9 +267,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg ...@@ -279,9 +267,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
Q_ASSERT(missionItem.target_system == _mockLink->vehicleId()); Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
Q_ASSERT(!_missionItems.contains(missionItem.seq));
Q_ASSERT(missionItem.seq == _writeSequenceIndex);
_missionItems[missionItem.seq] = missionItem; _missionItems[missionItem.seq] = missionItem;
_writeSequenceIndex++; _writeSequenceIndex++;
...@@ -302,9 +287,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg ...@@ -302,9 +287,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
_sendAck(ack); _sendAck(ack);
} }
} }
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
} }
void MockLinkMissionItemHandler::_missionItemResponseTimeout(void) void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
...@@ -330,9 +312,8 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void) ...@@ -330,9 +312,8 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
Q_ASSERT(false); Q_ASSERT(false);
} }
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly) void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
{ {
_failureFirstTimeOnly = firstTimeOnly;
_failureMode = failureMode; _failureMode = failureMode;
} }
......
src/comm/MockLinkMissionItemHandler.h
View file @ 4131c134
...@@ -74,8 +74,7 @@ public: ...@@ -74,8 +74,7 @@ public:
/// Sets a failure mode for unit testing /// Sets a failure mode for unit testing
/// @param failureMode Type of failure to simulate /// @param failureMode Type of failure to simulate
/// @param firstTimeOnly true: fail first call, success subsequent calls, false: fail all calls void setMissionItemFailureMode(FailureMode_t failureMode);
void setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state /// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType); void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType);
...@@ -114,7 +113,6 @@ private: ...@@ -114,7 +113,6 @@ private:
QTimer* _missionItemResponseTimer; QTimer* _missionItemResponseTimer;
FailureMode_t _failureMode; FailureMode_t _failureMode;
bool _failureFirstTimeOnly;
bool _sendHomePositionOnEmptyList; bool _sendHomePositionOnEmptyList;
MAVLinkProtocol* _mavlinkProtocol; MAVLinkProtocol* _mavlinkProtocol;
}; };
......
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