mavlink-ftp - Initial code structure for write file support

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

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