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

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 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
    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:
        _handleMissionItem(msg);
        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:
73
        _handleMissionClearAll(msg);
74 75 76 77
        break;

    default:
        return false;
78
    }
Don Gagne's avatar
Don Gagne committed
79 80
    
    return true;
81 82
}

83 84 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
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
117
void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
118
{
Don Gagne's avatar
Don Gagne committed
119
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
120
    
121 122 123 124 125 126 127 128
    _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 {
129 130
        mavlink_mission_request_list_t request;
        
131
        _failReadRequestListFirstResponse = true;
132 133 134
        mavlink_msg_mission_request_list_decode(&msg, &request);
        
        Q_ASSERT(request.target_system == _mockLink->vehicleId());
Don Gagne's avatar
Don Gagne committed
135

136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153
        _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
154
        }
155 156 157
        
        mavlink_message_t   responseMsg;
        
158
        mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(),
159
                                            MAV_COMP_ID_AUTOPILOT1,
160
                                            _mockLink->mavlinkChannel(),
Donald Gagne's avatar
Donald Gagne committed
161 162 163 164
                                            &responseMsg,               // Outgoing message
                                            msg.sysid,                  // Target is original sender
                                            msg.compid,                 // Target is original sender
                                            itemCount,                  // Number of mission items
165
                                            _requestType);
166 167
        _mockLink->respondWithMavlinkMessage(responseMsg);
    }
Don Gagne's avatar
Don Gagne committed
168
}
169

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

180 181 182 183 184 185 186
    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";
187 188 189 190 191 192 193 194 195 196 197
    } 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) ||
198
                (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
199 200
            _sendAck(MAV_MISSION_ERROR);
        } else {
201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223
            mavlink_mission_item_t  item;
            mavlink_message_t       responseMsg;

            switch (request.mission_type) {
            case MAV_MISSION_TYPE_MISSION:
                if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
                    item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
                    item.command = MAV_CMD_NAV_WAYPOINT;
                    item.current = false;
                    item.autocontinue = true;
                    item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
                } else {
                    item = _missionItems[request.seq];
                }
                break;
            case MAV_MISSION_TYPE_FENCE:
                item = _fenceItems[request.seq];
                break;
            case MAV_MISSION_TYPE_RALLY:
                item = _rallyItems[request.seq];
                break;
            default:
                Q_ASSERT(false);
Don Gagne's avatar
Don Gagne committed
224
            }
225
            
226
            mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
227
                                               MAV_COMP_ID_AUTOPILOT1,
228 229 230 231 232 233 234 235 236 237
                                               _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,
Donald Gagne's avatar
Donald Gagne committed
238
                                               item.x, item.y, item.z,
239
                                               _requestType);
240 241 242
            _mockLink->respondWithMavlinkMessage(responseMsg);
        }
    }
243 244
}

Don Gagne's avatar
Don Gagne committed
245
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
246
{
Don Gagne's avatar
Don Gagne committed
247 248 249 250 251
    mavlink_mission_count_t missionCount;
    
    mavlink_msg_mission_count_decode(&msg, &missionCount);
    Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());
    
252
    _requestType = (MAV_MISSION_TYPE)missionCount.mission_type;
Don Gagne's avatar
Don Gagne committed
253 254 255
    _writeSequenceCount = missionCount.count;
    Q_ASSERT(_writeSequenceCount >= 0);
    
256
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
Don Gagne's avatar
Don Gagne committed
257
    
258 259 260 261 262 263 264 265 266 267 268
    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
269 270
    
    if (_writeSequenceCount == 0) {
271
        _sendAck(MAV_MISSION_ACCEPTED);
272
    } else {
273 274 275 276 277 278 279 280 281 282
        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
283 284
        _writeSequenceIndex = 0;
        _requestNextMissionItem(_writeSequenceIndex);
285 286 287
    }
}

Don Gagne's avatar
Don Gagne committed
288
void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
289
{
290
    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
Don Gagne's avatar
Don Gagne committed
291
    
292 293
    if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
294 295 296 297 298 299 300
    } else {
        if (sequenceNumber >= _writeSequenceCount) {
            qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
            return;
        }
        
        if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
301
                (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
302 303 304 305
            sequenceNumber ++;
        }
        
        if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
306
                (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
307 308 309
            qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
            _sendAck(MAV_MISSION_ERROR);
        } else {
310 311 312
            mavlink_message_t message;

            mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
313
                                                  MAV_COMP_ID_AUTOPILOT1,
314 315 316 317 318 319
                                                  _mockLink->mavlinkChannel(),
                                                  &message,
                                                  _mavlinkProtocol->getSystemId(),
                                                  _mavlinkProtocol->getComponentId(),
                                                  sequenceNumber,
                                                  _requestType);
320 321 322 323 324
            _mockLink->respondWithMavlinkMessage(message);

            // If response with Mission Item doesn't come before timer fires it's an error
            _startMissionItemResponseTimer();
        }
325
    }
326 327 328 329 330 331
}

void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
    
332
    mavlink_message_t message;
Don Gagne's avatar
Don Gagne committed
333
    
334
    mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
335
                                      MAV_COMP_ID_AUTOPILOT1,
336 337 338 339 340 341
                                      _mockLink->mavlinkChannel(),
                                      &message,
                                      _mavlinkProtocol->getSystemId(),
                                      _mavlinkProtocol->getComponentId(),
                                      ackType,
                                      _requestType);
Don Gagne's avatar
Don Gagne committed
342
    _mockLink->respondWithMavlinkMessage(message);
343 344
}

Don Gagne's avatar
Don Gagne committed
345
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
346
{
Don Gagne's avatar
Don Gagne committed
347 348
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
    
349 350
    _missionItemResponseTimer->stop();
    
Don Gagne's avatar
Don Gagne committed
351 352 353 354 355 356
    mavlink_mission_item_t missionItem;
    
    mavlink_msg_mission_item_decode(&msg, &missionItem);
    
    Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
    
357 358 359 360 361 362 363 364 365 366 367 368
    switch (missionItem.mission_type) {
    case MAV_MISSION_TYPE_MISSION:
        _missionItems[missionItem.seq] = missionItem;
        break;
    case MAV_MISSION_TYPE_FENCE:
        _fenceItems[missionItem.seq] = missionItem;
        break;
    case MAV_MISSION_TYPE_RALLY:
        _rallyItems[missionItem.seq] = missionItem;
        break;
    }

Don Gagne's avatar
Don Gagne committed
369 370
    _writeSequenceIndex++;
    if (_writeSequenceIndex < _writeSequenceCount) {
371 372 373 374 375 376
        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
            // Send MAV_MISSION_ACCPETED ack too early
            _sendAck(MAV_MISSION_ACCEPTED);
        } else {
            _requestNextMissionItem(_writeSequenceIndex);
        }
377
    } else {
378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411
        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);
}

412
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
413 414 415 416 417 418 419 420
{
    _failureMode = failureMode;
}

void MockLinkMissionItemHandler::shutdown(void)
{
    if (_missionItemResponseTimer) {
        delete _missionItemResponseTimer;
421 422
    }
}