Merge pull request #1529 from crashmatt/master

Working basic mavlink ftp file upload

src/uas/QGCUASFileManager.cc

@@ -199,6 +199,96 @@ 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;
+
+    // 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(_writeOffset + _writeSize >= _writeFileSize){
+        _writeFileAccumulator.clear();
+        _writeFileSize = 0;
+        _currentOperation = kCOIdle;
+        emit uploadFileComplete();
+    }
+
+    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 != sizeof(uint32_t)) {
+        _currentOperation = kCOIdle;
+        _writeFileAccumulator.clear();
+        _emitErrorMessage(tr("Write: Returned invalid size of write size data"));
+        return;
+    }
+
+
+    if( writeAck->writeFileLength !=_writeSize){
+        _currentOperation = kCOIdle;
+        _writeFileAccumulator.clear();
+        _emitErrorMessage(tr("Write: Size returned (%1) differs from size requested (%2)").arg(writeAck->writeFileLength).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){
+        _sendTerminateCommand();
+        return;
+    }
+
+    _writeOffset += _writeSize;
+
+    Request request;
+    request.hdr.session = _activeSession;
+    request.hdr.opcode = kCmdWriteFile;
+    request.hdr.offset = _writeOffset;
+
+    if(_writeFileSize -_writeOffset > sizeof(request.data) )
+        _writeSize = sizeof(request.data);
+    else
+        _writeSize = _writeFileSize - _writeOffset;
+
+    request.hdr.size = _writeSize;
+
+    memcpy(request.data, &_writeFileAccumulator.data()[_writeOffset], _writeSize);
+
+    _sendRequest(&request);
+}
+
+
 void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message)
 {
     Q_UNUSED(link);
@@ -259,6 +349,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;
@@ -281,6 +379,11 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me
             // This is not an error, just the end of the read loop
             _closeReadSession(true /* success */);
             return;
+        } else if (previousOperation == kCOCreate) {
+            // End a failed create file operation
+            _sendTerminateCommand();
+            _emitErrorMessage(tr("Nak received creating file, error: %1").arg(errorString(request->data[0])));
+            return;
         } else {
             // Generic Nak handling
             if (previousOperation == kCORead) {
@@ -365,6 +468,53 @@ 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(_currentOperation != kCOIdle){
+        _emitErrorMessage(tr("UAS File manager busy.  Try again later"));
+        return;
+    }
+
+    if (toPath.isEmpty()) {
+        return;
+    }
+
+    if(!uploadFile.isReadable()){
+        _emitErrorMessage(tr("File (%1) is not readable for upload").arg(uploadFile.path()));
+    }
+
+    QFile file(uploadFile.absoluteFilePath());
+    if (!file.open(QIODevice::ReadOnly)) {
+            _emitErrorMessage(tr("Unable to open local file for upload (%1)").arg(uploadFile.absoluteFilePath()));
+            return;
+        }
+
+    _writeFileAccumulator = file.readAll();
+    _writeFileSize = _writeFileAccumulator.size();
+
+    file.close();
+
+    if (_writeFileAccumulator.size() == 0) {
+        _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 = 0;
+    _fillRequestWithString(&request, toPath + "/" + uploadFile.fileName());
+    _sendRequest(&request);
+}
+
+
+
 QString QGCUASFileManager::errorString(uint8_t errorCode)
 {
     switch(errorCode) {
@@ -384,6 +534,10 @@ QString QGCUASFileManager::errorString(uint8_t errorCode)
             return QString("invalid session");
         case kErrNoSessionsAvailable:
             return QString("no sessions availble");
+        case kErrFailFileExists:
+            return QString("File already exists on target");
+        case kErrFailFileProtected:
+            return QString("File is write protected");
         default:
             return QString("unknown error code");
     }
@@ -441,6 +595,17 @@ void QGCUASFileManager::_ackTimeout(void)
             _emitErrorMessage(tr("Timeout waiting for ack: Sending Terminate command"));
             _sendTerminateCommand();
             break;
+        case kCOCreate:
+            _currentOperation = kCOAck;
+            _writeFileAccumulator.clear();
+            _emitErrorMessage(tr("Timeout waiting for ack: Sending Create command"));
+            _sendTerminateCommand();
+        case kCOWrite:
+            _currentOperation = kCOAck;
+            _writeFileAccumulator.clear();
+            _emitErrorMessage(tr("Timeout waiting for ack: Sending Write 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:
     
@@ -100,6 +111,9 @@ protected:
 
             // File length returned by Open command
             uint32_t openFileLength;
+
+            // Length of file chunk written by write command
+            uint32_t writeFileLength;
         };
     };
 
@@ -134,7 +148,9 @@ protected:
 		kErrInvalidSession,         ///< Session is not currently open
 		kErrNoSessionsAvailable,	///< All available Sessions in use
 		kErrEOF,                    ///< Offset past end of file for List and Read commands
-		kErrUnknownCommand          ///< Unknown command opcode
+        kErrUnknownCommand,          ///< Unknown command opcode
+        kErrFailFileExists,         ///< File exists already
+        kErrFailFileProtected       ///< File is write protected
     };
 
     enum OperationState
@@ -144,6 +160,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 +179,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 +200,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
     

src/ui/QGCUASFileView.cc

@@ -33,7 +33,8 @@ QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) :
     QWidget(parent),
     _manager(manager),
     _listInProgress(false),
-    _downloadInProgress(false)
+    _downloadInProgress(false),
+    _uploadInProgress(false)
 {
     _ui.setupUi(this);
     
@@ -48,6 +49,8 @@ QGCUASFileView::QGCUASFileView(QWidget *parent, QGCUASFileManager *manager) :
     Q_ASSERT(success);
     success = connect(_ui.downloadButton, SIGNAL(clicked()), this, SLOT(_downloadFile()));
     Q_ASSERT(success);
+    success = connect(_ui.uploadButton, SIGNAL(clicked()), this, SLOT(_uploadFile()));
+    Q_ASSERT(success);
     success = connect(_ui.treeWidget, SIGNAL(currentItemChanged(QTreeWidgetItem*, QTreeWidgetItem*)), this, SLOT(_currentItemChanged(QTreeWidgetItem*, QTreeWidgetItem*)));
     Q_ASSERT(success);
 }
@@ -90,6 +93,36 @@ void QGCUASFileView::_downloadFile(void)
     }
 }
 
+/// @brief uploads a file into the currently selected directory the tree view
+void QGCUASFileView::_uploadFile(void)
+{
+    Q_ASSERT(!_uploadInProgress);
+
+    _ui.statusText->clear();
+
+    // get and check directory from list view
+    QTreeWidgetItem* item = _ui.treeWidget->currentItem();
+    if (item && item->type() != _typeDir) {
+        return;
+    }
+
+    // Find complete path for upload directory
+    QString path;
+    do {
+        QString name = item->text(0).split("\t")[0];    // Strip off file sizes
+        path.prepend("/" + name);
+        item = item->parent();
+    } while (item);
+
+    QString uploadFromHere = QGCFileDialog::getOpenFileName(this, tr("Upload File"),
+                                                               QDir::homePath());
+
+    qDebug() << "Upload: " << uploadFromHere << "to path" << path;
+
+    _manager->uploadPath(path, uploadFromHere);
+}
+
+
 /// @brief Called when length of file being downloaded is known.
 void QGCUASFileView::_downloadLength(unsigned int length)
 {
@@ -267,6 +300,7 @@ void QGCUASFileView::_currentItemChanged(QTreeWidgetItem* current, QTreeWidgetIt
     Q_UNUSED(previous);
     // FIXME: Should not enable when downloading
     _ui.downloadButton->setEnabled(current ? (current->type() == _typeFile) : false);
+    _ui.uploadButton->setEnabled(current ? (current->type() == _typeDir) : false);
 }
 
 void QGCUASFileView::_requestDirectoryList(const QString& dir)

src/ui/QGCUASFileView.h

@@ -47,6 +47,7 @@ private slots:
     void _listComplete(void);
     
     void _downloadFile(void);
+    void _uploadFile(void);
     void _downloadLength(unsigned int length);
     void _downloadProgress(unsigned int length);
     void _downloadErrorMessage(const QString& msg);
@@ -74,6 +75,7 @@ private:
     
     bool _listInProgress;       ///< Indicates that a listDirectory command is in progress
     bool _downloadInProgress;   ///< Indicates that a downloadPath command is in progress
+    bool _uploadInProgress;     ///< Indicates that a upload command is in progress
 };
 
 #endif // QGCUASFILEVIEW_H

src/ui/QGCUASFileView.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>200</width>
+    <width>216</width>
     <height>518</height>
    </rect>
   </property>
@@ -14,7 +14,7 @@
    <string>Form</string>
   </property>
   <layout class="QGridLayout" name="gridLayout">
-   <item row="3" column="1">
+   <item row="5" column="1">
     <widget class="QPushButton" name="listFilesButton">
      <property name="text">
       <string>List Files</string>
@@ -33,6 +33,9 @@
    </item>
    <item row="0" column="0" colspan="3">
     <widget class="QTreeWidget" name="treeWidget">
+     <property name="contextMenuPolicy">
+      <enum>Qt::NoContextMenu</enum>
+     </property>
      <property name="headerHidden">
       <bool>true</bool>
      </property>
@@ -43,7 +46,7 @@
      </column>
     </widget>
    </item>
-   <item row="3" column="2">
+   <item row="5" column="2">
     <widget class="QPushButton" name="downloadButton">
      <property name="enabled">
       <bool>false</bool>
@@ -53,7 +56,7 @@
      </property>
     </widget>
    </item>
-   <item row="2" column="0" colspan="3">
+   <item row="3" column="0" colspan="3">
     <widget class="QLabel" name="statusText">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
@@ -66,6 +69,16 @@
      </property>
     </widget>
    </item>
+   <item row="4" column="2">
+    <widget class="QPushButton" name="uploadButton">
+     <property name="enabled">
+      <bool>false</bool>
+     </property>
+     <property name="text">
+      <string>Upload File</string>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>