MockLinkMissionItemHandler.cc 18.1 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.
 *
 ****************************************************************************/

10 11

#include "MockLinkMissionItemHandler.h"
Don Gagne's avatar
Don Gagne committed
12
#include "MockLink.h"
13 14 15

#include <QDebug>

Don Gagne's avatar
Don Gagne committed
16
QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")
17

18
MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLinkProtocol* mavlinkProtocol)
19
    : _mockLink(mockLink)
20
    , _missionItemResponseTimer(nullptr)
21
    , _failureMode(FailNone)
Don Gagne's avatar
Don Gagne committed
22
    , _sendHomePositionOnEmptyList(false)
23
    , _mavlinkProtocol(mavlinkProtocol)
24 25 26
    , _failReadRequestListFirstResponse(true)
    , _failReadRequest1FirstResponse(true)
    , _failWriteMissionCountFirstResponse(true)
27
{
Don Gagne's avatar
Don Gagne committed
28
    Q_ASSERT(mockLink);
29 30
}

31 32 33 34 35 36 37 38 39 40 41 42 43 44
MockLinkMissionItemHandler::~MockLinkMissionItemHandler()
{

}

void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
{
    if (!_missionItemResponseTimer) {
        _missionItemResponseTimer = new QTimer();
        connect(_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout);
    }
    _missionItemResponseTimer->start(500);
}

Don Gagne's avatar
Don Gagne committed
45
bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
46 47
{
    switch (msg.msgid) {
48 49 50 51 52 53 54 55 56
    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
        _handleMissionRequestList(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_REQUEST:
        _handleMissionRequest(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM:
57 58 59 60 61
        _handleMissionItem(msg, false);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        _handleMissionItem(msg, true);
62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
        break;

    case MAVLINK_MSG_ID_MISSION_COUNT:
        _handleMissionCount(msg);
        break;

    case MAVLINK_MSG_ID_MISSION_ACK:
        // Acks are received back for each MISSION_ITEM message
        break;

    case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
        // Sets the currently active mission item
        break;

    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
77
        _handleMissionClearAll(msg);
78 79 80 81
        break;

    default:
        return false;
82
    }
Don Gagne's avatar
Don Gagne committed
83 84
    
    return true;
85 86
}

87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120
void MockLinkMissionItemHandler::_handleMissionClearAll(const mavlink_message_t& msg)
{

    mavlink_mission_clear_all_t clearAll;

    mavlink_msg_mission_clear_all_decode(&msg, &clearAll);

    Q_ASSERT(clearAll.target_system == _mockLink->vehicleId());

    _requestType = (MAV_MISSION_TYPE)clearAll.mission_type;
    qCDebug(MockLinkMissionItemHandlerLog) << QStringLiteral("_handleMissionClearAll %1").arg(_requestType);

    switch (_requestType) {
    case MAV_MISSION_TYPE_MISSION:
        _missionItems.clear();
        break;
    case MAV_MISSION_TYPE_FENCE:
        _fenceItems.clear();
        break;
    case MAV_MISSION_TYPE_RALLY:
        _rallyItems.clear();
        break;
    case MAV_MISSION_TYPE_ALL:
        _missionItems.clear();
        _fenceItems.clear();
        _rallyItems.clear();
        break;
    default:
        Q_ASSERT(false);
    }

    _sendAck(MAV_MISSION_ACCEPTED);
}

Don Gagne's avatar
Don Gagne committed
121
void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
122
{
Don Gagne's avatar
Don Gagne committed
123
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
124
    
125 126 127 128 129 130 131 132
    _failReadRequest1FirstResponse = true;

    if (_failureMode == FailReadRequestListNoResponse) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListNoResponse";
    } else if (_failureMode == FailReadRequestListFirstResponse && _failReadRequestListFirstResponse) {
        _failReadRequestListFirstResponse = false;
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListFirstResponse";
    } else {
133 134
        mavlink_mission_request_list_t request;
        
135
        _failReadRequestListFirstResponse = true;
136 137 138
        mavlink_msg_mission_request_list_decode(&msg, &request);
        
        Q_ASSERT(request.target_system == _mockLink->vehicleId());
Don Gagne's avatar
Don Gagne committed
139

140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157
        _requestType = (MAV_MISSION_TYPE)request.mission_type;

        int itemCount;
        switch (_requestType) {
        case MAV_MISSION_TYPE_MISSION:
            itemCount = _missionItems.count();
            if (itemCount == 0 && _sendHomePositionOnEmptyList) {
                itemCount = 1;
            }
            break;
        case MAV_MISSION_TYPE_FENCE:
            itemCount = _fenceItems.count();
            break;
        case MAV_MISSION_TYPE_RALLY:
            itemCount = _rallyItems.count();
            break;
        default:
            Q_ASSERT(false);
Don Gagne's avatar
Don Gagne committed
158
        }
159 160 161
        
        mavlink_message_t   responseMsg;
        
162
        mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(),
163
                                            MAV_COMP_ID_AUTOPILOT1,
164
                                            _mockLink->mavlinkChannel(),
Donald Gagne's avatar
Donald Gagne committed
165 166 167 168
                                            &responseMsg,               // Outgoing message
                                            msg.sysid,                  // Target is original sender
                                            msg.compid,                 // Target is original sender
                                            itemCount,                  // Number of mission items
169
                                            _requestType);
170 171
        _mockLink->respondWithMavlinkMessage(responseMsg);
    }
Don Gagne's avatar
Don Gagne committed
172
}
173

Don Gagne's avatar
Don Gagne committed
174
void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
175
{
Don Gagne's avatar
Don Gagne committed
176
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence";
177
    
Don Gagne's avatar
Don Gagne committed
178
    mavlink_mission_request_t request;
179
    
Don Gagne's avatar
Don Gagne committed
180
    mavlink_msg_mission_request_decode(&msg, &request);
181
    
Don Gagne's avatar
Don Gagne committed
182
    Q_ASSERT(request.target_system == _mockLink->vehicleId());
183

184 185 186 187 188 189 190
    if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
    } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1NoResponse";
    } else if (_failureMode == FailReadRequest1FirstResponse && request.seq == 1 && _failReadRequest1FirstResponse) {
        _failReadRequest1FirstResponse = false;
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1FirstResponse";
191 192 193 194 195 196 197 198 199 200 201
    } 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) ||
202
                (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
203 204
            _sendAck(MAV_MISSION_ERROR);
        } else {
205
            MissionItemBoth_t missionItemBoth;
206 207 208 209

            switch (request.mission_type) {
            case MAV_MISSION_TYPE_MISSION:
                if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
210 211 212 213
                    memset(&missionItemBoth, 0, sizeof(missionItemBoth));
                    missionItemBoth.missionItem.frame =         MAV_FRAME_GLOBAL_RELATIVE_ALT;
                    missionItemBoth.missionItem.command =       MAV_CMD_NAV_WAYPOINT;
                    missionItemBoth.missionItem.autocontinue =  true;
214
                } else {
215
                    missionItemBoth = _missionItems[request.seq];
216 217 218
                }
                break;
            case MAV_MISSION_TYPE_FENCE:
219
                missionItemBoth = _fenceItems[request.seq];
220 221
                break;
            case MAV_MISSION_TYPE_RALLY:
222
                missionItemBoth = _rallyItems[request.seq];
223 224 225
                break;
            default:
                Q_ASSERT(false);
Don Gagne's avatar
Don Gagne committed
226
            }
227
            
228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261
            mavlink_message_t responseMsg;
            if (missionItemBoth.isIntItem) {
                mavlink_mission_item_int_t& item = missionItemBoth.missionItemInt;
                mavlink_msg_mission_item_int_pack_chan(_mockLink->vehicleId(),
                                                   MAV_COMP_ID_AUTOPILOT1,
                                                   _mockLink->mavlinkChannel(),
                                                   &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,
                                                   _requestType);
            } else {
                mavlink_mission_item_t& item = missionItemBoth.missionItem;
                mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
                                                   MAV_COMP_ID_AUTOPILOT1,
                                                   _mockLink->mavlinkChannel(),
                                                   &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,
                                                   _requestType);
            }
262 263 264
            _mockLink->respondWithMavlinkMessage(responseMsg);
        }
    }
265 266
}

Don Gagne's avatar
Don Gagne committed
267
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
268
{
Don Gagne's avatar
Don Gagne committed
269 270 271 272 273
    mavlink_mission_count_t missionCount;
    
    mavlink_msg_mission_count_decode(&msg, &missionCount);
    Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());
    
274
    _requestType = (MAV_MISSION_TYPE)missionCount.mission_type;
Don Gagne's avatar
Don Gagne committed
275 276 277
    _writeSequenceCount = missionCount.count;
    Q_ASSERT(_writeSequenceCount >= 0);
    
278
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
Don Gagne's avatar
Don Gagne committed
279
    
280 281 282 283 284 285 286 287 288 289 290
    switch (missionCount.mission_type) {
    case MAV_MISSION_TYPE_MISSION:
        _missionItems.clear();
        break;
    case MAV_MISSION_TYPE_FENCE:
        _fenceItems.clear();
        break;
    case MAV_MISSION_TYPE_RALLY:
        _rallyItems.clear();
        break;
    }
Don Gagne's avatar
Don Gagne committed
291 292
    
    if (_writeSequenceCount == 0) {
293
        _sendAck(MAV_MISSION_ACCEPTED);
294
    } else {
295 296 297 298 299 300 301 302 303 304
        if (_failureMode == FailWriteMissionCountNoResponse) {
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
            return;
        }
        if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) {
            _failWriteMissionCountFirstResponse = false;
            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
            return;
        }
        _failWriteMissionCountFirstResponse = true;
Don Gagne's avatar
Don Gagne committed
305 306
        _writeSequenceIndex = 0;
        _requestNextMissionItem(_writeSequenceIndex);
307 308 309
    }
}

Don Gagne's avatar
Don Gagne committed
310
void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
311
{
312
    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
Don Gagne's avatar
Don Gagne committed
313
    
314 315
    if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
316 317 318 319 320 321 322
    } else {
        if (sequenceNumber >= _writeSequenceCount) {
            qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
            return;
        }
        
        if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
323
                (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
324 325 326 327
            sequenceNumber ++;
        }
        
        if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
328
                (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
329 330 331
            qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
            _sendAck(MAV_MISSION_ERROR);
        } else {
332 333 334
            mavlink_message_t message;

            mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
335
                                                  MAV_COMP_ID_AUTOPILOT1,
336 337 338 339 340 341
                                                  _mockLink->mavlinkChannel(),
                                                  &message,
                                                  _mavlinkProtocol->getSystemId(),
                                                  _mavlinkProtocol->getComponentId(),
                                                  sequenceNumber,
                                                  _requestType);
342 343 344 345 346
            _mockLink->respondWithMavlinkMessage(message);

            // If response with Mission Item doesn't come before timer fires it's an error
            _startMissionItemResponseTimer();
        }
347
    }
348 349 350 351 352 353
}

void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
    
354
    mavlink_message_t message;
Don Gagne's avatar
Don Gagne committed
355
    
356
    mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
357
                                      MAV_COMP_ID_AUTOPILOT1,
358 359 360 361 362 363
                                      _mockLink->mavlinkChannel(),
                                      &message,
                                      _mavlinkProtocol->getSystemId(),
                                      _mavlinkProtocol->getComponentId(),
                                      ackType,
                                      _requestType);
Don Gagne's avatar
Don Gagne committed
364
    _mockLink->respondWithMavlinkMessage(message);
365 366
}

367
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg, bool missionItemInt)
368
{
Don Gagne's avatar
Don Gagne committed
369 370
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
    
371 372
    _missionItemResponseTimer->stop();
    
373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390
    MAV_MISSION_TYPE    missionType;
    uint16_t            seq;
    MissionItemBoth_t   missionItemBoth;

    missionItemBoth.isIntItem = missionItemInt;
    if (missionItemInt) {
        mavlink_mission_item_int_t missionItem;
        mavlink_msg_mission_item_int_decode(&msg, &missionItem);
        missionItemBoth.missionItemInt = missionItem;
        missionType = static_cast<MAV_MISSION_TYPE>(missionItem.mission_type);
        seq = missionItem.seq;
    } else {
        mavlink_mission_item_t missionItem;
        mavlink_msg_mission_item_decode(&msg, &missionItem);
        missionItemBoth.missionItem = missionItem;
        missionType = static_cast<MAV_MISSION_TYPE>(missionItem.mission_type);
        seq = missionItem.seq;
    }
Don Gagne's avatar
Don Gagne committed
391
    
392
    switch (missionType) {
393
    case MAV_MISSION_TYPE_MISSION:
394
        _missionItems[seq] = missionItemBoth;
395 396
        break;
    case MAV_MISSION_TYPE_FENCE:
397
        _fenceItems[seq] = missionItemBoth;
398 399
        break;
    case MAV_MISSION_TYPE_RALLY:
400 401 402 403 404
        _rallyItems[seq] = missionItemBoth;
        break;
    case MAV_MISSION_TYPE_ENUM_END:
    case MAV_MISSION_TYPE_ALL:
        qWarning() << "Internal error";
405 406 407
        break;
    }

Don Gagne's avatar
Don Gagne committed
408 409
    _writeSequenceIndex++;
    if (_writeSequenceIndex < _writeSequenceCount) {
410 411 412 413 414 415
        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
            // Send MAV_MISSION_ACCPETED ack too early
            _sendAck(MAV_MISSION_ACCEPTED);
        } else {
            _requestNextMissionItem(_writeSequenceIndex);
        }
416
    } else {
417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450
        if (_failureMode != FailWriteFinalAckNoResponse) {
            MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED;
            
            if (_failureMode ==  FailWriteFinalAckErrorAck) {
                ack = MAV_MISSION_ERROR;
            }
            _sendAck(ack);
        }
    }
}

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

451
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
452 453 454 455 456 457 458 459
{
    _failureMode = failureMode;
}

void MockLinkMissionItemHandler::shutdown(void)
{
    if (_missionItemResponseTimer) {
        delete _missionItemResponseTimer;
460 461
    }
}