MissionManagerTest.cc 17 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9
 *
 * 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, MAV_MISSION_RESULT failureAckResult, bool shouldFail)
Don Gagne's avatar
Don Gagne committed
31
{
32
    _mockLink->setMissionItemFailureMode(failureMode, failureAckResult);
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
    MissionItem* homeItem = new MissionItem(nullptr /* Vehicle */, this);
39
    homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
40 41 42
    homeItem->setParam5(47.3769);
    homeItem->setParam6(8.549444);
    homeItem->setParam7(0);
43
    homeItem->setSequenceNumber(0);
44
    missionItems.append(homeItem);
45

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

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

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

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

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

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

Don Gagne's avatar
Don Gagne committed
91
        checkExpectedMessageBox();
92 93 94 95 96
    } else {
        // This should be clean run

        // Wait for write sequence to complete. We should get:
        //      inProgressChanged(false) signal
97 98 99
        //      sednComplete signal
        _multiSpyMissionManager->waitForSignalByIndex(sendCompleteSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | sendCompleteSignalMask), true);
100 101 102 103 104 105 106 107 108 109 110 111 112

        // 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);
113
    }
Don Gagne's avatar
Don Gagne committed
114
    
115
    _multiSpyMissionManager->clearAllSignals();
116 117
}

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

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

Don Gagne's avatar
Don Gagne committed
137 138
        setExpectedMessageBox(QMessageBox::Ok);

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

146 147
        // Validate inProgressChanged signal value
        _checkInProgressValues(false);
148

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

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

    // Validate returned items
Don Gagne's avatar
Don Gagne committed
170
    
DonLakeFlyer's avatar
DonLakeFlyer committed
171
    int cMissionItemsExpected;
Don Gagne's avatar
Don Gagne committed
172
    
173 174 175
    if (shouldFail) {
        cMissionItemsExpected = 0;
    } else {
Don Gagne's avatar
Don Gagne committed
176 177 178 179 180
        cMissionItemsExpected = (int)_cTestCases;
        if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            // Home position at position 0 comes from vehicle
            cMissionItemsExpected++;
        }
181 182
    }
    
183
    QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected);
Don Gagne's avatar
Don Gagne committed
184

DonLakeFlyer's avatar
DonLakeFlyer committed
185
    int firstActualItem = 0;
Don Gagne's avatar
Don Gagne committed
186 187 188 189 190 191
    if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        // First item is home position, don't validate it
        firstActualItem++;
    }

    int testCaseIndex = 0;
DonLakeFlyer's avatar
DonLakeFlyer committed
192
    for (int actualItemIndex=firstActualItem; actualItemIndex<cMissionItemsExpected; actualItemIndex++) {
Don Gagne's avatar
Don Gagne committed
193 194 195 196 197 198 199 200
        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++;
        }

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

        testCaseIndex++;
Don Gagne's avatar
Don Gagne committed
217
    }
218
    
Don Gagne's avatar
Don Gagne committed
219
}
220

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

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

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

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

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();
}
332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362

void MissionManagerTest::_testErrorAckFailureStrings(void)
{
    _initForFirmwareType(MAV_AUTOPILOT_PX4);

    typedef struct {
        const char*                                 ackResultStr;
        MAV_MISSION_RESULT                          ackResult;
    } ErrorStringTestCase_t;

    static const ErrorStringTestCase_t rgTestCases[] = {
        { "MAV_MISSION_UNSUPPORTED_FRAME",  MAV_MISSION_UNSUPPORTED_FRAME },
        { "MAV_MISSION_UNSUPPORTED",        MAV_MISSION_UNSUPPORTED },
        { "MAV_MISSION_INVALID_PARAM1",     MAV_MISSION_INVALID_PARAM1 },
        { "MAV_MISSION_INVALID_PARAM2",     MAV_MISSION_INVALID_PARAM2 },
        { "MAV_MISSION_INVALID_PARAM3",     MAV_MISSION_INVALID_PARAM3 },
        { "MAV_MISSION_INVALID_PARAM4",     MAV_MISSION_INVALID_PARAM4 },
        { "MAV_MISSION_INVALID_PARAM5_X",   MAV_MISSION_INVALID_PARAM5_X },
        { "MAV_MISSION_INVALID_PARAM6_Y",   MAV_MISSION_INVALID_PARAM6_Y },
        { "MAV_MISSION_INVALID_PARAM7",     MAV_MISSION_INVALID_PARAM7 },
        { "MAV_MISSION_INVALID_SEQUENCE",   MAV_MISSION_INVALID_SEQUENCE },
    };

    for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
        const ErrorStringTestCase_t* pCase = &rgTestCases[i];
        qDebug() << "TEST CASE _testErrorAckFailureStrings" << pCase->ackResultStr;
        _writeItems(MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, pCase->ackResult, true /* shouldFail */);
        _mockLink->resetMissionItemHandler();
    }

}