332 lines
15 KiB
C++
332 lines
15 KiB
C++
/****************************************************************************
|
|
*
|
|
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
|
*
|
|
* QGroundControl is licensed according to the terms in the file
|
|
* COPYING.md in the root of the source code directory.
|
|
*
|
|
****************************************************************************/
|
|
|
|
|
|
#include "MissionManagerTest.h"
|
|
#include "LinkManager.h"
|
|
#include "MultiVehicleManager.h"
|
|
|
|
const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = {
|
|
{ "0\t0\t3\t16\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 0, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_WAYPOINT, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
|
|
{ "1\t0\t3\t17\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_UNLIM, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
|
|
{ "2\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 2, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
|
|
{ "3\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 3, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
|
|
{ "4\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 4, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
|
|
{ "6\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 5, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } },
|
|
};
|
|
const size_t MissionManagerTest::_cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
|
|
|
|
MissionManagerTest::MissionManagerTest(void)
|
|
{
|
|
|
|
}
|
|
|
|
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
|
|
{
|
|
_mockLink->setMissionItemFailureMode(failureMode);
|
|
|
|
// Setup our test case data
|
|
QList<MissionItem*> missionItems;
|
|
|
|
// Editor has a home position item on the front, so we do the same
|
|
MissionItem* homeItem = new MissionItem(nullptr /* Vehicle */, this);
|
|
homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
|
|
homeItem->setParam5(47.3769);
|
|
homeItem->setParam6(8.549444);
|
|
homeItem->setParam7(0);
|
|
homeItem->setSequenceNumber(0);
|
|
missionItems.append(homeItem);
|
|
|
|
for (size_t i=0; i<_cTestCases; i++) {
|
|
const TestCase_t* testCase = &_rgTestCases[i];
|
|
|
|
MissionItem* missionItem = new MissionItem(this);
|
|
|
|
QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly);
|
|
QVERIFY(missionItem->load(loadStream));
|
|
|
|
// Mission Manager expects to get 1-base sequence numbers for write
|
|
missionItem->setSequenceNumber(missionItem->sequenceNumber() + 1);
|
|
|
|
missionItems.append(missionItem);
|
|
}
|
|
|
|
// Send the items to the vehicle
|
|
_missionManager->writeMissionItems(missionItems);
|
|
|
|
// writeMissionItems should emit these signals before returning:
|
|
// inProgressChanged
|
|
QVERIFY(_missionManager->inProgress());
|
|
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask), true);
|
|
_checkInProgressValues(true);
|
|
|
|
_multiSpyMissionManager->clearAllSignals();
|
|
|
|
if (shouldFail) {
|
|
// This should be a failed run
|
|
|
|
setExpectedMessageBox(QMessageBox::Ok);
|
|
|
|
// Wait for write sequence to complete. We should get:
|
|
// inProgressChanged(false) signal
|
|
// error(errorCode, QString) signal
|
|
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
|
|
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true);
|
|
|
|
// Validate inProgressChanged signal value
|
|
_checkInProgressValues(false);
|
|
|
|
// Validate error signal values
|
|
QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
|
|
QList<QVariant> signalArgs = spy->takeFirst();
|
|
QCOMPARE(signalArgs.count(), 2);
|
|
qDebug() << signalArgs[1].toString();
|
|
|
|
checkExpectedMessageBox();
|
|
} else {
|
|
// This should be clean run
|
|
|
|
// Wait for write sequence to complete. We should get:
|
|
// inProgressChanged(false) signal
|
|
// sednComplete signal
|
|
_multiSpyMissionManager->waitForSignalByIndex(sendCompleteSignalIndex, _missionManagerSignalWaitTime);
|
|
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | sendCompleteSignalMask), 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();
|
|
}
|
|
|
|
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
|
|
{
|
|
_writeItems(MockLinkMissionItemHandler::FailNone, false);
|
|
|
|
_mockLink->setMissionItemFailureMode(failureMode);
|
|
|
|
// Read the items back from the vehicle
|
|
_missionManager->loadFromVehicle();
|
|
|
|
// requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it
|
|
QVERIFY(_missionManager->inProgress());
|
|
QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
|
|
_checkInProgressValues(true);
|
|
|
|
_multiSpyMissionManager->clearAllSignals();
|
|
|
|
if (shouldFail) {
|
|
// This should be a failed run
|
|
|
|
setExpectedMessageBox(QMessageBox::Ok);
|
|
|
|
// Wait for read sequence to complete. We should get:
|
|
// inProgressChanged(false) signal to signal completion
|
|
// error(errorCode, QString) signal
|
|
// newMissionItemsAvailable signal
|
|
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
|
|
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true);
|
|
|
|
// Validate inProgressChanged signal value
|
|
_checkInProgressValues(false);
|
|
|
|
// Validate error signal values
|
|
QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
|
|
QList<QVariant> signalArgs = spy->takeFirst();
|
|
QCOMPARE(signalArgs.count(), 2);
|
|
qDebug() << signalArgs[1].toString();
|
|
|
|
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();
|
|
|
|
// Validate returned items
|
|
|
|
int cMissionItemsExpected;
|
|
|
|
if (shouldFail) {
|
|
cMissionItemsExpected = 0;
|
|
} else {
|
|
cMissionItemsExpected = (int)_cTestCases;
|
|
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
|
|
// Home position at position 0 comes from vehicle
|
|
cMissionItemsExpected++;
|
|
}
|
|
}
|
|
|
|
QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected);
|
|
|
|
int firstActualItem = 0;
|
|
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
|
|
// First item is home position, don't validate it
|
|
firstActualItem++;
|
|
}
|
|
|
|
int testCaseIndex = 0;
|
|
for (int actualItemIndex=firstActualItem; actualItemIndex<cMissionItemsExpected; actualItemIndex++) {
|
|
const TestCase_t* testCase = &_rgTestCases[testCaseIndex];
|
|
|
|
int expectedSequenceNumber = testCase->expectedItem.sequenceNumber;
|
|
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
|
|
// Account for home position in first item
|
|
expectedSequenceNumber++;
|
|
}
|
|
|
|
MissionItem* actual = _missionManager->missionItems()[actualItemIndex];
|
|
|
|
qDebug() << "Test case" << testCaseIndex;
|
|
QCOMPARE(actual->sequenceNumber(), expectedSequenceNumber);
|
|
QCOMPARE(actual->coordinate().latitude(), testCase->expectedItem.coordinate.latitude());
|
|
QCOMPARE(actual->coordinate().longitude(), testCase->expectedItem.coordinate.longitude());
|
|
QCOMPARE(actual->coordinate().altitude(), testCase->expectedItem.coordinate.altitude());
|
|
QCOMPARE((int)actual->command(), (int)testCase->expectedItem.command);
|
|
QCOMPARE(actual->param1(), testCase->expectedItem.param1);
|
|
QCOMPARE(actual->param2(), testCase->expectedItem.param2);
|
|
QCOMPARE(actual->param3(), testCase->expectedItem.param3);
|
|
QCOMPARE(actual->param4(), testCase->expectedItem.param4);
|
|
QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue);
|
|
QCOMPARE(actual->frame(), testCase->expectedItem.frame);
|
|
|
|
testCaseIndex++;
|
|
}
|
|
|
|
}
|
|
|
|
void MissionManagerTest::_testWriteFailureHandlingWorker(void)
|
|
{
|
|
/*
|
|
/// 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;
|
|
bool shouldFail;
|
|
} WriteTestCase_t;
|
|
|
|
static const WriteTestCase_t rgTestCases[] = {
|
|
{ "No Failure", MockLinkMissionItemHandler::FailNone, false },
|
|
{ "FailWriteMissionCountNoResponse", MockLinkMissionItemHandler::FailWriteMissionCountNoResponse, true },
|
|
{ "FailWriteMissionCountFirstResponse", MockLinkMissionItemHandler::FailWriteMissionCountFirstResponse, false },
|
|
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse, true },
|
|
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence, true },
|
|
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence, true },
|
|
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck, true },
|
|
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, true },
|
|
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse, true },
|
|
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck, true },
|
|
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests, true },
|
|
};
|
|
|
|
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
|
const WriteTestCase_t* pCase = &rgTestCases[i];
|
|
qDebug() << "TEST CASE " << pCase->failureText;
|
|
_writeItems(pCase->failureMode, pCase->shouldFail);
|
|
_mockLink->resetMissionItemHandler();
|
|
}
|
|
}
|
|
|
|
void MissionManagerTest::_testReadFailureHandlingWorker(void)
|
|
{
|
|
/*
|
|
/// 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;
|
|
bool shouldFail;
|
|
} ReadTestCase_t;
|
|
|
|
/*
|
|
static const ReadTestCase_t rgTestCases[] = {
|
|
{ "FailReadRequest1FirstResponse", MockLinkMissionItemHandler::FailReadRequest1FirstResponse, false },
|
|
};*/
|
|
|
|
static const ReadTestCase_t rgTestCases[] = {
|
|
{ "No Failure", MockLinkMissionItemHandler::FailNone, false },
|
|
{ "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++) {
|
|
const ReadTestCase_t* pCase = &rgTestCases[i];
|
|
qDebug() << "TEST CASE " << pCase->failureText;
|
|
_roundTripItems(pCase->failureMode, pCase->shouldFail);
|
|
_mockLink->resetMissionItemHandler();
|
|
_multiSpyMissionManager->clearAllSignals();
|
|
}
|
|
}
|
|
|
|
void MissionManagerTest::_testWriteFailureHandlingAPM(void)
|
|
{
|
|
_initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
|
|
_testWriteFailureHandlingWorker();
|
|
}
|
|
|
|
void MissionManagerTest::_testReadFailureHandlingAPM(void)
|
|
{
|
|
_initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
|
|
_testReadFailureHandlingWorker();
|
|
}
|
|
|
|
|
|
void MissionManagerTest::_testWriteFailureHandlingPX4(void)
|
|
{
|
|
_initForFirmwareType(MAV_AUTOPILOT_PX4);
|
|
_testWriteFailureHandlingWorker();
|
|
}
|
|
|
|
void MissionManagerTest::_testReadFailureHandlingPX4(void)
|
|
{
|
|
_initForFirmwareType(MAV_AUTOPILOT_PX4);
|
|
_testReadFailureHandlingWorker();
|
|
}
|