zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #2537 from DonLakeFlyer/MissionProtocol 

Mission protocol robustness rework
QGC4.4
Don Gagne 10 years ago
parent
5a48f92b36 4131c134d2
commit
fd1e047d79
8 changed files with 153 additions and 263 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 219
      src/MissionManager/MissionManager.cc
  2. 18
      src/MissionManager/MissionManager.h
  3. 91
      src/MissionManager/MissionManagerTest.cc
  4. 8
      src/MissionManager/MissionManagerTest.h
  5. 4
      src/comm/MockLink.cc
  6. 3
      src/comm/MockLink.h
  7. 21
      src/comm/MockLinkMissionItemHandler.cc
  8. 4
      src/comm/MockLinkMissionItemHandler.h

219
src/MissionManager/MissionManager.cc
Unescape View File

@ -34,9 +34,10 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") @@ -34,9 +34,10 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
MissionManager::MissionManager(Vehicle* vehicle)
: _vehicle(vehicle)
, _cMissionItems(0)
, _ackTimeoutTimer(NULL)
, _retryAck(AckNone)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
{
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();
_retryCount = 0;
_missionItems.clear();
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;
}
mavlink_message_t message;
mavlink_mission_count_t missionCount;
_expectedSequenceNumber = 0;
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";
// Prime write list
for (int i=0; i<_missionItems.count(); i++) {
_itemIndicesToWrite << i;
}
_writeTransactionInProgress = true;
mavlink_message_t message;
mavlink_mission_count_t missionCount;
_expectedSequenceNumber = 0;
missionCount.target_system = _vehicle->id();
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionCount.count = _missionItems.count();
@ -116,6 +100,7 @@ void MissionManager::_retryWrite(void) @@ -116,6 +100,7 @@ void MissionManager::_retryWrite(void)
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
}
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_mission_request_list_t request;
_retryCount = 0;
_requestItemRetryCount = 0;
_itemIndicesToRead.clear();
_readTransactionInProgress = true;
_clearMissionItems();
request.target_system = _vehicle->id();
@ -138,24 +125,6 @@ void MissionManager::requestMissionItems(void) @@ -138,24 +125,6 @@ void MissionManager::requestMissionItems(void)
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)
{
AckType_t timedOutAck = _retryAck;
@ -168,10 +137,8 @@ void MissionManager::_ackTimeout(void) @@ -168,10 +137,8 @@ void MissionManager::_ackTimeout(void)
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)));
}
_finishTransaction(false);
}
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();
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)));
}
_finishTransaction(false);
success = false;
} else {
success = true;
@ -219,7 +183,7 @@ void MissionManager::_readTransactionComplete(void) @@ -219,7 +183,7 @@ void MissionManager::_readTransactionComplete(void)
_vehicle->sendMessage(message);
emit newMissionItemsAvailable();
emit inProgressChanged(false);
_finishTransaction(true);
}
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);
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;
_cMissionItems = missionCount.count;
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << _cMissionItems;
if (_cMissionItems == 0) {
if (missionCount.count == 0) {
_readTransactionComplete();
} 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) {
qCWarning(MissionManagerLog) << "_requestNextMissionItem requested seqeuence number > item count sequenceNumber::_cMissionItems" << sequenceNumber << _cMissionItems;
_sendError(InternalError, QString("QGroundControl requested mission item outside of range (internal error): %1:%2").arg(sequenceNumber).arg(_cMissionItems));
if (_itemIndicesToRead.count() == 0) {
_sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
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_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = sequenceNumber;
_expectedSequenceNumber = sequenceNumber;
missionRequest.seq = _itemIndicesToRead[0];
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;
if (missionItem.seq != _expectedSequenceNumber) {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received out of sequence expected:actual" << _expectedSequenceNumber << missionItem.seq;
if (!_retrySequence(AckMissionItem)) {
_sendError(ItemMismatchError, QString("Vehicle returned incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionItem.seq));
}
return;
}
if (_itemIndicesToRead.contains(missionItem.seq)) {
_requestItemRetryCount = 0;
_itemIndicesToRead.removeOne(missionItem.seq);
MissionItem* item = new MissionItem(missionItem.seq,
(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,
this);
_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 (nextSequenceNumber == _cMissionItems) {
if (_itemIndicesToRead.count() == 0) {
_readTransactionComplete();
} else {
_requestNextMissionItem(nextSequenceNumber);
_requestNextMissionItem();
}
}
void MissionManager::_clearMissionItems(void)
{
_cMissionItems = 0;
_itemIndicesToRead.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;
if (missionRequest.seq != _expectedSequenceNumber) {
qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested: _expectedSequenceNumber:missionRequest.seq" << _expectedSequenceNumber << missionRequest.seq;
if (!_retrySequence(AckMissionRequest)) {
_sendError(ItemMismatchError, QString("Vehicle requested incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionRequest.seq));
}
if (!_itemIndicesToWrite.contains(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));
_finishTransaction(false);
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_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) {
case AckNone:
// 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)));
break;
case AckMissionCount:
// MISSION_COUNT message expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionCount)) {
_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);
break;
case AckMissionItem:
// 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)));
}
_finishTransaction(false);
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
if (_expectedSequenceNumber == _missionItems.count()) {
if (_itemIndicesToWrite.count() == 0) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
emit inProgressChanged(false);
_finishTransaction(true);
} else {
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle did not reqeust all items: _expectedSequenceNumber:_missionItems.count" << _expectedSequenceNumber << _missionItems.count();
if (!_retrySequence(AckMissionRequest)) {
_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()));
}
_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()));
_finishTransaction(false);
}
} else {
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionRequest)) {
_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));
}
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
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)
{
emit inProgressChanged(false);
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;
qCDebug(MissionManagerLog) << "Sending error" << errorCode << errorMsg;
switch (ackType) {
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;
}
emit error(errorCode, errorMsg);
}
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");
}
}
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;
}

18
src/MissionManager/MissionManager.h
Unescape View File

@ -52,7 +52,7 @@ public: @@ -52,7 +52,7 @@ public:
// Property accessors
bool inProgress(void) { return _retryAck != AckNone; }
bool inProgress(void);
QmlObjectListModel* missionItems(void) { return &_missionItems; }
// C++ methods
@ -76,6 +76,7 @@ public: @@ -76,6 +76,7 @@ public:
ItemMismatchError, ///< Vehicle returned item with seq # different than requested
VehicleError, ///< Vehicle returned error
MissingRequestsError, ///< Vehicle did not request all items during write sequence
MaxRetryExceeded, ///< Retry failed
} ErrorCode_t;
// 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 _handleMissionRequest(const mavlink_message_t& message);
void _handleMissionAck(const mavlink_message_t& message);
void _requestNextMissionItem(int sequenceNumber);
void _requestNextMissionItem(void);
void _clearMissionItems(void);
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 _missionResultToString(MAV_MISSION_RESULT result);
void _finishTransaction(bool success);
private:
Vehicle* _vehicle;
int _cMissionItems; ///< Mission items on vehicle
QTimer* _ackTimeoutTimer;
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;

91
src/MissionManager/MissionManagerTest.cc
Unescape View File

@ -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);
if (failFirstTimeOnly) {
// Should fail first time, then retry should succed
failureMode = MockLinkMissionItemHandler::FailNone;
}
_mockLink->setMissionItemFailureMode(failureMode);
// Setup our test case data
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();
QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString();
QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
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();
}
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);
if (failFirstTimeOnly) {
// Should fail first time, then retry should succed
failureMode = MockLinkMissionItemHandler::FailNone;
}
_mockLink->setMissionItemFailureMode(failureMode);
// Read the items back from the vehicle
_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();
QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString();
QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
checkExpectedMessageBox();
}
@ -202,30 +192,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode @@ -202,30 +192,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
size_t cMissionItemsExpected;
if (failureMode == MockLinkMissionItemHandler::FailNone || failFirstTimeOnly == true) {
if (failureMode == MockLinkMissionItemHandler::FailNone) {
cMissionItemsExpected = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
cMissionItemsExpected++;
}
} else {
switch (failureMode) {
case MockLinkMissionItemHandler::FailReadRequestListNoResponse:
case MockLinkMissionItemHandler::FailReadRequest0NoResponse:
case MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence:
case MockLinkMissionItemHandler::FailReadRequest0ErrorAck:
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);
@ -287,28 +261,24 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void) @@ -287,28 +261,24 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void)
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
MissionManager::ErrorCode_t errorCode;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, MissionManager::AckTimeoutError },
{ "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse, MissionManager::AckTimeoutError },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse, MissionManager::AckTimeoutError },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck, MissionManager::VehicleError },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, MissionManager::VehicleError },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse, MissionManager::AckTimeoutError },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck, MissionManager::VehicleError },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests, MissionManager::MissingRequestsError },
{ "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false";
_writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
_mockLink->resetMissionItemHandler();
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
_writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_writeItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
}
}
@ -329,27 +299,22 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void) @@ -329,27 +299,22 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void)
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
MissionManager::ErrorCode_t errorCode;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, MissionManager::AckTimeoutError },
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse, MissionManager::AckTimeoutError },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse, MissionManager::AckTimeoutError },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse, MissionManager::AckTimeoutError },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck, MissionManager::VehicleError },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck, MissionManager::VehicleError },
{ "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false";
_roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
_mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals();
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
_roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_roundTripItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals();
}

8
src/MissionManager/MissionManagerTest.h
Unescape View File

@ -40,14 +40,14 @@ public: @@ -40,14 +40,14 @@ public:
MissionManagerTest(void);
private slots:
void _testWriteFailureHandlingAPM(void);
void _testReadFailureHandlingAPM(void);
void _testWriteFailureHandlingPX4(void);
void _testWriteFailureHandlingAPM(void);
void _testReadFailureHandlingPX4(void);
void _testReadFailureHandlingAPM(void);
private:
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly);
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _testWriteFailureHandlingWorker(void);
void _testReadFailureHandlingWorker(void);

4
src/comm/MockLink.cc
Unescape View File

@ -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)

3
src/comm/MockLink.h
Unescape View File

@ -131,8 +131,7 @@ public: @@ -131,8 +131,7 @@ public:
/// Sets a failure mode for unit testing
/// @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, bool firstTimeOnly);
void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }

21
src/comm/MockLinkMissionItemHandler.cc
Unescape View File

@ -119,10 +119,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message @@ -119,10 +119,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
} else {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
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);
}
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
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();
}
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
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(!_missionItems.contains(missionItem.seq));
Q_ASSERT(missionItem.seq == _writeSequenceIndex);
_missionItems[missionItem.seq] = missionItem;
_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);
}
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
@ -330,9 +312,8 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void) @@ -330,9 +312,8 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
Q_ASSERT(false);
}
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly)
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
{
_failureFirstTimeOnly = firstTimeOnly;
_failureMode = failureMode;
}

4
src/comm/MockLinkMissionItemHandler.h
Unescape View File

@ -74,8 +74,7 @@ public: @@ -74,8 +74,7 @@ public:
/// Sets a failure mode for unit testing
/// @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, bool firstTimeOnly);
void setMissionItemFailureMode(FailureMode_t failureMode);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType);
@ -114,7 +113,6 @@ private: @@ -114,7 +113,6 @@ private:
QTimer* _missionItemResponseTimer;
FailureMode_t _failureMode;
bool _failureFirstTimeOnly;
bool _sendHomePositionOnEmptyList;
MAVLinkProtocol* _mavlinkProtocol;
};

Write Preview
Loading…
Cancel
Save