MockLinkMissionItemHandler.cc 13.8 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"
12
#include "MockLink.h"
13 14 15

#include <QDebug>

16
QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")
17

18
MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLinkProtocol* mavlinkProtocol)
19 20 21
    : _mockLink(mockLink)
    , _missionItemResponseTimer(NULL)
    , _failureMode(FailNone)
22
    , _sendHomePositionOnEmptyList(false)
23
    , _mavlinkProtocol(mavlinkProtocol)
24 25 26
    , _failReadRequestListFirstResponse(true)
    , _failReadRequest1FirstResponse(true)
    , _failWriteMissionCountFirstResponse(true)
27
{
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);
}

45
bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
46 47 48
{
    switch (msg.msgid) {
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
49
            _handleMissionRequestList(msg);
50
            break;
51
            
52
        case MAVLINK_MSG_ID_MISSION_REQUEST:
53
            _handleMissionRequest(msg);
54
            break;
55 56 57 58 59
            
        case MAVLINK_MSG_ID_MISSION_ITEM:
            _handleMissionItem(msg);
            break;
            
60
        case MAVLINK_MSG_ID_MISSION_COUNT:
61
            _handleMissionCount(msg);
62
            break;
63 64 65 66 67 68 69
            
        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
70 71 72 73
            break;
    
        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
            // Delete all mission items
74
            _missionItems.clear();
75
            break;
76 77 78
            
        default:
            return false;
79
    }
80 81
    
    return true;
82 83
}

84
void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
85
{
86
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
87
    
88 89 90 91 92 93 94 95
    _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 {
96 97
        mavlink_mission_request_list_t request;
        
98
        _failReadRequestListFirstResponse = true;
99 100 101
        mavlink_msg_mission_request_list_decode(&msg, &request);
        
        Q_ASSERT(request.target_system == _mockLink->vehicleId());
102 103 104 105 106

        int itemCount = _missionItems.count();
        if (itemCount == 0 && _sendHomePositionOnEmptyList) {
            itemCount = 1;
        }
107 108 109
        
        mavlink_message_t   responseMsg;
        
110 111 112 113 114 115 116
        mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(),
                                            MAV_COMP_ID_MISSIONPLANNER,
                                            _mockLink->mavlinkChannel(),
                                            &responseMsg,            // Outgoing message
                                            msg.sysid,               // Target is original sender
                                            msg.compid,              // Target is original sender
                                            itemCount);              // Number of mission items
117 118
        _mockLink->respondWithMavlinkMessage(responseMsg);
    }
119
}
120

121
void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
122
{
123
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence";
124
    
125
    mavlink_mission_request_t request;
126
    
127
    mavlink_msg_mission_request_decode(&msg, &request);
128
    
129 130
    Q_ASSERT(request.target_system == _mockLink->vehicleId());
    Q_ASSERT(request.seq < _missionItems.count());
131
    
132 133 134 135 136 137 138
    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";
139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154
    } 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) ||
            (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
            _sendAck(MAV_MISSION_ERROR);
        } else {
            mavlink_message_t   responseMsg;
            
155 156 157 158 159 160 161 162 163 164
            mavlink_mission_item_t item;
            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];
            }
165
            
166 167 168 169 170 171 172 173 174 175 176 177 178
            mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
                                               MAV_COMP_ID_MISSIONPLANNER,
                                               _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);
179 180 181
            _mockLink->respondWithMavlinkMessage(responseMsg);
        }
    }
182 183
}

184
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
185
{
186 187 188 189 190 191 192 193
    mavlink_mission_count_t missionCount;
    
    mavlink_msg_mission_count_decode(&msg, &missionCount);
    Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());
    
    _writeSequenceCount = missionCount.count;
    Q_ASSERT(_writeSequenceCount >= 0);
    
194
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
195 196 197 198
    
    _missionItems.clear();
    
    if (_writeSequenceCount == 0) {
199
        _sendAck(MAV_MISSION_ACCEPTED);
200
    } else {
201 202 203 204 205 206 207 208 209 210
        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;
211 212
        _writeSequenceIndex = 0;
        _requestNextMissionItem(_writeSequenceIndex);
213 214 215
    }
}

216
void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
217
{
218
    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
219
    
220 221
    if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240
    } else {
        if (sequenceNumber >= _writeSequenceCount) {
            qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
            return;
        }
        
        if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
            (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
            sequenceNumber ++;
        }
        
        if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
            (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
            qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
            _sendAck(MAV_MISSION_ERROR);
        } else {
            mavlink_message_t           message;
            mavlink_mission_request_t   missionRequest;
            
241 242
            missionRequest.target_system =      _mavlinkProtocol->getSystemId();
            missionRequest.target_component =   _mavlinkProtocol->getComponentId();
243 244
            missionRequest.seq =                sequenceNumber;
            
245 246 247 248 249
            mavlink_msg_mission_request_encode_chan(_mockLink->vehicleId(),
                                                    MAV_COMP_ID_MISSIONPLANNER,
                                                    _mockLink->mavlinkChannel(),
                                                    &message,
                                                    &missionRequest);
250 251 252 253 254
            _mockLink->respondWithMavlinkMessage(message);

            // If response with Mission Item doesn't come before timer fires it's an error
            _startMissionItemResponseTimer();
        }
255
    }
256 257 258 259 260 261 262 263
}

void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
    
    mavlink_message_t       message;
    mavlink_mission_ack_t   missionAck;
264
    
265 266
    missionAck.target_system =      _mavlinkProtocol->getSystemId();
    missionAck.target_component =   _mavlinkProtocol->getComponentId();
267
    missionAck.type =               ackType;
268
    
269 270 271 272 273
    mavlink_msg_mission_ack_encode_chan(_mockLink->vehicleId(),
                                        MAV_COMP_ID_MISSIONPLANNER,
                                        _mockLink->mavlinkChannel(),
                                        &message,
                                        &missionAck);
274
    _mockLink->respondWithMavlinkMessage(message);
275 276
}

277
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
278
{
279 280
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
    
281 282
    _missionItemResponseTimer->stop();
    
283 284 285 286 287 288 289 290 291 292
    mavlink_mission_item_t missionItem;
    
    mavlink_msg_mission_item_decode(&msg, &missionItem);
    
    Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
    
    _missionItems[missionItem.seq] = missionItem;
    
    _writeSequenceIndex++;
    if (_writeSequenceIndex < _writeSequenceCount) {
293 294 295 296 297 298
        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
            // Send MAV_MISSION_ACCPETED ack too early
            _sendAck(MAV_MISSION_ACCEPTED);
        } else {
            _requestNextMissionItem(_writeSequenceIndex);
        }
299
    } else {
300 301 302 303 304 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 330 331 332 333
        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);
}

334
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
335 336 337 338 339 340 341 342
{
    _failureMode = failureMode;
}

void MockLinkMissionItemHandler::shutdown(void)
{
    if (_missionItemResponseTimer) {
        delete _missionItemResponseTimer;
343 344
    }
}