MockLinkMissionItemHandler.cc 12 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
/*=====================================================================
 
 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 "MockLinkMissionItemHandler.h"
25
#include "MockLink.h"
26 27 28

#include <QDebug>

29
QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")
30

31
MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink)
32 33 34
    : _mockLink(mockLink)
    , _missionItemResponseTimer(NULL)
    , _failureMode(FailNone)
35
{
36
    Q_ASSERT(mockLink);
37 38
}

39 40 41 42 43 44 45 46 47 48 49 50 51 52
MockLinkMissionItemHandler::~MockLinkMissionItemHandler()
{

}

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

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

92
void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
93
{
94
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
95
    
96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114
    if (_failureMode != FailReadRequestListNoResponse) {
        mavlink_mission_request_list_t request;
        
        mavlink_msg_mission_request_list_decode(&msg, &request);
        
        Q_ASSERT(request.target_system == _mockLink->vehicleId());
        
        mavlink_message_t   responseMsg;
        
        mavlink_msg_mission_count_pack(_mockLink->vehicleId(),
                                       MAV_COMP_ID_MISSIONPLANNER,
                                       &responseMsg,            // Outgoing message
                                       msg.sysid,               // Target is original sender
                                       msg.compid,              // Target is original sender
                                       _missionItems.count());  // Number of mission items
        _mockLink->respondWithMavlinkMessage(responseMsg);
    } else {
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
    }
115
    
116 117 118
    if (_failureFirstTimeOnly) {
        _failureMode = FailNone;
    }
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
    if ((_failureMode == FailReadRequest0NoResponse && request.seq == 0) ||
        (_failureMode == FailReadRequest1NoResponse && request.seq == 1)) {
134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode";
    } 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;
            
            mavlink_mission_item_t item = _missionItems[request.seq];
            
            mavlink_msg_mission_item_pack(_mockLink->vehicleId(),
                                          MAV_COMP_ID_MISSIONPLANNER,
                                          &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);
            _mockLink->respondWithMavlinkMessage(responseMsg);
        }
    }
168
    
169 170 171
    if (_failureFirstTimeOnly) {
        _failureMode = FailNone;
    }
172 173
}

174
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
175
{
176 177 178 179 180 181 182 183
    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);
    
184
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
185 186 187 188
    
    _missionItems.clear();
    
    if (_writeSequenceCount == 0) {
189
        _sendAck(MAV_MISSION_ACCEPTED);
190
    } else {
191 192
        _writeSequenceIndex = 0;
        _requestNextMissionItem(_writeSequenceIndex);
193 194 195
    }
}

196
void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
197
{
198
    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
199
    
200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231
    if ((_failureMode == FailWriteRequest0NoResponse && sequenceNumber == 0) ||
        (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1)) {
        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode";
    } 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;
            
            missionRequest.target_system =      MAVLinkProtocol::instance()->getSystemId();
            missionRequest.target_component =   MAVLinkProtocol::instance()->getComponentId();
            missionRequest.seq =                sequenceNumber;
            
            mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest);
            _mockLink->respondWithMavlinkMessage(message);

            // If response with Mission Item doesn't come before timer fires it's an error
            _startMissionItemResponseTimer();
        }
232
    }
233
    
234 235 236 237 238 239 240 241 242 243 244
    if (_failureFirstTimeOnly) {
        _failureMode = FailNone;
    }
}

void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
    qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
    
    mavlink_message_t       message;
    mavlink_mission_ack_t   missionAck;
245
    
246 247 248
    missionAck.target_system =      MAVLinkProtocol::instance()->getSystemId();
    missionAck.target_component =   MAVLinkProtocol::instance()->getComponentId();
    missionAck.type =               ackType;
249
    
250
    mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck);
251
    _mockLink->respondWithMavlinkMessage(message);
252 253
}

254
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
255
{
256 257
    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
    
258 259
    _missionItemResponseTimer->stop();
    
260 261 262 263 264 265 266 267 268 269 270 271 272
    mavlink_mission_item_t missionItem;
    
    mavlink_msg_mission_item_decode(&msg, &missionItem);
    
    Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
    
    Q_ASSERT(!_missionItems.contains(missionItem.seq));
    Q_ASSERT(missionItem.seq == _writeSequenceIndex);
    
    _missionItems[missionItem.seq] = missionItem;
    
    _writeSequenceIndex++;
    if (_writeSequenceIndex < _writeSequenceCount) {
273 274 275 276 277 278
        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
            // Send MAV_MISSION_ACCPETED ack too early
            _sendAck(MAV_MISSION_ACCEPTED);
        } else {
            _requestNextMissionItem(_writeSequenceIndex);
        }
279
    } else {
280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 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
        if (_failureMode != FailWriteFinalAckNoResponse) {
            MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED;
            
            if (_failureMode ==  FailWriteFinalAckErrorAck) {
                ack = MAV_MISSION_ERROR;
            }
            _sendAck(ack);
        }
    }
    if (_failureFirstTimeOnly) {
        _failureMode = FailNone;
    }
}

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

void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly)
{
    _failureFirstTimeOnly = firstTimeOnly;
    _failureMode = failureMode;
}

void MockLinkMissionItemHandler::shutdown(void)
{
    if (_missionItemResponseTimer) {
        delete _missionItemResponseTimer;
327 328
    }
}