MockLinkMissionItemHandler.cc 14 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 20 21
    : _mockLink(mockLink)
    , _missionItemResponseTimer(NULL)
    , _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 48
{
    switch (msg.msgid) {
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
Don Gagne's avatar
Don Gagne committed
49
            _handleMissionRequestList(msg);
50
            break;
Don Gagne's avatar
Don Gagne committed
51
            
52
        case MAVLINK_MSG_ID_MISSION_REQUEST:
Don Gagne's avatar
Don Gagne committed
53
            _handleMissionRequest(msg);
54
            break;
Don Gagne's avatar
Don Gagne committed
55 56 57 58 59
            
        case MAVLINK_MSG_ID_MISSION_ITEM:
            _handleMissionItem(msg);
            break;
            
60
        case MAVLINK_MSG_ID_MISSION_COUNT:
Don Gagne's avatar
Don Gagne committed
61
            _handleMissionCount(msg);
62
            break;
Don Gagne's avatar
Don Gagne committed
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
Don Gagne's avatar
Don Gagne committed
74
            _missionItems.clear();
75
            break;
Don Gagne's avatar
Don Gagne committed
76 77 78
            
        default:
            return false;
79
    }
Don Gagne's avatar
Don Gagne committed
80 81
    
    return true;
82 83
}

Don Gagne's avatar
Don Gagne committed
84
void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
85
{
Don Gagne's avatar
Don Gagne committed
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());
Don Gagne's avatar
Don Gagne committed
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
        mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(),
                                            MAV_COMP_ID_MISSIONPLANNER,
                                            _mockLink->mavlinkChannel(),
Donald Gagne's avatar
Donald Gagne committed
113 114 115 116 117
                                            &responseMsg,               // Outgoing message
                                            msg.sysid,                  // Target is original sender
                                            msg.compid,                 // Target is original sender
                                            itemCount,                  // Number of mission items
                                            MAV_MISSION_TYPE_MISSION);
118 119
        _mockLink->respondWithMavlinkMessage(responseMsg);
    }
Don Gagne's avatar
Don Gagne committed
120
}
121

Don Gagne's avatar
Don Gagne committed
122
void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
123
{
Don Gagne's avatar
Don Gagne committed
124
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence";
125
    
Don Gagne's avatar
Don Gagne committed
126
    mavlink_mission_request_t request;
127
    
Don Gagne's avatar
Don Gagne committed
128
    mavlink_msg_mission_request_decode(&msg, &request);
129
    
Don Gagne's avatar
Don Gagne committed
130 131
    Q_ASSERT(request.target_system == _mockLink->vehicleId());
    Q_ASSERT(request.seq < _missionItems.count());
132
    
133 134 135 136 137 138 139
    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";
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155
    } 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;
            
Don Gagne's avatar
Don Gagne committed
156 157 158 159 160 161 162 163 164 165
            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];
            }
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,
Donald Gagne's avatar
Donald Gagne committed
179 180
                                               item.x, item.y, item.z,
                                               MAV_MISSION_TYPE_MISSION);
181 182 183
            _mockLink->respondWithMavlinkMessage(responseMsg);
        }
    }
184 185
}

Don Gagne's avatar
Don Gagne committed
186
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
187
{
Don Gagne's avatar
Don Gagne committed
188 189 190 191 192 193 194 195
    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);
    
196
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
Don Gagne's avatar
Don Gagne committed
197 198 199 200
    
    _missionItems.clear();
    
    if (_writeSequenceCount == 0) {
201
        _sendAck(MAV_MISSION_ACCEPTED);
202
    } else {
203 204 205 206 207 208 209 210 211 212
        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
213 214
        _writeSequenceIndex = 0;
        _requestNextMissionItem(_writeSequenceIndex);
215 216 217
    }
}

Don Gagne's avatar
Don Gagne committed
218
void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
219
{
220
    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
Don Gagne's avatar
Don Gagne committed
221
    
222 223
    if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242
    } 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;
            
243 244
            missionRequest.target_system =      _mavlinkProtocol->getSystemId();
            missionRequest.target_component =   _mavlinkProtocol->getComponentId();
245 246
            missionRequest.seq =                sequenceNumber;
            
247 248 249 250 251
            mavlink_msg_mission_request_encode_chan(_mockLink->vehicleId(),
                                                    MAV_COMP_ID_MISSIONPLANNER,
                                                    _mockLink->mavlinkChannel(),
                                                    &message,
                                                    &missionRequest);
252 253 254 255 256
            _mockLink->respondWithMavlinkMessage(message);

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

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

Don Gagne's avatar
Don Gagne committed
279
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
280
{
Don Gagne's avatar
Don Gagne committed
281 282
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
    
283 284
    _missionItemResponseTimer->stop();
    
Don Gagne's avatar
Don Gagne committed
285 286 287 288 289 290 291 292 293 294
    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) {
295 296 297 298 299 300
        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
            // Send MAV_MISSION_ACCPETED ack too early
            _sendAck(MAV_MISSION_ACCEPTED);
        } else {
            _requestNextMissionItem(_writeSequenceIndex);
        }
301
    } else {
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 334 335
        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);
}

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

void MockLinkMissionItemHandler::shutdown(void)
{
    if (_missionItemResponseTimer) {
        delete _missionItemResponseTimer;
345 346
    }
}