MissionManagerTest.cc 15.3 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 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.
 *
 ****************************************************************************/

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14 15

#include "MissionManagerTest.h"
#include "LinkManager.h"
#include "MultiVehicleManager.h"

const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = {
16
    { "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 } },
Don Gagne's avatar
Don Gagne committed
17
    { "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 } },
18 19 20
    { "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 } },
21
    { "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 } },
Don Gagne's avatar
Don Gagne committed
22
};
Don Gagne's avatar
Don Gagne committed
23
const size_t MissionManagerTest::_cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
Don Gagne's avatar
Don Gagne committed
24 25 26 27 28 29

MissionManagerTest::MissionManagerTest(void)
{
    
}

30
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
Don Gagne's avatar
Don Gagne committed
31
{
32
    _mockLink->setMissionItemFailureMode(failureMode);
33
    
Don Gagne's avatar
Don Gagne committed
34
    // Setup our test case data
35
    QList<MissionItem*> missionItems;
Don Gagne's avatar
Don Gagne committed
36
    
37
    // Editor has a home position item on the front, so we do the same
38 39
    MissionItem* homeItem = new MissionItem(NULL /* Vehicle */, this);
    homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
40
    homeItem->setCoordinate(QGeoCoordinate(47.3769, 8.549444, 0));
41
    homeItem->setSequenceNumber(0);
42
    missionItems.append(homeItem);
43

Don Gagne's avatar
Don Gagne committed
44
    for (size_t i=0; i<_cTestCases; i++) {
Don Gagne's avatar
Don Gagne committed
45 46
        const TestCase_t* testCase = &_rgTestCases[i];
        
47
        MissionItem* missionItem = new MissionItem(this);
Don Gagne's avatar
Don Gagne committed
48 49
        
        QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly);
50
        QVERIFY(missionItem->load(loadStream));
51 52

        // Mission Manager expects to get 1-base sequence numbers for write
53
        missionItem->setSequenceNumber(missionItem->sequenceNumber() + 1);
Don Gagne's avatar
Don Gagne committed
54
        
55
        missionItems.append(missionItem);
Don Gagne's avatar
Don Gagne committed
56 57 58
    }
    
    // Send the items to the vehicle
59
    _missionManager->writeMissionItems(missionItems);
60
    
61 62 63
    // writeMissionItems should emit these signals before returning:
    //      inProgressChanged
    //      newMissionItemsAvailable
Don Gagne's avatar
Don Gagne committed
64
    QVERIFY(_missionManager->inProgress());
65
    QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | newMissionItemsAvailableSignalMask), true);
Don Gagne's avatar
Don Gagne committed
66 67
    _checkInProgressValues(true);
    
68
    _multiSpyMissionManager->clearAllSignals();
Don Gagne's avatar
Don Gagne committed
69
    
70
    if (shouldFail) {
71
        // This should be a failed run
72

Don Gagne's avatar
Don Gagne committed
73 74
        setExpectedMessageBox(QMessageBox::Ok);

75 76 77
        // Wait for write sequence to complete. We should get:
        //      inProgressChanged(false) signal
        //      error(errorCode, QString) signal
78 79
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true);
80

81 82
        // Validate inProgressChanged signal value
        _checkInProgressValues(false);
83

84
        // Validate error signal values
85
        QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
86 87 88 89
        QList<QVariant> signalArgs = spy->takeFirst();
        QCOMPARE(signalArgs.count(), 2);
        qDebug() << signalArgs[1].toString();

Don Gagne's avatar
Don Gagne committed
90
        checkExpectedMessageBox();
91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110
    } 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);
111
    }
Don Gagne's avatar
Don Gagne committed
112
    
113
    _multiSpyMissionManager->clearAllSignals();
114 115
}

116
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
117
{
118
    _writeItems(MockLinkMissionItemHandler::FailNone, false);
Don Gagne's avatar
Don Gagne committed
119
    
120
    _mockLink->setMissionItemFailureMode(failureMode);
121

Don Gagne's avatar
Don Gagne committed
122 123 124 125 126
    // Read the items back from the vehicle
    _missionManager->requestMissionItems();
    
    // requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it
    QVERIFY(_missionManager->inProgress());
127
    QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
Don Gagne's avatar
Don Gagne committed
128 129
    _checkInProgressValues(true);
    
130
    _multiSpyMissionManager->clearAllSignals();
Don Gagne's avatar
Don Gagne committed
131
    
132
    if (shouldFail) {
133
        // This should be a failed run
134

Don Gagne's avatar
Don Gagne committed
135 136
        setExpectedMessageBox(QMessageBox::Ok);

137 138 139 140
        // Wait for read sequence to complete. We should get:
        //      inProgressChanged(false) signal to signal completion
        //      error(errorCode, QString) signal
        //      newMissionItemsAvailable signal
141 142
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true);
143

144 145
        // Validate inProgressChanged signal value
        _checkInProgressValues(false);
146

147
        // Validate error signal values
148
        QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
149 150 151
        QList<QVariant> signalArgs = spy->takeFirst();
        QCOMPARE(signalArgs.count(), 2);
        qDebug() << signalArgs[1].toString();
152

Don Gagne's avatar
Don Gagne committed
153
        checkExpectedMessageBox();
154 155 156 157 158 159 160 161 162
    } 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);
163
    }
Don Gagne's avatar
Don Gagne committed
164
    
165
    _multiSpyMissionManager->clearAllSignals();
166 167

    // Validate returned items
Don Gagne's avatar
Don Gagne committed
168
    
169
    size_t cMissionItemsExpected;
Don Gagne's avatar
Don Gagne committed
170
    
171 172 173
    if (shouldFail) {
        cMissionItemsExpected = 0;
    } else {
Don Gagne's avatar
Don Gagne committed
174 175 176 177 178
        cMissionItemsExpected = (int)_cTestCases;
        if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            // Home position at position 0 comes from vehicle
            cMissionItemsExpected++;
        }
179 180
    }
    
181
    QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected);
Don Gagne's avatar
Don Gagne committed
182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198

    size_t firstActualItem = 0;
    if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        // First item is home position, don't validate it
        firstActualItem++;
    }

    int testCaseIndex = 0;
    for (size_t 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++;
        }

199
        MissionItem* actual = _missionManager->missionItems()[actualItemIndex];
Don Gagne's avatar
Don Gagne committed
200
        
Don Gagne's avatar
Don Gagne committed
201 202
        qDebug() << "Test case" << testCaseIndex;
        QCOMPARE(actual->sequenceNumber(),          expectedSequenceNumber);
Don Gagne's avatar
Don Gagne committed
203 204 205 206
        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);
Don Gagne's avatar
Don Gagne committed
207
        QCOMPARE(actual->param1(),                  testCase->expectedItem.param1);
Don Gagne's avatar
Don Gagne committed
208 209 210 211 212
        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);
Don Gagne's avatar
Don Gagne committed
213 214

        testCaseIndex++;
Don Gagne's avatar
Don Gagne committed
215
    }
216
    
Don Gagne's avatar
Don Gagne committed
217
}
218

Don Gagne's avatar
Don Gagne committed
219
void MissionManagerTest::_testWriteFailureHandlingWorker(void)
220 221 222 223 224 225 226 227 228 229 230 231 232 233 234
{
    /*
    /// 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;
235 236
        bool                                        shouldFail;
    } WriteTestCase_t;
237
    
238 239 240 241 242 243 244 245 246 247 248 249
    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 },
250 251 252
    };

    for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
253 254 255
        const WriteTestCase_t* pCase = &rgTestCases[i];
        qDebug() << "TEST CASE " << pCase->failureText;
        _writeItems(pCase->failureMode, pCase->shouldFail);
256 257 258
        _mockLink->resetMissionItemHandler();
    }
}
259

Don Gagne's avatar
Don Gagne committed
260
void MissionManagerTest::_testReadFailureHandlingWorker(void)
261 262 263 264 265 266 267 268 269 270 271 272 273 274 275
{
    /*
     /// 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;
276 277
        bool                                        shouldFail;
    } ReadTestCase_t;
278
    
279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294
    /*
    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 },
295
    };
296

297
    for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
298 299 300
        const ReadTestCase_t* pCase = &rgTestCases[i];
        qDebug() << "TEST CASE " << pCase->failureText;
        _roundTripItems(pCase->failureMode, pCase->shouldFail);
301
        _mockLink->resetMissionItemHandler();
302
        _multiSpyMissionManager->clearAllSignals();
303 304
    }
}
Don Gagne's avatar
Don Gagne committed
305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329

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();
}