Skip to content

Q
qgroundcontrol
Commit cfe59f34 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4221 from DonLakeFlyer/MissionProtocolHardening 

Harden mission protocol to lost packets
parents cbfa7cc3 4c604d0d
Show whitespace changes
Inline Side-by-side
Showing with 259 additions and 154 deletions
src/MissionManager/MissionManager.cc
View file @ cfe59f34
...@@ -23,7 +23,7 @@ MissionManager::MissionManager(Vehicle* vehicle) ...@@ -23,7 +23,7 @@ MissionManager::MissionManager(Vehicle* vehicle)
: _vehicle(vehicle) : _vehicle(vehicle)
, _dedicatedLink(NULL) , _dedicatedLink(NULL)
, _ackTimeoutTimer(NULL) , _ackTimeoutTimer(NULL)
, _retryAck(AckNone) , _expectedAck(AckNone)
, _readTransactionInProgress(false) , _readTransactionInProgress(false)
, _writeTransactionInProgress(false) , _writeTransactionInProgress(false)
, _currentMissionItem(-1) , _currentMissionItem(-1)
...@@ -81,7 +81,17 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems) ...@@ -81,7 +81,17 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
for (int i=0; i<_missionItems.count(); i++) { for (int i=0; i<_missionItems.count(); i++) {
_itemIndicesToWrite << i; _itemIndicesToWrite << i;
} }
_writeTransactionInProgress = true; _writeTransactionInProgress = true;
_retryCount = 0;
emit inProgressChanged(true);
_writeMissionCount();
}
/// This begins the write sequence with the vehicle. This may be called during a retry.
void MissionManager::_writeMissionCount(void)
{
qCDebug(MissionManagerLog) << "_writeMissionCount retry count" << _retryCount;
mavlink_message_t message; mavlink_message_t message;
mavlink_mission_count_t missionCount; mavlink_mission_count_t missionCount;
...@@ -99,7 +109,6 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems) ...@@ -99,7 +109,6 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
_vehicle->sendMessageOnLink(_dedicatedLink, message); _vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionRequest); _startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
} }
void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly) void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly)
...@@ -154,10 +163,19 @@ void MissionManager::requestMissionItems(void) ...@@ -154,10 +163,19 @@ void MissionManager::requestMissionItems(void)
return; return;
} }
_retryCount = 0;
emit inProgressChanged(true);
_requestList();
}
/// Internal call to request list of mission items. May be called during a retry sequence.
void MissionManager::_requestList(void)
{
qCDebug(MissionManagerLog) << "_requestList retry count" << _retryCount;
mavlink_message_t message; mavlink_message_t message;
mavlink_mission_request_list_t request; mavlink_mission_request_list_t request;
_requestItemRetryCount = 0;
_itemIndicesToRead.clear(); _itemIndicesToRead.clear();
_readTransactionInProgress = true; _readTransactionInProgress = true;
_clearMissionItems(); _clearMissionItems();
...@@ -174,54 +192,98 @@ void MissionManager::requestMissionItems(void) ...@@ -174,54 +192,98 @@ void MissionManager::requestMissionItems(void)
_vehicle->sendMessageOnLink(_dedicatedLink, message); _vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionCount); _startAckTimeout(AckMissionCount);
emit inProgressChanged(true);
} }
void MissionManager::_ackTimeout(void) void MissionManager::_ackTimeout(void)
{ {
AckType_t timedOutAck = _retryAck; if (_expectedAck == AckNone) {
_retryAck = AckNone;
if (timedOutAck == AckNone) {
qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
_sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_retryAck == AckNone");
return; return;
} }
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(timedOutAck))); switch (_expectedAck) {
case AckNone:
qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
_sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone");
break;
case AckMissionCount:
// MISSION_COUNT message expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying REQUEST_LIST retry Count" << _retryCount;
_requestList();
}
break;
case AckMissionItem:
// MISSION_ITEM expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded."));
_finishTransaction(false); _finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_REQUEST retry Count" << _retryCount;
_requestNextMissionItem();
}
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (_itemIndicesToWrite.count() == 0) {
// Vehicle did not send final MISSION_ACK at end of sequence
_sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack."));
_finishTransaction(false);
} else if (_itemIndicesToWrite[0] == 0) {
// Vehicle did not respond to MISSION_COUNT, try again
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_COUNT retry Count" << _retryCount;
_writeMissionCount();
}
} else {
// Vehicle did not request all items from ground station
_sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
default:
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
} }
void MissionManager::_startAckTimeout(AckType_t ack) void MissionManager::_startAckTimeout(AckType_t ack)
{ {
_retryAck = ack; _expectedAck = ack;
_ackTimeoutTimer->start(); _ackTimeoutTimer->start();
} }
bool MissionManager::_stopAckTimeout(AckType_t expectedAck) /// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped.
/// @return true: received ack matches expected ack
bool MissionManager::_checkForExpectedAck(AckType_t receivedAck)
{ {
bool success = false; if (receivedAck == _expectedAck) {
AckType_t savedRetryAck = _retryAck; _expectedAck = AckNone;
_retryAck = AckNone;
_ackTimeoutTimer->stop(); _ackTimeoutTimer->stop();
return true;
if (savedRetryAck != expectedAck) { } else {
if (savedRetryAck == AckNone) { if (_expectedAck == AckNone) {
// Don't annoy the user with warnings about unexpected mission commands, just ignore them; ArduPilot updates home position using // Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using
// spurious MISSION_ITEMs. // spurious MISSION_ITEMs.
} else { } else {
_sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(_ackTypeToString(savedRetryAck)).arg(_ackTypeToString(expectedAck))); // We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers.
_finishTransaction(false); // Whatever it is we let the ack timeout handle any error output to the user.
qCDebug(MissionManagerLog) << QString("Out of sequence ack expected:received %1:%2").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck));
} }
success = false; return false;
} else {
success = true;
} }
return success;
} }
void MissionManager::_readTransactionComplete(void) void MissionManager::_readTransactionComplete(void)
...@@ -251,10 +313,12 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message) ...@@ -251,10 +313,12 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
{ {
mavlink_mission_count_t missionCount; mavlink_mission_count_t missionCount;
if (!_stopAckTimeout(AckMissionCount)) { if (!_checkForExpectedAck(AckMissionCount)) {
return; return;
} }
_retryCount = 0;
mavlink_msg_mission_count_decode(&message, &missionCount); mavlink_msg_mission_count_decode(&message, &missionCount);
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count; qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;
...@@ -271,13 +335,13 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message) ...@@ -271,13 +335,13 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
void MissionManager::_requestNextMissionItem(void) void MissionManager::_requestNextMissionItem(void)
{ {
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:" << _itemIndicesToRead[0];
if (_itemIndicesToRead.count() == 0) { if (_itemIndicesToRead.count() == 0) {
_sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read"); _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
return; return;
} }
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount;
mavlink_message_t message; mavlink_message_t message;
mavlink_mission_request_t missionRequest; mavlink_mission_request_t missionRequest;
...@@ -299,7 +363,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) ...@@ -299,7 +363,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
{ {
mavlink_mission_item_t missionItem; mavlink_mission_item_t missionItem;
if (!_stopAckTimeout(AckMissionItem)) { if (!_checkForExpectedAck(AckMissionItem)) {
return; return;
} }
...@@ -308,7 +372,6 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) ...@@ -308,7 +372,6 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq; qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq;
if (_itemIndicesToRead.contains(missionItem.seq)) { if (_itemIndicesToRead.contains(missionItem.seq)) {
_requestItemRetryCount = 0;
_itemIndicesToRead.removeOne(missionItem.seq); _itemIndicesToRead.removeOne(missionItem.seq);
MissionItem* item = new MissionItem(missionItem.seq, MissionItem* item = new MissionItem(missionItem.seq,
...@@ -333,13 +396,12 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) ...@@ -333,13 +396,12 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
_missionItems.append(item); _missionItems.append(item);
} else { } else {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq; qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq;
if (++_requestItemRetryCount > _maxRetryCount) { // We have to put the ack timeout back since it was removed above
_sendError(RequestRangeError, QString("Vehicle would not send item %1 after max retries. Read from Vehicle failed.").arg(_itemIndicesToRead[0])); _startAckTimeout(AckMissionItem);
_finishTransaction(false);
return; return;
} }
}
_retryCount = 0;
if (_itemIndicesToRead.count() == 0) { if (_itemIndicesToRead.count() == 0) {
_readTransactionComplete(); _readTransactionComplete();
} else { } else {
...@@ -357,7 +419,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) ...@@ -357,7 +419,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
{ {
mavlink_mission_request_t missionRequest; mavlink_mission_request_t missionRequest;
if (!_stopAckTimeout(AckMissionRequest)) { if (!_checkForExpectedAck(AckMissionRequest)) {
return; return;
} }
...@@ -411,14 +473,14 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) ...@@ -411,14 +473,14 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
{ {
mavlink_mission_ack_t missionAck; mavlink_mission_ack_t missionAck;
// Save the retry ack before calling _stopAckTimeout since we'll need it to determine what // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
// type of a protocol sequence we are in. // type of a protocol sequence we are in.
AckType_t savedRetryAck = _retryAck; AckType_t savedExpectedAck = _expectedAck;
// We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not // We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
// a protocol sequence error. Call _stopAckTimeout with _retryAck so it will succeed no // a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
// matter what. // matter what.
if (!_stopAckTimeout(_retryAck)) { if (!_checkForExpectedAck(_expectedAck)) {
return; return;
} }
...@@ -426,7 +488,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) ...@@ -426,7 +488,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
switch (savedRetryAck) { switch (savedExpectedAck) {
case AckNone: case AckNone:
// State machine is idle. Vehicle is confused. // State machine is idle. Vehicle is confused.
_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)));
...@@ -438,7 +500,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) ...@@ -438,7 +500,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
break; break;
case AckMissionItem: case AckMissionItem:
// MISSION_ITEM expected // MISSION_ITEM expected
_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.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false); _finishTransaction(false);
break; break;
case AckMissionRequest: case AckMissionRequest:
...@@ -459,7 +521,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) ...@@ -459,7 +521,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
case AckGuidedItem: case AckGuidedItem:
// 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) {
qCDebug(MissionManagerLog) << "_handleMissionAck guide mode item accepted"; qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted";
_finishTransaction(true); _finishTransaction(true);
} else { } else {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
...@@ -587,12 +649,14 @@ void MissionManager::_finishTransaction(bool success) ...@@ -587,12 +649,14 @@ void MissionManager::_finishTransaction(bool success)
emit newMissionItemsAvailable(); emit newMissionItemsAvailable();
} }
_readTransactionInProgress = false;
_writeTransactionInProgress = false;
_itemIndicesToRead.clear(); _itemIndicesToRead.clear();
_itemIndicesToWrite.clear(); _itemIndicesToWrite.clear();
if (_readTransactionInProgress || _writeTransactionInProgress) {
_readTransactionInProgress = false;
_writeTransactionInProgress = false;
emit inProgressChanged(false); emit inProgressChanged(false);
}
} }
bool MissionManager::inProgress(void) bool MissionManager::inProgress(void)
......
src/MissionManager/MissionManager.h
View file @ cfe59f34
...@@ -62,7 +62,7 @@ public: ...@@ -62,7 +62,7 @@ public:
} 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
static const int _ackTimeoutMilliseconds= 2000; static const int _ackTimeoutMilliseconds = 1000;
static const int _maxRetryCount = 5; static const int _maxRetryCount = 5;
signals: signals:
...@@ -85,7 +85,7 @@ private: ...@@ -85,7 +85,7 @@ private:
} AckType_t; } AckType_t;
void _startAckTimeout(AckType_t ack); void _startAckTimeout(AckType_t ack);
bool _stopAckTimeout(AckType_t expectedAck); bool _checkForExpectedAck(AckType_t receivedAck);
void _readTransactionComplete(void); void _readTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message); void _handleMissionCount(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message); void _handleMissionItem(const mavlink_message_t& message);
...@@ -98,14 +98,16 @@ private: ...@@ -98,14 +98,16 @@ private:
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); void _finishTransaction(bool success);
void _requestList(void);
void _writeMissionCount(void);
private: private:
Vehicle* _vehicle; Vehicle* _vehicle;
LinkInterface* _dedicatedLink; LinkInterface* _dedicatedLink;
QTimer* _ackTimeoutTimer; QTimer* _ackTimeoutTimer;
AckType_t _retryAck; AckType_t _expectedAck;
int _requestItemRetryCount; int _retryCount;
bool _readTransactionInProgress; bool _readTransactionInProgress;
bool _writeTransactionInProgress; bool _writeTransactionInProgress;
......
src/MissionManager/MissionManagerTest.cc
View file @ cfe59f34
...@@ -27,7 +27,7 @@ MissionManagerTest::MissionManagerTest(void) ...@@ -27,7 +27,7 @@ MissionManagerTest::MissionManagerTest(void)
} }
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode) void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
{ {
_mockLink->setMissionItemFailureMode(failureMode); _mockLink->setMissionItemFailureMode(failureMode);
...@@ -67,27 +67,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f ...@@ -67,27 +67,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
_multiSpyMissionManager->clearAllSignals(); _multiSpyMissionManager->clearAllSignals();
if (failureMode == MockLinkMissionItemHandler::FailNone) { if (shouldFail) {
// This should be clean run
// Wait for write sequence to complete. We should get:
// inProgressChanged(false) signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate item count in mission manager
int expectedCount = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
expectedCount++;
}
QCOMPARE(_missionManager->missionItems().count(), expectedCount);
} else {
// This should be a failed run // This should be a failed run
setExpectedMessageBox(QMessageBox::Ok); setExpectedMessageBox(QMessageBox::Ok);
...@@ -108,14 +88,34 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f ...@@ -108,14 +88,34 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
qDebug() << signalArgs[1].toString(); qDebug() << signalArgs[1].toString();
checkExpectedMessageBox(); checkExpectedMessageBox();
} else {
// This should be clean run
// Wait for write sequence to complete. We should get:
// inProgressChanged(false) signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate item count in mission manager
int expectedCount = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
expectedCount++;
}
QCOMPARE(_missionManager->missionItems().count(), expectedCount);
} }
_multiSpyMissionManager->clearAllSignals(); _multiSpyMissionManager->clearAllSignals();
} }
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode) void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
{ {
_writeItems(MockLinkMissionItemHandler::FailNone); _writeItems(MockLinkMissionItemHandler::FailNone, false);
_mockLink->setMissionItemFailureMode(failureMode); _mockLink->setMissionItemFailureMode(failureMode);
...@@ -129,17 +129,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode ...@@ -129,17 +129,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
_multiSpyMissionManager->clearAllSignals(); _multiSpyMissionManager->clearAllSignals();
if (failureMode == MockLinkMissionItemHandler::FailNone) { if (shouldFail) {
// This should be clean run
// Now wait for read sequence to complete. We should get:
// inProgressChanged(false) signal to signal completion
// newMissionItemsAvailable signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
_checkInProgressValues(false);
} else {
// This should be a failed run // This should be a failed run
setExpectedMessageBox(QMessageBox::Ok); setExpectedMessageBox(QMessageBox::Ok);
...@@ -161,6 +151,15 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode ...@@ -161,6 +151,15 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
qDebug() << signalArgs[1].toString(); qDebug() << signalArgs[1].toString();
checkExpectedMessageBox(); checkExpectedMessageBox();
} else {
// This should be clean run
// Now wait for read sequence to complete. We should get:
// inProgressChanged(false) signal to signal completion
// newMissionItemsAvailable signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
_checkInProgressValues(false);
} }
_multiSpyMissionManager->clearAllSignals(); _multiSpyMissionManager->clearAllSignals();
...@@ -169,14 +168,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode ...@@ -169,14 +168,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
size_t cMissionItemsExpected; size_t cMissionItemsExpected;
if (failureMode == MockLinkMissionItemHandler::FailNone) { if (shouldFail) {
cMissionItemsExpected = 0;
} else {
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 {
cMissionItemsExpected = 0;
} }
QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected); QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected);
...@@ -233,24 +232,27 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void) ...@@ -233,24 +232,27 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void)
typedef struct { typedef struct {
const char* failureText; const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode; MockLinkMissionItemHandler::FailureMode_t failureMode;
} TestCase_t; bool shouldFail;
} WriteTestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone }, static const WriteTestCase_t rgTestCases[] = {
{ "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse }, { "No Failure", MockLinkMissionItemHandler::FailNone, false },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse }, { "FailWriteMissionCountNoResponse", MockLinkMissionItemHandler::FailWriteMissionCountNoResponse, true },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence }, { "FailWriteMissionCountFirstResponse", MockLinkMissionItemHandler::FailWriteMissionCountFirstResponse, false },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence }, { "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse, true },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck }, { "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence, true },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck }, { "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence, true },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse }, { "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck, true },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck }, { "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, true },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests }, { "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse, true },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck, true },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests, true },
}; };
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; const WriteTestCase_t* pCase = &rgTestCases[i];
_writeItems(rgTestCases[i].failureMode); qDebug() << "TEST CASE " << pCase->failureText;
_writeItems(pCase->failureMode, pCase->shouldFail);
_mockLink->resetMissionItemHandler(); _mockLink->resetMissionItemHandler();
} }
} }
...@@ -271,22 +273,31 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void) ...@@ -271,22 +273,31 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void)
typedef struct { typedef struct {
const char* failureText; const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode; MockLinkMissionItemHandler::FailureMode_t failureMode;
} TestCase_t; bool shouldFail;
} ReadTestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone }, /*
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse }, static const ReadTestCase_t rgTestCases[] = {
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse }, { "FailReadRequest1FirstResponse", MockLinkMissionItemHandler::FailReadRequest1FirstResponse, false },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse }, };*/
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence }, static const ReadTestCase_t rgTestCases[] = {
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck }, { "No Failure", MockLinkMissionItemHandler::FailNone, false },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck }, { "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse, true },
{ "FailReadRequestListFirstResponse", MockLinkMissionItemHandler::FailReadRequestListFirstResponse, false },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse, true },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse, true },
{ "FailReadRequest1FirstResponse", MockLinkMissionItemHandler::FailReadRequest1FirstResponse, false },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence, true },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence, true },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck, true },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck, true },
}; };
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; const ReadTestCase_t* pCase = &rgTestCases[i];
_roundTripItems(rgTestCases[i].failureMode); qDebug() << "TEST CASE " << pCase->failureText;
_roundTripItems(pCase->failureMode, pCase->shouldFail);
_mockLink->resetMissionItemHandler(); _mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals(); _multiSpyMissionManager->clearAllSignals();
} }
......
src/MissionManager/MissionManagerTest.h
View file @ cfe59f34
...@@ -33,12 +33,12 @@ private slots: ...@@ -33,12 +33,12 @@ private slots:
void _testReadFailureHandlingAPM(void); void _testReadFailureHandlingAPM(void);
private: private:
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode); void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode); void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail);
void _testWriteFailureHandlingWorker(void); void _testWriteFailureHandlingWorker(void);
void _testReadFailureHandlingWorker(void); void _testReadFailureHandlingWorker(void);
static const MissionControllerManagerTest::TestCase_t _rgTestCases[]; static const TestCase_t _rgTestCases[];
static const size_t _cTestCases; static const size_t _cTestCases;
}; };
......
src/comm/MockLinkMissionItemHandler.cc
View file @ cfe59f34
...@@ -21,6 +21,9 @@ MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLi ...@@ -21,6 +21,9 @@ MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLi
, _failureMode(FailNone) , _failureMode(FailNone)
, _sendHomePositionOnEmptyList(false) , _sendHomePositionOnEmptyList(false)
, _mavlinkProtocol(mavlinkProtocol) , _mavlinkProtocol(mavlinkProtocol)
, _failReadRequestListFirstResponse(true)
, _failReadRequest1FirstResponse(true)
, _failWriteMissionCountFirstResponse(true)
{ {
Q_ASSERT(mockLink); Q_ASSERT(mockLink);
} }
...@@ -82,9 +85,17 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message ...@@ -82,9 +85,17 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
{ {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence"; qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
if (_failureMode != FailReadRequestListNoResponse) { _failReadRequest1FirstResponse = true;
if (_failureMode == FailReadRequestListNoResponse) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListNoResponse";
} else if (_failureMode == FailReadRequestListFirstResponse && _failReadRequestListFirstResponse) {
_failReadRequestListFirstResponse = false;
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListFirstResponse";
} else {
mavlink_mission_request_list_t request; mavlink_mission_request_list_t request;
_failReadRequestListFirstResponse = true;
mavlink_msg_mission_request_list_decode(&msg, &request); mavlink_msg_mission_request_list_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId()); Q_ASSERT(request.target_system == _mockLink->vehicleId());
...@@ -104,8 +115,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message ...@@ -104,8 +115,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
msg.compid, // Target is original sender msg.compid, // Target is original sender
itemCount); // Number of mission items itemCount); // Number of mission items
_mockLink->respondWithMavlinkMessage(responseMsg); _mockLink->respondWithMavlinkMessage(responseMsg);
} else {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
} }
} }
...@@ -120,9 +129,13 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& ...@@ -120,9 +129,13 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
Q_ASSERT(request.target_system == _mockLink->vehicleId()); Q_ASSERT(request.target_system == _mockLink->vehicleId());
Q_ASSERT(request.seq < _missionItems.count()); Q_ASSERT(request.seq < _missionItems.count());
if ((_failureMode == FailReadRequest0NoResponse && request.seq == 0) || if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
(_failureMode == FailReadRequest1NoResponse && request.seq == 1)) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode"; } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1NoResponse";
} else if (_failureMode == FailReadRequest1FirstResponse && request.seq == 1 && _failReadRequest1FirstResponse) {
_failReadRequest1FirstResponse = false;
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1FirstResponse";
} else { } else {
// FIXME: Track whether all items are requested, or requested in sequence // FIXME: Track whether all items are requested, or requested in sequence
...@@ -185,6 +198,16 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms ...@@ -185,6 +198,16 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
if (_writeSequenceCount == 0) { if (_writeSequenceCount == 0) {
_sendAck(MAV_MISSION_ACCEPTED); _sendAck(MAV_MISSION_ACCEPTED);
} else { } else {
if (_failureMode == FailWriteMissionCountNoResponse) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
return;
}
if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) {
_failWriteMissionCountFirstResponse = false;
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
return;
}
_failWriteMissionCountFirstResponse = true;
_writeSequenceIndex = 0; _writeSequenceIndex = 0;
_requestNextMissionItem(_writeSequenceIndex); _requestNextMissionItem(_writeSequenceIndex);
} }
...@@ -194,9 +217,8 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) ...@@ -194,9 +217,8 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
{ {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode; qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
if ((_failureMode == FailWriteRequest0NoResponse && sequenceNumber == 0) || if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
(_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1)) { qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode";
} else { } else {
if (sequenceNumber >= _writeSequenceCount) { if (sequenceNumber >= _writeSequenceCount) {
qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount; qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
......
src/comm/MockLinkMissionItemHandler.h
View file @ cfe59f34
...@@ -42,18 +42,21 @@ public: ...@@ -42,18 +42,21 @@ public:
typedef enum { typedef enum {
FailNone, // No failures FailNone, // No failures
FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST
FailReadRequestListFirstResponse, // Don't send MISSION_COUNT in response to first MISSION_REQUEST_LIST, allow subsequent to go through
FailReadRequest0NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 0 FailReadRequest0NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 0
FailReadRequest1NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 1 FailReadRequest1NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 1
FailReadRequest1FirstResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 1 on first try, allow subsequent request to go through
FailReadRequest0IncorrectSequence, // Respond to MISSION_REQUEST 0 with incorrect sequence number in MISSION_ITEM FailReadRequest0IncorrectSequence, // Respond to MISSION_REQUEST 0 with incorrect sequence number in MISSION_ITEM
FailReadRequest1IncorrectSequence, // Respond to MISSION_REQUEST 1 with incorrect sequence number in MISSION_ITEM FailReadRequest1IncorrectSequence, // Respond to MISSION_REQUEST 1 with incorrect sequence number in MISSION_ITEM
FailReadRequest0ErrorAck, // Respond to MISSION_REQUEST 0 with MISSION_ACK error FailReadRequest0ErrorAck, // Respond to MISSION_REQUEST 0 with MISSION_ACK error
FailReadRequest1ErrorAck, // Respond to MISSION_REQUEST 1 bogus MISSION_ACK error FailReadRequest1ErrorAck, // Respond to MISSION_REQUEST 1 bogus MISSION_ACK error
FailWriteRequest0NoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0 FailWriteMissionCountNoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0
FailWriteMissionCountFirstResponse, // Don't respond to first MISSION_COUNT with MISSION_REQUEST 0, respond to subsequent MISSION_COUNT requests
FailWriteRequest1NoResponse, // Don't respond to MISSION_ITEM 0 with MISSION_REQUEST 1 FailWriteRequest1NoResponse, // Don't respond to MISSION_ITEM 0 with MISSION_REQUEST 1
FailWriteRequest0IncorrectSequence, // Respond to MISSION_COUNT 0 with MISSION_REQUEST with wrong sequence number FailWriteRequest0IncorrectSequence, // Item 0 MISSION_REQUEST sent has wrong sequence number
FailWriteRequest1IncorrectSequence, // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number FailWriteRequest1IncorrectSequence, // Item 1 MISSION_REQUEST sent has wrong sequence number
FailWriteRequest0ErrorAck, // Respond to MISSION_COUNT 0 with MISSION_ACK error FailWriteRequest0ErrorAck, // Instead of sending MISSION_REQUEST 0, send MISSION_ACK error
FailWriteRequest1ErrorAck, // Respond to MISSION_ITEM 0 with MISSION_ACK error FailWriteRequest1ErrorAck, // Instead of sending MISSION_REQUEST 1, send MISSION_ACK error
FailWriteFinalAckNoResponse, // Don't send the final MISSION_ACK FailWriteFinalAckNoResponse, // Don't send the final MISSION_ACK
FailWriteFinalAckErrorAck, // Send an error as the final MISSION_ACK FailWriteFinalAckErrorAck, // Send an error as the final MISSION_ACK
FailWriteFinalAckMissingRequests, // Send the MISSION_ACK before all items have been requested FailWriteFinalAckMissingRequests, // Send the MISSION_ACK before all items have been requested
...@@ -102,6 +105,9 @@ private: ...@@ -102,6 +105,9 @@ private:
FailureMode_t _failureMode; FailureMode_t _failureMode;
bool _sendHomePositionOnEmptyList; bool _sendHomePositionOnEmptyList;
MAVLinkProtocol* _mavlinkProtocol; MAVLinkProtocol* _mavlinkProtocol;
bool _failReadRequestListFirstResponse;
bool _failReadRequest1FirstResponse;
bool _failWriteMissionCountFirstResponse;
}; };
#endif #endif
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