MissionManagerTest.cc 15 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

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

const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = {
29
    { "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 } },
30
    { "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 } },
31 32 33
    { "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 } },
34
    { "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 } },
35
};
Don Gagne's avatar
Don Gagne committed
36
const size_t MissionManagerTest::_cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
37 38 39 40 41 42

MissionManagerTest::MissionManagerTest(void)
{
    
}

43
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
44
{
45
    _mockLink->setMissionItemFailureMode(failureMode);
46
    
47
    // Setup our test case data
48
    QList<MissionItem*> missionItems;
49
    
50
    // Editor has a home position item on the front, so we do the same
51 52
    MissionItem* homeItem = new MissionItem(NULL /* Vehicle */, this);
    homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
53
    homeItem->setCoordinate(QGeoCoordinate(47.3769, 8.549444, 0));
54
    homeItem->setSequenceNumber(0);
55
    missionItems.append(homeItem);
56

Don Gagne's avatar
Don Gagne committed
57
    for (size_t i=0; i<_cTestCases; i++) {
58 59
        const TestCase_t* testCase = &_rgTestCases[i];
        
60
        MissionItem* missionItem = new MissionItem(this);
61 62
        
        QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly);
63
        QVERIFY(missionItem->load(loadStream));
64 65

        // Mission Manager expects to get 1-base sequence numbers for write
66
        missionItem->setSequenceNumber(missionItem->sequenceNumber() + 1);
67
        
68
        missionItems.append(missionItem);
69 70 71
    }
    
    // Send the items to the vehicle
72
    _missionManager->writeMissionItems(missionItems);
73
    
74 75 76
    // writeMissionItems should emit these signals before returning:
    //      inProgressChanged
    //      newMissionItemsAvailable
77
    QVERIFY(_missionManager->inProgress());
78
    QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | newMissionItemsAvailableSignalMask), true);
79 80
    _checkInProgressValues(true);
    
81
    _multiSpyMissionManager->clearAllSignals();
82
    
83 84 85 86 87
    if (failureMode == MockLinkMissionItemHandler::FailNone) {
        // This should be clean run
        
        // Wait for write sequence to complete. We should get:
        //      inProgressChanged(false) signal
88 89
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
90 91 92 93
        
        // Validate inProgressChanged signal value
        _checkInProgressValues(false);

Don Gagne's avatar
Don Gagne committed
94 95 96 97 98 99 100 101
        // 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++;
        }

102
        QCOMPARE(_missionManager->missionItems().count(), expectedCount);
103 104 105
    } else {
        // This should be a failed run
        
106 107
        setExpectedMessageBox(QMessageBox::Ok);

108 109 110
        // Wait for write sequence to complete. We should get:
        //      inProgressChanged(false) signal
        //      error(errorCode, QString) signal
111 112
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true);
113 114 115 116 117
        
        // Validate inProgressChanged signal value
        _checkInProgressValues(false);
        
        // Validate error signal values
118
        QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
119 120 121 122
        QList<QVariant> signalArgs = spy->takeFirst();
        QCOMPARE(signalArgs.count(), 2);
        qDebug() << signalArgs[1].toString();

123
        checkExpectedMessageBox();
124
    }
125
    
126
    _multiSpyMissionManager->clearAllSignals();
127 128
}

129
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
130
{
131
    _writeItems(MockLinkMissionItemHandler::FailNone);
132
    
133
    _mockLink->setMissionItemFailureMode(failureMode);
134

135 136 137 138 139
    // 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());
140
    QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
141 142
    _checkInProgressValues(true);
    
143
    _multiSpyMissionManager->clearAllSignals();
144
    
145 146 147 148 149 150
    if (failureMode == MockLinkMissionItemHandler::FailNone) {
        // This should be clean run
        
        // Now wait for read sequence to complete. We should get:
        //      inProgressChanged(false) signal to signal completion
        //      newMissionItemsAvailable signal
151 152
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
153
        _checkInProgressValues(false);
Don Gagne's avatar
Don Gagne committed
154

155 156 157
    } else {
        // This should be a failed run
        
158 159
        setExpectedMessageBox(QMessageBox::Ok);

160 161 162 163
        // Wait for read sequence to complete. We should get:
        //      inProgressChanged(false) signal to signal completion
        //      error(errorCode, QString) signal
        //      newMissionItemsAvailable signal
164 165
        _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
        QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true);
166 167 168 169 170
        
        // Validate inProgressChanged signal value
        _checkInProgressValues(false);
        
        // Validate error signal values
171
        QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
172 173 174
        QList<QVariant> signalArgs = spy->takeFirst();
        QCOMPARE(signalArgs.count(), 2);
        qDebug() << signalArgs[1].toString();
175 176
        
        checkExpectedMessageBox();
177
    }
178
    
179
    _multiSpyMissionManager->clearAllSignals();
180 181

    // Validate returned items
182
    
183
    size_t cMissionItemsExpected;
184
    
185
    if (failureMode == MockLinkMissionItemHandler::FailNone) {
Don Gagne's avatar
Don Gagne committed
186 187 188 189 190
        cMissionItemsExpected = (int)_cTestCases;
        if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
            // Home position at position 0 comes from vehicle
            cMissionItemsExpected++;
        }
191
    } else {
192
        cMissionItemsExpected = 0;
193 194
    }
    
195
    QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected);
Don Gagne's avatar
Don Gagne committed
196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212

    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++;
        }

213
        MissionItem* actual = _missionManager->missionItems()[actualItemIndex];
214
        
Don Gagne's avatar
Don Gagne committed
215 216
        qDebug() << "Test case" << testCaseIndex;
        QCOMPARE(actual->sequenceNumber(),          expectedSequenceNumber);
217 218 219 220
        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
221
        QCOMPARE(actual->param1(),                  testCase->expectedItem.param1);
222 223 224 225 226
        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
227 228

        testCaseIndex++;
229
    }
230
    
231
}
232

Don Gagne's avatar
Don Gagne committed
233
void MissionManagerTest::_testWriteFailureHandlingWorker(void)
234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251
{
    /*
    /// 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;
    } TestCase_t;
    
    static const TestCase_t rgTestCases[] = {
252 253 254 255 256 257 258 259 260 261
        { "No Failure",                         MockLinkMissionItemHandler::FailNone },
        { "FailWriteRequest0NoResponse",        MockLinkMissionItemHandler::FailWriteRequest0NoResponse },
        { "FailWriteRequest1NoResponse",        MockLinkMissionItemHandler::FailWriteRequest1NoResponse },
        { "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence },
        { "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence },
        { "FailWriteRequest0ErrorAck",          MockLinkMissionItemHandler::FailWriteRequest0ErrorAck },
        { "FailWriteRequest1ErrorAck",          MockLinkMissionItemHandler::FailWriteRequest1ErrorAck },
        { "FailWriteFinalAckNoResponse",        MockLinkMissionItemHandler::FailWriteFinalAckNoResponse },
        { "FailWriteFinalAckErrorAck",          MockLinkMissionItemHandler::FailWriteFinalAckErrorAck },
        { "FailWriteFinalAckMissingRequests",   MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests },
262 263 264
    };

    for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
265 266
        qDebug() << "TEST CASE " << rgTestCases[i].failureText;
        _writeItems(rgTestCases[i].failureMode);
267 268 269
        _mockLink->resetMissionItemHandler();
    }
}
270

Don Gagne's avatar
Don Gagne committed
271
void MissionManagerTest::_testReadFailureHandlingWorker(void)
272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289
{
    /*
     /// 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;
    } TestCase_t;
    
    static const TestCase_t rgTestCases[] = {
290 291 292 293 294 295 296 297
        { "No Failure",                         MockLinkMissionItemHandler::FailNone },
        { "FailReadRequestListNoResponse",      MockLinkMissionItemHandler::FailReadRequestListNoResponse },
        { "FailReadRequest0NoResponse",         MockLinkMissionItemHandler::FailReadRequest0NoResponse },
        { "FailReadRequest1NoResponse",         MockLinkMissionItemHandler::FailReadRequest1NoResponse },
        { "FailReadRequest0IncorrectSequence",  MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence },
        { "FailReadRequest1IncorrectSequence",  MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence  },
        { "FailReadRequest0ErrorAck",           MockLinkMissionItemHandler::FailReadRequest0ErrorAck },
        { "FailReadRequest1ErrorAck",           MockLinkMissionItemHandler::FailReadRequest1ErrorAck },
298 299 300
    };
    
    for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
301 302
        qDebug() << "TEST CASE " << rgTestCases[i].failureText;
        _roundTripItems(rgTestCases[i].failureMode);
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();
}