QGCUASFileManager.cc 20.4 KB
Newer Older
1
/*=====================================================================
2

3
 QGroundControl Open Source Ground Control Station
4

5
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
6

7
 This file is part of the QGROUNDCONTROL project
8

9 10 11 12
 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.
13

14 15 16 17
 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.
18

19 20
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
21

22 23
 ======================================================================*/

24 25
#include "QGCUASFileManager.h"
#include "QGC.h"
Lorenz Meier's avatar
Lorenz Meier committed
26
#include "MAVLinkProtocol.h"
27
#include "MainWindow.h"
Lorenz Meier's avatar
Lorenz Meier committed
28 29 30

#include <QFile>
#include <QDir>
none's avatar
none committed
31
#include <string>
Lorenz Meier's avatar
Lorenz Meier committed
32

33

34
QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas, uint8_t unitTestSystemIdQGC) :
35
    QObject(parent),
36
    _currentOperation(kCOIdle),
Lorenz Meier's avatar
Lorenz Meier committed
37
    _mav(uas),
38
    _lastOutgoingSeqNumber(0),
39 40
    _activeSession(0),
    _systemIdQGC(unitTestSystemIdQGC)
Lorenz Meier's avatar
Lorenz Meier committed
41
{
42 43 44
    connect(&_ackTimer, &QTimer::timeout, this, &QGCUASFileManager::_ackTimeout);
    
    _systemIdServer = _mav->getUASID();
45 46
    
    // Make sure we don't have bad structure packing
47
    Q_ASSERT(sizeof(RequestHeader) == 12);
Lorenz Meier's avatar
Lorenz Meier committed
48 49
}

50 51
/// @brief Respond to the Ack associated with the Open command with the next Read command.
void QGCUASFileManager::_openAckResponse(Request* openAck)
52 53 54
{
    _currentOperation = kCORead;
    _activeSession = openAck->hdr.session;
55 56 57
    
    // File length comes back in data
    Q_ASSERT(openAck->hdr.size == sizeof(uint32_t));
58 59 60
    emit downloadFileLength(openAck->openFileLength);
    
    // Start the sequence of read commands
61

62 63
    _readOffset = 0;                // Start reading at beginning of file
    _readFileAccumulator.clear();   // Start with an empty file
64

65 66
    Request request;
    request.hdr.session = _activeSession;
67
    request.hdr.opcode = kCmdReadFile;
68 69
    request.hdr.offset = _readOffset;
    request.hdr.size = sizeof(request.data);
70

71 72 73
    _sendRequest(&request);
}

74 75 76 77 78 79
/// @brief Closes out a read session by writing the file and doing cleanup.
///     @param success true: successful download completion, false: error during download
void QGCUASFileManager::_closeReadSession(bool success)
{
    if (success) {
        QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename);
80

81 82 83 84 85
        QFile file(downloadFilePath);
        if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
            _emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath));
            return;
        }
86

87 88 89 90 91 92 93
        qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length());
        if (bytesWritten != _readFileAccumulator.length()) {
            file.close();
            _emitErrorMessage(tr("Unable to write data to local file (%1)").arg(downloadFilePath));
            return;
        }
        file.close();
94

95
        emit downloadFileComplete();
96
    }
97

98 99 100 101
    // Close the open session
    _sendTerminateCommand();
}

102
/// @brief Respond to the Ack associated with the Read command.
103
void QGCUASFileManager::_readAckResponse(Request* readAck)
104 105 106 107 108 109 110
{
    if (readAck->hdr.session != _activeSession) {
        _currentOperation = kCOIdle;
        _readFileAccumulator.clear();
        _emitErrorMessage(tr("Read: Incorrect session returned"));
        return;
    }
111

112 113 114 115 116 117 118 119
    if (readAck->hdr.offset != _readOffset) {
        _currentOperation = kCOIdle;
        _readFileAccumulator.clear();
        _emitErrorMessage(tr("Read: Offset returned (%1) differs from offset requested (%2)").arg(readAck->hdr.offset).arg(_readOffset));
        return;
    }

    _readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size);
120
    emit downloadFileProgress(_readFileAccumulator.length());
121

122 123
    if (readAck->hdr.size == sizeof(readAck->data)) {
        // Possibly still more data to read, send next read request
124

125
        _currentOperation = kCORead;
126

127
        _readOffset += readAck->hdr.size;
128

129 130
        Request request;
        request.hdr.session = _activeSession;
131
        request.hdr.opcode = kCmdReadFile;
132
        request.hdr.offset = _readOffset;
133
        request.hdr.size = 0;
134

135 136
        _sendRequest(&request);
    } else {
137
        // We only receieved a partial buffer back. These means we are at EOF
138
        _currentOperation = kCOIdle;
139
        _closeReadSession(true /* success */);
140 141 142 143 144 145 146 147 148 149 150
    }
}

/// @brief Respond to the Ack associated with the List command.
void QGCUASFileManager::_listAckResponse(Request* listAck)
{
    if (listAck->hdr.offset != _listOffset) {
        _currentOperation = kCOIdle;
        _emitErrorMessage(tr("List: Offset returned (%1) differs from offset requested (%2)").arg(listAck->hdr.offset).arg(_listOffset));
        return;
    }
151

152 153 154
    uint8_t offset = 0;
    uint8_t cListEntries = 0;
    uint8_t cBytes = listAck->hdr.size;
155

156 157 158
    // parse filenames out of the buffer
    while (offset < cBytes) {
        const char * ptr = ((const char *)listAck->data) + offset;
159

160 161
        // get the length of the name
        uint8_t cBytesLeft = cBytes - offset;
Don Gagne's avatar
Don Gagne committed
162
        uint8_t nlen = static_cast<uint8_t>(strnlen(ptr, cBytesLeft));
163
        if ((*ptr == 'S' && nlen > 1) || (*ptr != 'S' && nlen < 2)) {
164 165 166 167 168 169 170 171
            _currentOperation = kCOIdle;
            _emitErrorMessage(tr("Incorrectly formed list entry: '%1'").arg(ptr));
            return;
        } else if (nlen == cBytesLeft) {
            _currentOperation = kCOIdle;
            _emitErrorMessage(tr("Missing NULL termination in list entry"));
            return;
        }
172

173
        // Returned names are prepended with D for directory, F for file, S for skip
174 175
        if (*ptr == 'F' || *ptr == 'D') {
            // put it in the view
176
            _emitListEntry(ptr);
177 178
        } else if (*ptr == 'S') {
            // do nothing
179 180
        } else {
            qDebug() << "unknown entry" << ptr;
181
        }
182

183 184
        // account for the name + NUL
        offset += nlen + 1;
185

186 187 188
        cListEntries++;
    }

189 190 191 192
    if (listAck->hdr.size == 0) {
        // Directory is empty, we're done
        Q_ASSERT(listAck->hdr.opcode == kRspAck);
        _currentOperation = kCOIdle;
Don Gagne's avatar
Don Gagne committed
193
        emit listComplete();
194 195
    } else {
        // Possibly more entries to come, need to keep trying till we get EOF
196 197 198
        _currentOperation = kCOList;
        _listOffset += cListEntries;
        _sendListCommand();
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 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 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289

/// @brief Respond to the Ack associated with the create command.
void QGCUASFileManager::_createAckResponse(Request* createAck)
{
    _currentOperation = kCOWrite;
    _activeSession = createAck->hdr.session;

    // File length comes back in data. Compare with
    Q_ASSERT(createAck->hdr.size == sizeof(uint32_t));

    // Start the sequence of read commands

    _writeOffset = 0;                // Start writing at beginning of file
    _writeSize = 0;

    _writeFileDatablock();
}

/// @brief Respond to the Ack associated with the write command.
void QGCUASFileManager::_writeAckResponse(Request* writeAck)
{
    if (writeAck->hdr.session != _activeSession) {
        _currentOperation = kCOIdle;
        _writeFileAccumulator.clear();
        _emitErrorMessage(tr("Write: Incorrect session returned"));
        return;
    }

    if (writeAck->hdr.offset != _writeOffset) {
        _currentOperation = kCOIdle;
        _writeFileAccumulator.clear();
        _emitErrorMessage(tr("Write: Offset returned (%1) differs from offset requested (%2)").arg(writeAck->hdr.offset).arg(_writeOffset));
        return;
    }

    if (writeAck->hdr.size != _writeSize) {
        _currentOperation = kCOIdle;
        _writeFileAccumulator.clear();
        _emitErrorMessage(tr("Write: Offset returned (%1) differs from offset requested (%2)").arg(writeAck->hdr.offset).arg(_writeOffset));
        return;
    }

    if(writeAck->hdr.size !=_writeFileSize){
        _currentOperation = kCOIdle;
        _writeFileAccumulator.clear();
        _emitErrorMessage(tr("Write: Size returned (%1) differs from size requested (%2)").arg(writeAck->hdr.size).arg(_writeSize));
        return;
    }

    _writeFileDatablock();
}


/// @brief Send next write file data block.
void QGCUASFileManager::_writeFileDatablock(void)
{
    /// @brief Maximum data size in RequestHeader::data
//	static const uint8_t	kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
//    static const uint8_t	kMaxDataLength = Request.data;

    if(_writeOffset + _writeSize >= _writeFileSize){
        _currentOperation = kCOIdle;
        _writeFileAccumulator.clear();
        emit uploadFileComplete();
    }

    Request request;
    request.hdr.session = _activeSession;
    request.hdr.opcode = kCmdWriteFile;
    request.hdr.offset = _writeOffset;
    _writeOffset += _writeSize;

    if(_writeOffset + sizeof(request.data) < _writeFileSize )
        _writeSize = sizeof(request.data);
    else
        _writeSize = _writeFileSize - _writeOffset - 1;

    request.hdr.size = _writeSize;

    // memcpy this?   _writeFileAccumulator.mid(_writeOffset, _writeSize), _writeSize);
    for(uint32_t index=_writeOffset; index < _writeOffset+_writeSize; index++)
        request.data[index] = _writeFileAccumulator.at(index);


    _sendRequest(&request);
}


Lorenz Meier's avatar
Lorenz Meier committed
290 291
void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
292
    Q_UNUSED(link);
293

294 295
    // receiveMessage is signalled will all mavlink messages so we need to filter everything else out but ours.
    if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
296 297
        return;
    }
298
    
299 300 301 302 303 304
    mavlink_file_transfer_protocol_t data;
    mavlink_msg_file_transfer_protocol_decode(&message, &data);
    
    // Make sure we are the target system
    if (data.target_system != _systemIdQGC) {
        qDebug() << "Received MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL with possibly incorrect system id" << _systemIdQGC;
305 306
        return;
    }
307 308 309
    
    Request* request = (Request*)&data.payload[0];
    
310 311
    _clearAckTimeout();
    
312
    uint16_t incomingSeqNumber = request->hdr.seqNumber;
313 314 315 316 317 318 319 320 321 322 323
    
    // Make sure we have a good sequence number
    uint16_t expectedSeqNumber = _lastOutgoingSeqNumber + 1;
    if (incomingSeqNumber != expectedSeqNumber) {
        _currentOperation = kCOIdle;
        _emitErrorMessage(tr("Bad sequence number on received message: expected(%1) received(%2)").arg(expectedSeqNumber).arg(incomingSeqNumber));
        return;
    }
    
    // Move past the incoming sequence number for next request
    _lastOutgoingSeqNumber = incomingSeqNumber;
324

325
    if (request->hdr.opcode == kRspAck) {
326

327 328 329 330 331
        switch (_currentOperation) {
            case kCOIdle:
                // we should not be seeing anything here.. shut the other guy up
                _sendCmdReset();
                break;
332

333 334 335 336
            case kCOAck:
                // We are expecting an ack back
                _currentOperation = kCOIdle;
                break;
337

338
            case kCOList:
339
                _listAckResponse(request);
340
                break;
341

342
            case kCOOpen:
343
                _openAckResponse(request);
344
                break;
345

346
            case kCORead:
347
                _readAckResponse(request);
348 349
                break;

350 351 352 353 354 355 356 357
            case kCOCreate:
                _createAckResponse(request);
                break;

            case kCOWrite:
                _writeAckResponse(request);
                break;

358
            default:
Don Gagne's avatar
Don Gagne committed
359
                _emitErrorMessage(tr("Ack received in unexpected state"));
360 361 362
                break;
        }
    } else if (request->hdr.opcode == kRspNak) {
363

364 365
        OperationState previousOperation = _currentOperation;
        uint8_t errorCode = request->data[0];
366

367 368 369
        // Nak's normally have 1 byte of data for error code, except for kErrFailErrno which has additional byte for errno
        Q_ASSERT((errorCode == kErrFailErrno && request->hdr.size == 2) || request->hdr.size == 1);
        
370
        _currentOperation = kCOIdle;
371

372 373
        if (previousOperation == kCOList && errorCode == kErrEOF) {
            // This is not an error, just the end of the read loop
Don Gagne's avatar
Don Gagne committed
374
            emit listComplete();
375 376 377 378 379 380 381 382 383 384 385 386 387
            return;
        } else if (previousOperation == kCORead && errorCode == kErrEOF) {
            // This is not an error, just the end of the read loop
            _closeReadSession(true /* success */);
            return;
        } else {
            // Generic Nak handling
            if (previousOperation == kCORead) {
                // Nak error during read loop, download failed
                _closeReadSession(false /* failure */);
            }
            _emitErrorMessage(tr("Nak received, error: %1").arg(errorString(request->data[0])));
        }
388 389 390
    } else {
        // Note that we don't change our operation state. If something goes wrong beyond this, the operation
        // will time out.
391
        _emitErrorMessage(tr("Unknown opcode returned from server: %1").arg(request->hdr.opcode));
Lorenz Meier's avatar
Lorenz Meier committed
392 393 394
    }
}

395
void QGCUASFileManager::listDirectory(const QString& dirPath)
Lorenz Meier's avatar
Lorenz Meier committed
396
{
397 398 399
    if (_currentOperation != kCOIdle) {
        _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete."));
        return;
none's avatar
none committed
400 401 402
    }

    // initialise the lister
403
    _listPath = dirPath;
404 405
    _listOffset = 0;
    _currentOperation = kCOList;
none's avatar
none committed
406 407

    // and send the initial request
408
    _sendListCommand();
none's avatar
none committed
409 410
}

411 412 413
void QGCUASFileManager::_fillRequestWithString(Request* request, const QString& str)
{
    strncpy((char *)&request->data[0], str.toStdString().c_str(), sizeof(request->data));
Don Gagne's avatar
Don Gagne committed
414
    request->hdr.size = static_cast<uint8_t>(strnlen((const char *)&request->data[0], sizeof(request->data)));
415 416
}

417
void QGCUASFileManager::_sendListCommand(void)
none's avatar
none committed
418
{
419
    Request request;
none's avatar
none committed
420

421
    request.hdr.session = 0;
422
    request.hdr.opcode = kCmdListDirectory;
423
    request.hdr.offset = _listOffset;
424
    request.hdr.size = 0;
425

426
    _fillRequestWithString(&request, _listPath);
427

428
    _sendRequest(&request);
Lorenz Meier's avatar
Lorenz Meier committed
429 430
}

431 432 433 434
/// @brief Downloads the specified file.
///     @param from File to download from UAS, fully qualified path
///     @param downloadDir Local directory to download file to
void QGCUASFileManager::downloadPath(const QString& from, const QDir& downloadDir)
Lorenz Meier's avatar
Lorenz Meier committed
435
{
436 437 438
    if (from.isEmpty()) {
        return;
    }
439

440
    _readFileDownloadDir.setPath(downloadDir.absolutePath());
441

442 443 444 445 446 447 448 449 450 451
    // We need to strip off the file name from the fully qualified path. We can't use the usual QDir
    // routines because this path does not exist locally.
    int i;
    for (i=from.size()-1; i>=0; i--) {
        if (from[i] == '/') {
            break;
        }
    }
    i++; // move past slash
    _readFileDownloadFilename = from.right(from.size() - i);
452

453
    _currentOperation = kCOOpen;
Lorenz Meier's avatar
Lorenz Meier committed
454

455 456
    Request request;
    request.hdr.session = 0;
457
    request.hdr.opcode = kCmdOpenFile;
458
    request.hdr.offset = 0;
459
    request.hdr.size = 0;
460 461
    _fillRequestWithString(&request, from);
    _sendRequest(&request);
462
}
none's avatar
none committed
463

464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504
/// @brief Uploads the specified file.
///     @param toPath File in UAS to upload to, fully qualified path
///     @param uploadFile Local file to upload from
void QGCUASFileManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile)
{
    if (toPath.isEmpty()) {
        return;
    }

    if(not uploadFile.isReadable()){
        _emitErrorMessage(tr("File (%1) is not readable for upload").arg(uploadFile.path()));
    }

    QFile file(uploadFile.absoluteFilePath());
    if (!file.open(QIODevice::ReadOnly | QIODevice::Truncate)) {
            _emitErrorMessage(tr("Unable to open local file for upload (%1)").arg(uploadFile.absoluteFilePath()));
            return;
        }

    _writeFileAccumulator = file.readAll();

    qint64 bytesRead = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length());
    if (bytesRead <= 0) {
        file.close();
        _emitErrorMessage(tr("Unable to read data from local file (%1)").arg(uploadFile.absoluteFilePath()));
        return;
    }

    _currentOperation = kCOCreate;

    Request request;
    request.hdr.session = 0;
    request.hdr.opcode = kCmdCreateFile;
    request.hdr.offset = 0;
    request.hdr.size = bytesRead;
    _fillRequestWithString(&request, toPath);
    _sendRequest(&request);
}



none's avatar
none committed
505 506 507
QString QGCUASFileManager::errorString(uint8_t errorCode)
{
    switch(errorCode) {
508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525
        case kErrNone:
            return QString("no error");
        case kErrFail:
            return QString("unknown error");
        case kErrEOF:
            return QString("read beyond end of file");
        case kErrUnknownCommand:
            return QString("unknown command");
        case kErrFailErrno:
            return QString("command failed");
        case kErrInvalidDataSize:
            return QString("invalid data size");
        case kErrInvalidSession:
            return QString("invalid session");
        case kErrNoSessionsAvailable:
            return QString("no sessions availble");
        default:
            return QString("unknown error code");
none's avatar
none committed
526 527
    }
}
528 529 530 531 532 533 534 535 536 537 538

/// @brief Sends a command which only requires an opcode and no additional data
///     @param opcode Opcode to send
///     @param newOpState State to put state machine into
/// @return TRUE: command sent, FALSE: command not sent, waiting for previous command to finish
bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState)
{
    if (_currentOperation != kCOIdle) {
        // Can't have multiple commands in play at the same time
        return false;
    }
539

540 541 542 543 544
    Request request;
    request.hdr.session = 0;
    request.hdr.opcode = opcode;
    request.hdr.offset = 0;
    request.hdr.size = 0;
545

546
    _currentOperation = newOpState;
547

548
    _sendRequest(&request);
549

550
    return true;
551 552 553 554 555 556
}

/// @brief Starts the ack timeout timer
void QGCUASFileManager::_setupAckTimeout(void)
{
    Q_ASSERT(!_ackTimer.isActive());
557

558
    _ackTimer.setSingleShot(true);
Don Gagne's avatar
Don Gagne committed
559
    _ackTimer.start(ackTimerTimeoutMsecs);
560 561 562 563 564 565 566 567 568 569 570
}

/// @brief Clears the ack timeout timer
void QGCUASFileManager::_clearAckTimeout(void)
{
    _ackTimer.stop();
}

/// @brief Called when ack timeout timer fires
void QGCUASFileManager::_ackTimeout(void)
{
Don Gagne's avatar
Don Gagne committed
571 572 573
    // Make sure to set _currentOperation state before emitting error message. Code may respond
    // to error message signal by sending another command, which will fail if state is not back
    // to idle. FileView UI works this way with the List command.
574 575 576 577

    switch (_currentOperation) {
        case kCORead:
            _currentOperation = kCOAck;
Don Gagne's avatar
Don Gagne committed
578
            _emitErrorMessage(tr("Timeout waiting for ack: Sending Terminate command"));
579 580
            _sendTerminateCommand();
            break;
581 582 583 584 585 586
        case kCOCreate:
        case kCOWrite:
            _currentOperation = kCOAck;
            _emitErrorMessage(tr("Timeout waiting for ack: Sending Terminate command"));
            _sendTerminateCommand();
            break;
587 588
        default:
            _currentOperation = kCOIdle;
Don Gagne's avatar
Don Gagne committed
589
            _emitErrorMessage(tr("Timeout waiting for ack"));
590 591 592 593 594 595 596 597
            break;
    }
}

void QGCUASFileManager::_sendTerminateCommand(void)
{
    Request request;
    request.hdr.session = _activeSession;
598
    request.hdr.opcode = kCmdTerminateSession;
599
    request.hdr.size = 0;
600
    _sendRequest(&request);
601 602 603 604
}

void QGCUASFileManager::_emitErrorMessage(const QString& msg)
{
605
    qDebug() << "QGCUASFileManager: Error" << msg;
606 607 608
    emit errorMessage(msg);
}

609
void QGCUASFileManager::_emitListEntry(const QString& entry)
610
{
611 612
    qDebug() << "QGCUASFileManager: list entry" << entry;
    emit listEntry(entry);
613 614
}

615 616 617 618
/// @brief Sends the specified Request out to the UAS.
void QGCUASFileManager::_sendRequest(Request* request)
{
    mavlink_message_t message;
619

620
    _setupAckTimeout();
621 622
    
    _lastOutgoingSeqNumber++;
623

624 625 626
    request->hdr.seqNumber = _lastOutgoingSeqNumber;
    
    if (_systemIdQGC == 0) {
627
        _systemIdQGC = MAVLinkProtocol::instance()->getSystemId();
628 629 630 631 632 633 634 635 636 637 638
    }
    
    Q_ASSERT(_mav);
    mavlink_msg_file_transfer_protocol_pack(_systemIdQGC,       // QGC System ID
                                            0,                  // QGC Component ID
                                            &message,           // Mavlink Message to pack into
                                            0,                  // Target network
                                            _systemIdServer,    // Target system
                                            0,                  // Target component
                                            (uint8_t*)request); // Payload
    
639
    _mav->sendMessage(message);
Lorenz Meier's avatar
Lorenz Meier committed
640
}