diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 28091f95ad755de4ab38c879178f43fab26b1d13..edcb720d272982f3084604c8721501f0cd023c12 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -199,6 +199,94 @@ void QGCUASFileManager::_listAckResponse(Request* listAck) } } + +/// @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); +} + + void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) { Q_UNUSED(link); @@ -259,6 +347,14 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me _readAckResponse(request); break; + case kCOCreate: + _createAckResponse(request); + break; + + case kCOWrite: + _writeAckResponse(request); + break; + default: _emitErrorMessage(tr("Ack received in unexpected state")); break; @@ -365,6 +461,47 @@ void QGCUASFileManager::downloadPath(const QString& from, const QDir& downloadDi _sendRequest(&request); } +/// @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); +} + + + QString QGCUASFileManager::errorString(uint8_t errorCode) { switch(errorCode) { @@ -441,6 +578,12 @@ void QGCUASFileManager::_ackTimeout(void) _emitErrorMessage(tr("Timeout waiting for ack: Sending Terminate command")); _sendTerminateCommand(); break; + case kCOCreate: + case kCOWrite: + _currentOperation = kCOAck; + _emitErrorMessage(tr("Timeout waiting for ack: Sending Terminate command")); + _sendTerminateCommand(); + break; default: _currentOperation = kCOIdle; _emitErrorMessage(tr("Timeout waiting for ack")); diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 893ce4e70fa4f3a3c9b53a8ef0a767faeec15f16..ebdb2e5a32288dd2eb84b74e5d005935751c4157 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -64,14 +64,25 @@ signals: /// @brief Signalled during file download to indicate download progress /// @param bytesReceived Number of bytes currently received from file void downloadFileProgress(unsigned int bytesReceived); - + /// @brief Signaled to indicate completion of file download. If an error occurs during download this signal will not be emitted. void downloadFileComplete(void); + /// @brief Signalled after createFile acknowledge is returned to indicate length of file being downloaded + void uploadFileLength(unsigned int length); + + /// @brief Signalled during file upload to indicate progress + /// @param bytesReceived Number of bytes currently transmitted to file + void uploadFileProgress(unsigned int bytesReceived); + + /// @brief Signaled to indicate completion of file upload. If an error occurs during download this signal will not be emitted. + void uploadFileComplete(void); + public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); void listDirectory(const QString& dirPath); void downloadPath(const QString& from, const QDir& downloadDir); + void uploadPath(const QString& toPath, const QFileInfo& uploadFile); protected: @@ -144,6 +155,8 @@ protected: kCOList, // waiting for List response kCOOpen, // waiting for Open response kCORead, // waiting for Read response + kCOCreate, // waiting for Create response + kCOWrite, // waiting for Write response }; @@ -161,6 +174,9 @@ protected: void _openAckResponse(Request* openAck); void _readAckResponse(Request* readAck); void _listAckResponse(Request* listAck); + void _createAckResponse(Request* createAck); + void _writeAckResponse(Request* writeAck); + void _writeFileDatablock(void); void _sendListCommand(void); void _sendTerminateCommand(void); void _closeReadSession(bool success); @@ -179,10 +195,14 @@ protected: uint8_t _activeSession; ///< currently active session, 0 for none uint32_t _readOffset; ///< current read offset + uint32_t _writeOffset; ///< current write offset + uint32_t _writeSize; ///< current write data size + uint32_t _writeFileSize; ///< current write file size QByteArray _readFileAccumulator; ///< Holds file being downloaded + QByteArray _writeFileAccumulator; ///< Holds file being uploaded QDir _readFileDownloadDir; ///< Directory to download file to QString _readFileDownloadFilename; ///< Filename (no path) for download file - + uint8_t _systemIdQGC; ///< System ID for QGC uint8_t _systemIdServer; ///< System ID for server