Skip to content
GitLab
Commit b05575fa authored by Don Gagne's avatar Don Gagne
Browse files

MissionManagerTest protocol hardening tests 

- Fixed bugs found to unit test
parent e352f247
Hide whitespace changes
Inline Side-by-side
src/MissionManager/MissionManager.cc
View file @ b05575fa
......@@ -55,6 +55,7 @@ MissionManager::~MissionManager()
void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
{
_retryCount = 0;
_missionItems.clear();
for (int i=0; i<missionItems.count(); i++) {
_missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i])));
......@@ -64,13 +65,14 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
if (inProgress()) {
qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress";
// FIXME: Better error handling
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();
......@@ -78,10 +80,29 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest, message);
_startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
}
void MissionManager::_retryWrite(void)
{
qCDebug(MissionManagerLog) << "_retryWrite";
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(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest);
}
void MissionManager::requestMissionItems(void)
{
qCDebug(MissionManagerLog) << "requestMissionItems read sequence";
......@@ -89,6 +110,7 @@ void MissionManager::requestMissionItems(void)
mavlink_message_t message;
mavlink_mission_request_list_t request;
_retryCount = 0;
_clearMissionItems();
request.target_system = _vehicle->id();
......@@ -97,49 +119,71 @@ void MissionManager::requestMissionItems(void)
mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &request);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionCount, message);
_startAckTimeout(AckMissionCount);
emit inProgressChanged(true);
}
void MissionManager::_retryRead(void)
{
qCDebug(MissionManagerLog) << "_retryRead";
mavlink_message_t message;
mavlink_mission_request_list_t request;
request.target_system = _vehicle->id();
request.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &request);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionCount);
emit inProgressChanged(true);
}
void MissionManager::_ackTimeout(void)
{
if (_retryAck == AckNone) {
AckType_t timedOutAck = _retryAck;
_retryAck = AckNone;
if (timedOutAck == AckNone) {
qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
_sendError(InternalError, "Internal error occured during Mission Item communication: _ackTimeOut:_retryAck == AckNone");
return;
}
if (++_retryCount <= _maxRetryCount) {
qCDebug(MissionManagerLog) << "_ackTimeout retry _retryAck:_retryCount" << _retryAck << _retryCount;
_vehicle->sendMessage(_retryMessage);
} else {
qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << _retryAck << _retryCount;
if (!_retrySequence(timedOutAck)) {
qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << timedOutAck << _retryCount;
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(timedOutAck));
}
}
void MissionManager::_startAckTimeout(AckType_t ack, const mavlink_message_t& message)
void MissionManager::_startAckTimeout(AckType_t ack)
{
_retryAck = ack;
_retryCount = 0;
_retryMessage = message;
_ackTimeoutTimer->start();
}
bool MissionManager::_stopAckTimeout(AckType_t expectedAck)
{
bool success;
bool success = false;
AckType_t savedRetryAck = _retryAck;
_retryAck = AckNone;
_ackTimeoutTimer->stop();
if (_retryAck != expectedAck) {
qCWarning(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << _retryAck << expectedAck;
if (savedRetryAck != expectedAck) {
qCDebug(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << savedRetryAck << expectedAck;
if (_retrySequence(expectedAck)) {
_sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(savedRetryAck).arg(expectedAck));
}
success = false;
} else {
success = true;
}
_retryAck = AckNone;
return success;
}
......@@ -189,6 +233,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber)
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));
return;
}
......@@ -203,7 +248,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber)
mavlink_msg_mission_request_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionRequest);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionItem, message);
_startAckTimeout(AckMissionItem);
}
void MissionManager::_handleMissionItem(const mavlink_message_t& message)
......@@ -220,6 +265,9 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
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;
}
......@@ -267,11 +315,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq;
if (missionRequest.seq >= _missionItems.count()) {
qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested:count" << missionRequest.seq << _missionItems.count();
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));
}
return;
}
_expectedSequenceNumber++;
mavlink_message_t messageOut;
mavlink_mission_item_t missionItem;
......@@ -295,8 +349,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
mavlink_msg_mission_item_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &messageOut, &missionItem);
_vehicle->sendMessage(messageOut);
// FIXME: This ack sequence isn't quite write
_startAckTimeout(AckMissionRequest, messageOut);
_startAckTimeout(AckMissionRequest);
}
void MissionManager::_handleMissionAck(const mavlink_message_t& message)
......@@ -309,14 +362,24 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
mavlink_msg_mission_ack_decode(&message, &missionAck);
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << missionAck.type;
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
if (_expectedSequenceNumber == _missionItems.count()) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
emit inProgressChanged(false);
} 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").arg(_expectedSequenceNumber).arg(_missionItems.count()));
}
}
} else {
_missionItems.clear();
emit newMissionItemsAvailable();
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type;
if (!_retrySequence(AckMissionRequest)) {
_sendError(VehicleError, QString("Vehicle returned error: %1").arg(missionAck.type));
}
}
emit inProgressChanged(false);
}
/// Called when a new mavlink message for out vehicle is received
......@@ -359,3 +422,39 @@ QmlObjectListModel* MissionManager::copyMissionItems(void)
return list;
}
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:" << ackType << "_retryCount" << _retryCount;
if (++_retryCount <= _maxRetryCount) {
switch (ackType) {
case AckMissionCount:
case AckMissionItem:
// We are in the middle of a read sequence, start over
_retryRead();
return true;
break;
case AckMissionRequest:
// We are in the middle of a write sequence, start over
_retryWrite();
return true;
break;
default:
qCWarning(MissionManagerLog) << "_retrySequence fell through switch: ackType:" << ackType;
_sendError(InternalError, QString("Internal error occured during Mission Item communication: _retrySequence fell through switch: ackType:").arg(ackType));
return false;
}
} else {
qCDebug(MissionManagerLog) << "Retries exhausted";
return false;
}
}
src/MissionManager/MissionManager.h
View file @ b05575fa
......@@ -67,12 +67,24 @@ public:
/// Returns a copy of the current set of mission items. Caller is responsible for
/// freeing returned object.
QmlObjectListModel* copyMissionItems(void);
/// Error codes returned in error signal
typedef enum {
InternalError,
AckTimeoutError, ///< Timed out waiting for response from vehicle
ProtocolOrderError, ///< Incorrect protocol sequence from vehicle
RequestRangeError, ///< Vehicle requested item out of range
ItemMismatchError, ///< Vehicle returned item with seq # different than requested
VehicleError, ///< Vehicle returned error
MissingRequestsError, ///< Vehicle did not request all items during write sequence
} ErrorCode_t;
signals:
// Public signals
void canEditChanged(bool canEdit);
void newMissionItemsAvailable(void);
void inProgressChanged(bool inProgress);
void error(int errorCode, const QString& errorMsg);
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
......@@ -86,7 +98,7 @@ private:
AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
} AckType_t;
void _startAckTimeout(AckType_t ack, const mavlink_message_t& message);
void _startAckTimeout(AckType_t ack);
bool _stopAckTimeout(AckType_t expectedAck);
void _sendTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message);
......@@ -95,6 +107,10 @@ private:
void _handleMissionAck(const mavlink_message_t& message);
void _requestNextMissionItem(int sequenceNumber);
void _clearMissionItems(void);
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
void _retryWrite(void);
void _retryRead(void);
bool _retrySequence(AckType_t ackType);
private:
Vehicle* _vehicle;
......@@ -104,7 +120,6 @@ private:
QTimer* _ackTimeoutTimer;
AckType_t _retryAck;
mavlink_message_t _retryMessage;
int _retryCount;
int _expectedSequenceNumber;
......@@ -113,7 +128,7 @@ private:
QmlObjectListModel _missionItems;
static const int _ackTimeoutMilliseconds= 1000;
static const int _ackTimeoutMilliseconds= 500;
static const int _maxRetryCount = 5;
};
......
src/MissionManager/MissionManagerTest.cc
View file @ b05575fa
......@@ -70,6 +70,7 @@ void MissionManagerTest::init(void)
_rgSignals[canEditChangedSignalIndex] = SIGNAL(canEditChanged(bool));
_rgSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(void));
_rgSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool));
_rgSignals[errorSignalIndex] = SIGNAL(error(int, const QString&));
_multiSpy = new MultiSignalSpy();
Q_CHECK_PTR(_multiSpy);
......@@ -133,8 +134,14 @@ void MissionManagerTest::_readEmptyVehicle(void)
QCOMPARE(_missionManager->canEdit(), true);
}
void MissionManagerTest::_roundTripItems(void)
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly)
{
_mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly);
if (failFirstTimeOnly) {
// Should fail first time, then retry should succed
failureMode = MockLinkMissionItemHandler::FailNone;
}
// Setup our test case data
const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
QmlObjectListModel* list = new QmlObjectListModel();
......@@ -152,7 +159,7 @@ void MissionManagerTest::_roundTripItems(void)
// Send the items to the vehicle
_missionManager->writeMissionItems(*list);
// writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress());
QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
......@@ -160,17 +167,74 @@ void MissionManagerTest::_roundTripItems(void)
_multiSpy->clearAllSignals();
// Now wait for write sequence to complete. We should only get an inProgressChanged signal to signal completion.
_multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000);
QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
_checkInProgressValues(false);
if (failureMode == MockLinkMissionItemHandler::FailNone) {
// This should be clean run
// Wait for write sequence to complete. We should get:
// inProgressChanged(false) signal
_multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 10000);
QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// We should have gotten back all mission items
QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
} else {
// This should be a failed run
// Wait for write sequence to complete. We should get:
// inProgressChanged(false) signal
// error(errorCode, QString) signal
_multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 10000);
QCOMPARE(_multiSpy->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate error signal values
QSignalSpy* spy = _multiSpy->getSpyByIndex(errorSignalIndex);
QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString();
QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
/*
// FIXME: This should be on the read side
// Validate correct number of mission items
int expectedMissionCount = 0;
switch (ErrorCode) {
case FailWriteRequest0NoResponse:
// Don't respond to MISSION_COUNT with MISSION_REQUEST 0
expectedMissionCount = 0;
break;
case FailWriteRequest1NoResponse: // Don't respond to MISSION_ITEM 0 with MISSION_REQUEST 1
case FailWriteRequest0IncorrectSequence: // Respond to MISSION_COUNT 0 with MISSION_REQUEST with wrong sequence number
case FailWriteRequest1IncorrectSequence: // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number
case FailWriteRequest0ErrorAck: // Respond to MISSION_COUNT 0 with MISSION_ACK error
case FailWriteRequest1ErrorAck: // Respond to MISSION_ITEM 0 with MISSION_ACK error
case FailWriteFinalAckNoResponse: // Don't send the final MISSION_ACK
case FailWriteFinalAckErrorAck: // Send an error as the final MISSION_ACK
case FailWriteFinalAckMissingRequests: // Send the MISSION_ACK before all items have been requested
break;
}
// FIXME: Count depends on errorCode
//QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
*/
}
QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
QCOMPARE(_missionManager->canEdit(), true);
delete list;
list = NULL;
_multiSpy->clearAllSignals();
}
void MissionManagerTest::_roundTripItems(void)
{
_writeItems(MockLinkMissionItemHandler::FailNone, MissionManager::InternalError, false);
// Read the items back from the vehicle
_missionManager->requestMissionItems();
......@@ -190,6 +254,7 @@ void MissionManagerTest::_roundTripItems(void)
QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true);
_checkInProgressValues(false);
const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
QCOMPARE(_missionManager->canEdit(), true);
......@@ -212,3 +277,64 @@ void MissionManagerTest::_roundTripItems(void)
QCOMPARE(actual->frame(), testCase->expectedItem.frame);
}
}
void MissionManagerTest::_testWriteFailureHandling(void)
{
/*
FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST
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
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
FailReadRequest0ErrorAck, // Respond to MISSION_REQUEST 0 with MISSION_ACK error
FailReadRequest1ErrorAck, // Respond to MISSION_REQUEST 1 bogus MISSION_ACK error
FailWriteRequest0NoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0
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
FailWriteRequest1IncorrectSequence, // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number
FailWriteRequest0ErrorAck, // Respond to MISSION_COUNT 0 with MISSION_ACK error
FailWriteRequest1ErrorAck, // Respond to MISSION_ITEM 0 with MISSION_ACK error
FailWriteFinalAckNoResponse, // Don't send the final MISSION_ACK
FailWriteFinalAckErrorAck, // Send an error as the final MISSION_ACK
FailWriteFinalAckMissingRequests, // Send the MISSION_ACK before all items have been requested
*/
/*
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
/// Called to send a MISSION_ITEM message while the MissionManager is in idle state
void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); }
/// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); }
*/
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 },
};
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);
_mockLink->resetMissionItemHandler();
}
}
src/MissionManager/MissionManagerTest.h
View file @ b05575fa
......@@ -42,9 +42,11 @@ private slots:
void _readEmptyVehicle(void);
void _roundTripItems(void);
void _testWriteFailureHandling(void);
private:
void _checkInProgressValues(bool inProgress);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly);
MockLink* _mockLink;
MissionManager* _missionManager;
......@@ -53,6 +55,7 @@ private:
canEditChangedSignalIndex = 0,
newMissionItemsAvailableSignalIndex,
inProgressChangedSignalIndex,
errorSignalIndex,
maxSignalIndex
};
......@@ -60,6 +63,7 @@ private:
canEditChangedSignalMask = 1 << canEditChangedSignalIndex,
newMissionItemsAvailableSignalMask = 1 << newMissionItemsAvailableSignalIndex,
inProgressChangedSignalMask = 1 << inProgressChangedSignalIndex,
errorSignalMask = 1 << errorSignalIndex,
};
MultiSignalSpy* _multiSpy;
......
src/Vehicle/Vehicle.cc
View file @ b05575fa
......@@ -85,6 +85,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
, _satelliteLock(0)
, _wpm(NULL)
, _updateCount(0)
, _missionManager(NULL)
, _armed(false)
, _base_mode(0)
, _custom_mode(0)
......@@ -164,6 +165,9 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
Vehicle::~Vehicle()
{
delete _missionManager;
_missionManager = NULL;
// Stop listening for system messages
disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage);
// Disconnect any previously connected active MAV
......
src/comm/MockLink.cc
View file @ b05575fa
......@@ -66,17 +66,18 @@ union px4_custom_mode {
float data_float;
};
MockLink::MockLink(MockConfiguration* config) :
_name("MockLink"),
_connected(false),
_vehicleSystemId(128), // FIXME: Pull from eventual parameter manager
_vehicleComponentId(200), // FIXME: magic number?
_inNSH(false),
_mavlinkStarted(false),
_mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
_mavState(MAV_STATE_STANDBY),
_autopilotType(MAV_AUTOPILOT_PX4),
_fileServer(NULL)
MockLink::MockLink(MockConfiguration* config)
: _missionItemHandler(this)
, _name("MockLink")
, _connected(false)
, _vehicleSystemId(128) // FIXME: Pull from eventual parameter manager
, _vehicleComponentId(200) // FIXME: magic number?
, _inNSH(false)
, _mavlinkStarted(false)
, _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
, _mavState(MAV_STATE_STANDBY)
, _autopilotType(MAV_AUTOPILOT_PX4)
, _fileServer(NULL)
{
_config = config;
union px4_custom_mode px4_cm;
......@@ -88,10 +89,8 @@ MockLink::MockLink(MockConfiguration* config) :
_fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this);
Q_CHECK_PTR(_fileServer);
_missionItemHandler = new MockLinkMissionItemHandler(this);
Q_CHECK_PTR(_missionItemHandler);
moveToThread(this);
_loadParams();
QObject::connect(this, &MockLink::_incomingBytes, this, &MockLink::_handleIncomingBytes);
}
......@@ -149,6 +148,8 @@ void MockLink::run(void)
QObject::disconnect(&_timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks);
QObject::disconnect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
QObject::disconnect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks);
_missionItemHandler.shutdown();
}
void MockLink::_run1HzTasks(void)
......@@ -303,8 +304,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
continue;
}
Q_ASSERT(_missionItemHandler);
if (_missionItemHandler->handleMessage(msg)) {
if (_missionItemHandler.handleMessage(msg)) {
continue;
}
......@@ -668,3 +668,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
}
}
void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly)
{
_missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly);
}
src/comm/MockLink.h
View file @ b05575fa
......@@ -85,6 +85,23 @@ public:
bool disconnect(void);
LinkConfiguration* getLinkConfiguration() { return _config; }
/// 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);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
/// Called to send a MISSION_ITEM message while the MissionManager is in idle state
void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); }
/// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); }
/// Reset the state of the MissionItemHandler to no items, no transactions in progress.
void resetMissionItemHandler(void) { _missionItemHandler.reset(); }
signals:
/// @brief Used internally to move data to the thread.
......@@ -126,7 +143,7 @@ private:
float _floatUnionForParam(int componentId, const QString& paramName);
void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
MockLinkMissionItemHandler* _missionItemHandler;
MockLinkMissionItemHandler _missionItemHandler;
QString _name;
bool _connected;
......
src/comm/MockLinkMissionItemHandler.cc
View file @ b05575fa
......@@ -29,12 +29,27 @@
QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")
MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink)
: QObject(mockLink)
, _mockLink(mockLink)
: _mockLink(mockLink)
, _missionItemResponseTimer(NULL)
, _failureMode(FailNone)
{
Q_ASSERT(mockLink);
}
MockLinkMissionItemHandler::~MockLinkMissionItemHandler()
{
}
void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
{
if (!_missionItemResponseTimer) {
_missionItemResponseTimer = new QTimer();
connect(_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout);
}
_missionItemResponseTimer->start(500);
}
bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
{
switch (msg.msgid) {
......@@ -78,21 +93,29 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
{
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
mavlink_mission_request_list_t request;
mavlink_msg_mission_request_list_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId());
mavlink_message_t responseMsg;
if (_failureMode != FailReadRequestListNoResponse) {
mavlink_mission_request_list_t request;
mavlink_msg_mission_request_list_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId());
mavlink_message_t responseMsg;
mavlink_msg_mission_count_pack(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
_missionItems.count()); // Number of mission items
_mockLink->respondWithMavlinkMessage(responseMsg);
} else {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
}
mavlink_msg_mission_count_pack(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
_missionItems.count()); // Number of mission items
_mockLink->respondWithMavlinkMessage(responseMsg);
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
......@@ -106,29 +129,50 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
Q_ASSERT(request.target_system == _mockLink->vehicleId());
Q_ASSERT(request.seq < _missionItems.count());
mavlink_message_t responseMsg;
mavlink_mission_item_t item = _missionItems[request.seq];
if ((_failureMode == FailReadRequest0NoResponse && request.seq != 0) ||
(_failureMode == FailReadRequest1NoResponse && request.seq != 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode";
} else {
// FIXME: Track whether all items are requested, or requested in sequence
if ((_failureMode == FailReadRequest0IncorrectSequence && request.seq == 0) ||
(_failureMode == FailReadRequest1IncorrectSequence && request.seq == 1)) {
// Send back the incorrect sequence number
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest sending bad sequence number";
request.seq++;
}
if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) ||
(_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
_sendAck(MAV_MISSION_ERROR);
} else {
mavlink_message_t responseMsg;
mavlink_mission_item_t item = _missionItems[request.seq];
mavlink_msg_mission_item_pack(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
request.seq, // Index of mission item being sent
item.frame,
item.command,
item.current,
item.autocontinue,
item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z);
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
mavlink_msg_mission_item_pack(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
request.seq, // Index of mission item being sent
item.frame,
item.command,
item.current,
item.autocontinue,
item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z);
_mockLink->respondWithMavlinkMessage(responseMsg);
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence";
mavlink_mission_count_t missionCount;
mavlink_msg_mission_count_decode(&msg, &missionCount);
......@@ -137,12 +181,12 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
_writeSequenceCount = missionCount.count;
Q_ASSERT(_writeSequenceCount >= 0);
// FIXME: Set up a timer for a failed write sequence
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
_missionItems.clear();
if (_writeSequenceCount == 0) {
// FIXME: NYI
_sendAck(MAV_MISSION_ACCEPTED);
} else {
_writeSequenceIndex = 0;
_requestNextMissionItem(_writeSequenceIndex);
......@@ -151,31 +195,68 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber;
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
if (sequenceNumber >= _writeSequenceCount) {
qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
return;
if ((_failureMode == FailWriteRequest0NoResponse && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode";
} else {
if (sequenceNumber >= _writeSequenceCount) {
qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
return;
}
if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
sequenceNumber ++;
}
if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
_sendAck(MAV_MISSION_ERROR);
} else {
mavlink_message_t message;
mavlink_mission_request_t missionRequest;
missionRequest.target_system = MAVLinkProtocol::instance()->getSystemId();
missionRequest.target_component = MAVLinkProtocol::instance()->getComponentId();
missionRequest.seq = sequenceNumber;
mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest);
_mockLink->respondWithMavlinkMessage(message);
// If response with Mission Item doesn't come before timer fires it's an error
_startMissionItemResponseTimer();
}
}
mavlink_message_t message;
mavlink_mission_request_t missionRequest;
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
mavlink_message_t message;
mavlink_mission_ack_t missionAck;
missionRequest.target_system = MAVLinkProtocol::instance()->getSystemId();
missionRequest.target_component = MAVLinkProtocol::instance()->getComponentId();
missionRequest.seq = sequenceNumber;
missionAck.target_system = MAVLinkProtocol::instance()->getSystemId();
missionAck.target_component = MAVLinkProtocol::instance()->getComponentId();
missionAck.type = ackType;
mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest);
mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck);
_mockLink->respondWithMavlinkMessage(message);
// FIXME: Timeouts
}
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
_missionItemResponseTimer->stop();
mavlink_mission_item_t missionItem;
mavlink_msg_mission_item_decode(&msg, &missionItem);
......@@ -187,21 +268,61 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
_missionItems[missionItem.seq] = missionItem;
// FIXME: Timeouts
_writeSequenceIndex++;
if (_writeSequenceIndex < _writeSequenceCount) {
_requestNextMissionItem(_writeSequenceIndex);
if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
// Send MAV_MISSION_ACCPETED ack too early
_sendAck(MAV_MISSION_ACCEPTED);
} else {
_requestNextMissionItem(_writeSequenceIndex);
}
} else {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem sending final ack, write sequence complete";
mavlink_message_t message;
mavlink_mission_ack_t missionAck;
missionAck.target_system = MAVLinkProtocol::instance()->getSystemId();
missionAck.target_component = MAVLinkProtocol::instance()->getComponentId();
missionAck.type = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck);
_mockLink->respondWithMavlinkMessage(message);
if (_failureMode != FailWriteFinalAckNoResponse) {
MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED;
if (_failureMode == FailWriteFinalAckErrorAck) {
ack = MAV_MISSION_ERROR;
}
_sendAck(ack);
}
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
{
qWarning() << "Timeout waiting for next MISSION_ITEM";
Q_ASSERT(false);
}
void MockLinkMissionItemHandler::sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType)
{
_sendAck(ackType);
}
void MockLinkMissionItemHandler::sendUnexpectedMissionItem(void)
{
// FIXME: NYI
Q_ASSERT(false);
}
void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
{
// FIXME: NYI
Q_ASSERT(false);
}
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly)
{
_failureFirstTimeOnly = firstTimeOnly;
_failureMode = failureMode;
}
void MockLinkMissionItemHandler::shutdown(void)
{
if (_missionItemResponseTimer) {
delete _missionItemResponseTimer;
}
}
src/comm/MockLinkMissionItemHandler.h
View file @ b05575fa
......@@ -26,6 +26,7 @@
#include <QObject>
#include <QMap>
#include <QTimer>
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
......@@ -40,11 +41,55 @@ class MockLinkMissionItemHandler : public QObject
public:
MockLinkMissionItemHandler(MockLink* mockLink);
~MockLinkMissionItemHandler();
// Prepares for destruction on correct thread
void shutdown(void);
/// @brief Called to handle mission item related messages. All messages should be passed to this method.
/// It will handle the appropriate set.
/// @return true: message handled
bool handleMessage(const mavlink_message_t& msg);
typedef enum {
FailNone, // No failures
FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST
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
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
FailReadRequest0ErrorAck, // Respond to MISSION_REQUEST 0 with MISSION_ACK error
FailReadRequest1ErrorAck, // Respond to MISSION_REQUEST 1 bogus MISSION_ACK error
FailWriteRequest0NoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0
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
FailWriteRequest1IncorrectSequence, // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number
FailWriteRequest0ErrorAck, // Respond to MISSION_COUNT 0 with MISSION_ACK error
FailWriteRequest1ErrorAck, // Respond to MISSION_ITEM 0 with MISSION_ACK error
FailWriteFinalAckNoResponse, // Don't send the final MISSION_ACK
FailWriteFinalAckErrorAck, // Send an error as the final MISSION_ACK
FailWriteFinalAckMissingRequests, // Send the MISSION_ACK before all items have been requested
} FailureMode_t;
/// 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);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType);
/// Called to send a MISSION_ITEM message while the MissionManager is in idle state
void sendUnexpectedMissionItem(void);
/// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
void sendUnexpectedMissionRequest(void);
/// Reset the state of the MissionItemHandler to no items, no transactions in progress.
void reset(void) { _missionItems.clear(); }
private slots:
void _missionItemResponseTimeout(void);
private:
void _handleMissionRequestList(const mavlink_message_t& msg);
......@@ -52,6 +97,8 @@ private:
void _handleMissionItem(const mavlink_message_t& msg);
void _handleMissionCount(const mavlink_message_t& msg);
void _requestNextMissionItem(int sequenceNumber);
void _sendAck(MAV_MISSION_RESULT ackType);
void _startMissionItemResponseTimer(void);
private:
MockLink* _mockLink;
......@@ -61,6 +108,10 @@ private:
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
MissionList_t _missionItems;
QTimer* _missionItemResponseTimer;
FailureMode_t _failureMode;
bool _failureFirstTimeOnly;
};
#endif
Supports Markdown
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