Commit f12c0bf3 authored by Don Gagne's avatar Don Gagne

Add support for FTP open/read commands

parent e93fd779
...@@ -23,11 +23,160 @@ ...@@ -23,11 +23,160 @@
#include "MockMavlinkFileServer.h" #include "MockMavlinkFileServer.h"
void MockMavlinkFileServer::sendMessage(mavlink_message_t message) const char* MockMavlinkFileServer::smallFilename = "small";
const char* MockMavlinkFileServer::largeFilename = "large";
// FIXME: -2 to avoid eof on full packet
const uint8_t MockMavlinkFileServer::smallFileLength = sizeof(((QGCUASFileManager::Request*)0)->data) - 2;
const uint8_t MockMavlinkFileServer::largeFileLength = sizeof(((QGCUASFileManager::Request*)0)->data) + 1;
const uint8_t MockMavlinkFileServer::_sessionId = 1;
/// @brief Handles List command requests. Only supports root folder paths.
/// File list returned is set using the setFileList method.
void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request)
{ {
QGCUASFileManager::Request ackResponse; QGCUASFileManager::Request ackResponse;
QString path; QString path;
// We only support root path
path = (char *)&request->data[0];
if (!path.isEmpty() && path != "/") {
_sendNak(QGCUASFileManager::kErrNotDir);
return;
}
// Offset requested is past the end of the list
if (request->hdr.offset > (uint32_t)_fileList.size()) {
_sendNak(QGCUASFileManager::kErrEOF);
return;
}
ackResponse.hdr.magic = 'f';
ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
ackResponse.hdr.session = 0;
ackResponse.hdr.size = 0;
if (request->hdr.offset == 0) {
// Requesting first batch of file names
Q_ASSERT(_fileList.size());
char *bufPtr = (char *)&ackResponse.data[0];
for (int i=0; i<_fileList.size(); i++) {
const char *filename = _fileList[i].toStdString().c_str();
size_t cchFilename = strlen(filename);
strcpy(bufPtr, filename);
ackResponse.hdr.size += cchFilename + 1;
bufPtr += cchFilename + 1;
}
// Final double termination
*bufPtr = 0;
ackResponse.hdr.size++;
} else {
// All filenames fit in first ack, send final null terminated ack
ackResponse.data[0] = 0;
ackResponse.hdr.size = 1;
}
_emitResponse(&ackResponse);
}
/// @brief Handles Open command requests. Two filenames are supported:
/// '/small' - file fits in a single packet
/// '/large' - file requires multiple packets to be sent
/// In all cases file contents are one byte data length, followed by a single
/// byte repeating increasing sequence (0x00, 0x01, .. 0xFF) for specified
/// number of bytes.
void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request)
{
QGCUASFileManager::Request response;
QString path;
// Make sure one byte of length is enough to overflow into two packets.
Q_ASSERT((sizeof(request->data) & 0xFF) == sizeof(request->data));
path = (char *)&request->data[0];
if (path == smallFilename) {
_readFileLength = smallFileLength;
qDebug() << "Reading file length" << smallFileLength;
} else if (path == largeFilename) {
_readFileLength = largeFileLength;
qDebug() << "Reading file length" << largeFileLength;
} else {
_sendNak(QGCUASFileManager::kErrNotFile);
return;
}
response.hdr.magic = 'f';
response.hdr.opcode = QGCUASFileManager::kRspAck;
response.hdr.session = _sessionId;
response.hdr.size = 0;
_emitResponse(&response);
}
/// @brief Handles Read command requests.
void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request)
{
qDebug() << "Read command:" << request->hdr.offset;
QGCUASFileManager::Request response;
if (request->hdr.session != _sessionId) {
_sendNak(QGCUASFileManager::kErrNoSession);
return;
}
if (request->hdr.size > sizeof(response.data)) {
_sendNak(QGCUASFileManager::kErrPerm);
return;
}
uint8_t size = 0;
uint32_t offset = request->hdr.offset;
// Offset check is > instead of >= to take into accoutn extra length byte at beginning of file
if (offset > _readFileLength) {
_sendNak(QGCUASFileManager::kErrIO);
return;
}
// Write length byte if needed
if (offset == 0) {
response.data[0] = _readFileLength;
offset++;
size++;
}
// Write file bytes. Data is a repeating sequence of 0x00, 0x01, .. 0xFF.
for (; size < sizeof(response.data) && offset <= _readFileLength; offset++, size++) {
response.data[size] = (offset - 1) & 0xFF;
}
qDebug() << "_readCommand bytes written" << size;
// If we didn't write any bytes it was a bad request
if (size == 0) {
_sendNak(QGCUASFileManager::kErrEOF);
return;
}
response.hdr.magic = 'f';
response.hdr.opcode = QGCUASFileManager::kRspAck;
response.hdr.session = _sessionId;
response.hdr.size = size;
response.hdr.offset = request->hdr.offset;
_emitResponse(&response);
}
/// @brief Handles messages sent to the FTP server.
void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
{
QGCUASFileManager::Request ackResponse;
Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA);
mavlink_encapsulated_data_t requestEncapsulatedData; mavlink_encapsulated_data_t requestEncapsulatedData;
...@@ -59,57 +208,21 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) ...@@ -59,57 +208,21 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
break; break;
case QGCUASFileManager::kCmdList: case QGCUASFileManager::kCmdList:
// We only support root path _listCommand(request);
path = (char *)&request->data[0]; break;
if (!path.isEmpty() && path != "/") {
_sendNak(QGCUASFileManager::kErrNotDir);
break;
}
if (request->hdr.offset > (uint32_t)_fileList.size()) {
_sendNak(QGCUASFileManager::kErrEOF);
break;
}
ackResponse.hdr.magic = 'f';
ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
ackResponse.hdr.session = 0;
ackResponse.hdr.size = 0;
if (request->hdr.offset == 0) {
// Requesting first batch of file names
Q_ASSERT(_fileList.size());
char *bufPtr = (char *)&ackResponse.data[0];
for (int i=0; i<_fileList.size(); i++) {
const char *filename = _fileList[i].toStdString().c_str();
size_t cchFilename = strlen(filename);
strcpy(bufPtr, filename);
ackResponse.hdr.size += cchFilename + 1;
bufPtr += cchFilename + 1;
}
// Final double termination
*bufPtr = 0;
ackResponse.hdr.size++;
} else {
// All filenames fit in first ack, send final null terminated ack
ackResponse.data[0] = 0;
ackResponse.hdr.size = 1;
}
_emitResponse(&ackResponse); case QGCUASFileManager::kCmdOpen:
_openCommand(request);
break;
case QGCUASFileManager::kCmdRead:
_readCommand(request);
break; break;
// Remainder of commands are NYI // Remainder of commands are NYI
case QGCUASFileManager::kCmdTerminate: case QGCUASFileManager::kCmdTerminate:
// releases sessionID, closes file // releases sessionID, closes file
case QGCUASFileManager::kCmdOpen:
// opens <path> for reading, returns <session>
case QGCUASFileManager::kCmdRead:
// reads <size> bytes from <offset> in <session>
case QGCUASFileManager::kCmdCreate: case QGCUASFileManager::kCmdCreate:
// creates <path> for writing, returns <session> // creates <path> for writing, returns <session>
case QGCUASFileManager::kCmdWrite: case QGCUASFileManager::kCmdWrite:
...@@ -123,6 +236,7 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) ...@@ -123,6 +236,7 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
} }
} }
/// @brief Sends a Nak with the specified error code.
void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error)
{ {
QGCUASFileManager::Request nakResponse; QGCUASFileManager::Request nakResponse;
...@@ -136,6 +250,7 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) ...@@ -136,6 +250,7 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error)
_emitResponse(&nakResponse); _emitResponse(&nakResponse);
} }
/// @brief Emits a Request through the messageReceived signal.
void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request) void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request)
{ {
mavlink_message_t mavlinkMessage; mavlink_message_t mavlinkMessage;
......
...@@ -27,6 +27,11 @@ ...@@ -27,6 +27,11 @@
#include "MockMavlinkInterface.h" #include "MockMavlinkInterface.h"
#include "QGCUASFileManager.h" #include "QGCUASFileManager.h"
/// @file
/// @brief Mock implementation of Mavlink FTP server. Used as mavlink plugin to MockUAS.
/// Only root directory access is supported.
///
/// @author Don Gagne <don@thegagnes.com>
#include <QStringList> #include <QStringList>
...@@ -37,16 +42,29 @@ class MockMavlinkFileServer : public MockMavlinkInterface ...@@ -37,16 +42,29 @@ class MockMavlinkFileServer : public MockMavlinkInterface
public: public:
MockMavlinkFileServer(void) { }; MockMavlinkFileServer(void) { };
/// @brief Sets the list of files returned by the List command. Prepend names with F or D
/// to indicate (F)ile or (D)irectory.
void setFileList(QStringList& fileList) { _fileList = fileList; } void setFileList(QStringList& fileList) { _fileList = fileList; }
// From MockMavlinkInterface // From MockMavlinkInterface
virtual void sendMessage(mavlink_message_t message); virtual void sendMessage(mavlink_message_t message);
static const char* smallFilename;
static const char* largeFilename;
static const uint8_t smallFileLength;
static const uint8_t largeFileLength;
private: private:
void _sendNak(QGCUASFileManager::ErrorCode error); void _sendNak(QGCUASFileManager::ErrorCode error);
void _emitResponse(QGCUASFileManager::Request* request); void _emitResponse(QGCUASFileManager::Request* request);
void _listCommand(QGCUASFileManager::Request* request);
void _openCommand(QGCUASFileManager::Request* request);
void _readCommand(QGCUASFileManager::Request* request);
QStringList _fileList; ///< List of files returned by List command
QStringList _fileList; static const uint8_t _sessionId;
uint8_t _readFileLength; ///< Length of active file being read
}; };
#endif #endif
...@@ -24,7 +24,9 @@ ...@@ -24,7 +24,9 @@
#include "QGCUASFileManagerTest.h" #include "QGCUASFileManagerTest.h"
/// @file /// @file
/// @brief QGCUASFileManager unit test /// @brief QGCUASFileManager unit test. Note: All code here assumes all work between
/// the unit test, mack mavlink file server and file manager is happening on
/// the same thread.
/// ///
/// @author Don Gagne <don@thegagnes.com> /// @author Don Gagne <don@thegagnes.com>
...@@ -154,3 +156,63 @@ void QGCUASFileManagerUnitTest::_listTest(void) ...@@ -154,3 +156,63 @@ void QGCUASFileManagerUnitTest::_listTest(void)
QCOMPARE(_multiSpy->checkNoSignalByMask(errorMessageSignalMask), true); // We should not get an error signals QCOMPARE(_multiSpy->checkNoSignalByMask(errorMessageSignalMask), true); // We should not get an error signals
QVERIFY(_fileListReceived == fileListExpected); QVERIFY(_fileListReceived == fileListExpected);
} }
void QGCUASFileManagerUnitTest::_validateFileContents(const QString& filePath, uint8_t length)
{
QFile file(filePath);
QVERIFY(file.open(QIODevice::ReadOnly));
QByteArray bytes = file.readAll();
file.close();
QCOMPARE(bytes.length(), length + 1); // +1 for length byte
// Validate length byte
QCOMPARE((uint8_t)bytes[0], length);
// Validate file contents
for (uint8_t i=1; i<bytes.length(); i++) {
QCOMPARE((uint8_t)bytes[i], (uint8_t)((i-1) & 0xFF));
}
}
void QGCUASFileManagerUnitTest::_openTest(void)
{
Q_ASSERT(_fileManager);
Q_ASSERT(_multiSpy);
Q_ASSERT(_multiSpy->checkNoSignals() == true);
// Send a bogus path
// We should get a single resetStatusMessages signal
// We should get a single errorMessage signal
_fileManager->downloadPath("bogus", QDir::temp());
QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask | resetStatusMessagesSignalMask), true);
_multiSpy->clearAllSignals();
// Clean previous downloads
QString smallFilePath;
smallFilePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::smallFilename);
if (QFile::exists(smallFilePath)) {
Q_ASSERT(QFile::remove(smallFilePath));
}
QString largeFilePath;
largeFilePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::largeFilename);
if (QFile::exists(largeFilePath)) {
Q_ASSERT(QFile::remove(largeFilePath));
}
// Send a valid file to download
// We should get a single resetStatusMessages signal
// We should get a single statusMessage signal, which indicated download completion
_fileManager->downloadPath(MockMavlinkFileServer::smallFilename, QDir::temp());
QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true);
_multiSpy->clearAllSignals();
_validateFileContents(smallFilePath, MockMavlinkFileServer::smallFileLength);
_fileManager->downloadPath(MockMavlinkFileServer::largeFilename, QDir::temp());
QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true);
_multiSpy->clearAllSignals();
_validateFileContents(largeFilePath, MockMavlinkFileServer::largeFileLength);
}
...@@ -56,11 +56,14 @@ private slots: ...@@ -56,11 +56,14 @@ private slots:
void _noAckTest(void); void _noAckTest(void);
void _resetTest(void); void _resetTest(void);
void _listTest(void); void _listTest(void);
void _openTest(void);
// Connected to QGCUASFileManager statusMessage signal // Connected to QGCUASFileManager statusMessage signal
void statusMessage(const QString&); void statusMessage(const QString&);
private: private:
void _validateFileContents(const QString& filePath, uint8_t length);
enum { enum {
statusMessageSignalIndex = 0, statusMessageSignalIndex = 0,
errorMessageSignalIndex, errorMessageSignalIndex,
......
...@@ -69,7 +69,8 @@ QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : ...@@ -69,7 +69,8 @@ QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) :
QObject(parent), QObject(parent),
_currentOperation(kCOIdle), _currentOperation(kCOIdle),
_mav(uas), _mav(uas),
_encdata_seq(0) _encdata_seq(0),
_activeSession(0)
{ {
bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout())); bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout()));
Q_ASSERT(connected); Q_ASSERT(connected);
...@@ -103,6 +104,81 @@ void QGCUASFileManager::nothingMessage() ...@@ -103,6 +104,81 @@ void QGCUASFileManager::nothingMessage()
// FIXME: Connect ui correctly // FIXME: Connect ui correctly
} }
/// @brief Respond to the Ack associated with the Open command.
void QGCUASFileManager::_openResponse(Request* openAck)
{
_currentOperation = kCORead;
_activeSession = openAck->hdr.session;
_readOffset = 0;
_readFileAccumulator.clear();
Request request;
request.hdr.magic = 'f';
request.hdr.session = _activeSession;
request.hdr.opcode = kCmdRead;
request.hdr.offset = _readOffset;
request.hdr.size = sizeof(request.data);
_sendRequest(&request);
}
/// @brief Respond to the Ack associated with the Read command.
void QGCUASFileManager::_readResponse(Request* readAck)
{
if (readAck->hdr.session != _activeSession) {
_currentOperation = kCOIdle;
_readFileAccumulator.clear();
_emitErrorMessage(tr("Read: Incorrect session returned"));
return;
}
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;
}
qDebug() << "Accumulator size" << readAck->hdr.size;
_readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size);
if (readAck->hdr.size == sizeof(readAck->data)) {
// Still more data to read
_currentOperation = kCORead;
_readOffset += sizeof(readAck->data);
Request request;
request.hdr.magic = 'f';
request.hdr.session = _activeSession;
request.hdr.opcode = kCmdRead;
request.hdr.offset = _readOffset;
request.hdr.size = sizeof(request.data);
_sendRequest(&request);
} else {
_currentOperation = kCOIdle;
QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename);
QFile file(downloadFilePath);
if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
_emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath));
return;
}
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();
_emitStatusMessage(tr("Download complete '%1'").arg(downloadFilePath));
}
}
void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message)
{ {
Q_UNUSED(link); Q_UNUSED(link);
...@@ -116,8 +192,6 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me ...@@ -116,8 +192,6 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me
mavlink_msg_encapsulated_data_decode(&message, &data); mavlink_msg_encapsulated_data_decode(&message, &data);
Request* request = (Request*)&data.data[0]; Request* request = (Request*)&data.data[0];
qDebug() << "FTP: opcode:" << request->hdr.opcode;
// FIXME: Check CRC // FIXME: Check CRC
if (request->hdr.opcode == kRspAck) { if (request->hdr.opcode == kRspAck) {
...@@ -132,14 +206,21 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me ...@@ -132,14 +206,21 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me
case kCOAck: case kCOAck:
// We are expecting an ack back // We are expecting an ack back
_clearAckTimeout();
_currentOperation = kCOIdle; _currentOperation = kCOIdle;
break; break;
case kCOList: case kCOList:
listDecode(&request->data[0], request->hdr.size); listDecode(&request->data[0], request->hdr.size);
break; break;
case kCOOpen:
_openResponse(request);
break;
case kCORead:
_readResponse(request);
break;
default: default:
_emitErrorMessage("Ack received in unexpected state"); _emitErrorMessage("Ack received in unexpected state");
break; break;
...@@ -196,7 +277,7 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) ...@@ -196,7 +277,7 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len)
} }
// put it in the view // put it in the view
emit statusMessage(s); _emitStatusMessage(s);
// account for the name + NUL // account for the name + NUL
offset += nlen + 1; offset += nlen + 1;
...@@ -217,6 +298,12 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) ...@@ -217,6 +298,12 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len)
} }
} }
void QGCUASFileManager::_fillRequestWithString(Request* request, const QString& str)
{
strncpy((char *)&request->data[0], str.toStdString().c_str(), sizeof(request->data));
request->hdr.size = strnlen((const char *)&request->data[0], sizeof(request->data));
}
void QGCUASFileManager::sendList() void QGCUASFileManager::sendList()
{ {
Request request; Request request;
...@@ -226,32 +313,44 @@ void QGCUASFileManager::sendList() ...@@ -226,32 +313,44 @@ void QGCUASFileManager::sendList()
request.hdr.opcode = kCmdList; request.hdr.opcode = kCmdList;
request.hdr.offset = _listOffset; request.hdr.offset = _listOffset;
strncpy((char *)&request.data[0], _listPath.toStdString().c_str(), sizeof(request.data)); _fillRequestWithString(&request, _listPath);
request.hdr.size = strnlen((const char *)&request.data[0], sizeof(request.data));
_sendRequest(&request); _sendRequest(&request);
} }
void QGCUASFileManager::downloadPath(const QString &from, const QString &to) /// @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)
{ {
Q_UNUSED(from); if (from.isEmpty()) {
return;
}
// Send path, e.g. /fs/microsd and download content _readFileDownloadDir.setPath(downloadDir.absolutePath());
// recursively into a local directory
// We need to strip off the file name from the fully qualified path. We can't use the usual QDir
char buf[255]; // routines because this path does not exist locally.
unsigned len = 10; int i;
for (i=from.size()-1; i>=0; i--) {
QByteArray data(buf, len); if (from[i] == '/') {
QString filename = "log001.bin"; // XXX get this from onboard break;
}
// Qt takes care of slash conversions in paths }
QFile file(to + QDir::separator() + filename); i++; // move past slash
file.open(QIODevice::WriteOnly); _readFileDownloadFilename = from.right(from.size() - i);
file.write(data);
file.close(); emit resetStatusMessages();
_currentOperation = kCOOpen;
emit statusMessage(QString("Downloaded: %1 to directory %2").arg(filename).arg(to)); Request request;
request.hdr.magic = 'f';
request.hdr.session = 0;
request.hdr.opcode = kCmdOpen;
request.hdr.offset = 0;
_fillRequestWithString(&request, from);
_sendRequest(&request);
} }
QString QGCUASFileManager::errorString(uint8_t errorCode) QString QGCUASFileManager::errorString(uint8_t errorCode)
...@@ -307,7 +406,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS ...@@ -307,7 +406,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS
request.hdr.size = 0; request.hdr.size = 0;
_currentOperation = newOpState; _currentOperation = newOpState;
_setupAckTimeout();
_sendRequest(&request); _sendRequest(&request);
...@@ -317,8 +415,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS ...@@ -317,8 +415,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS
/// @brief Starts the ack timeout timer /// @brief Starts the ack timeout timer
void QGCUASFileManager::_setupAckTimeout(void) void QGCUASFileManager::_setupAckTimeout(void)
{ {
qDebug() << "Setting Ack";
Q_ASSERT(!_ackTimer.isActive()); Q_ASSERT(!_ackTimer.isActive());
_ackTimer.setSingleShot(true); _ackTimer.setSingleShot(true);
...@@ -328,8 +424,6 @@ void QGCUASFileManager::_setupAckTimeout(void) ...@@ -328,8 +424,6 @@ void QGCUASFileManager::_setupAckTimeout(void)
/// @brief Clears the ack timeout timer /// @brief Clears the ack timeout timer
void QGCUASFileManager::_clearAckTimeout(void) void QGCUASFileManager::_clearAckTimeout(void)
{ {
qDebug() << "Clearing Ack";
Q_ASSERT(_ackTimer.isActive()); Q_ASSERT(_ackTimer.isActive());
_ackTimer.stop(); _ackTimer.stop();
...@@ -344,10 +438,16 @@ void QGCUASFileManager::_ackTimeout(void) ...@@ -344,10 +438,16 @@ void QGCUASFileManager::_ackTimeout(void)
void QGCUASFileManager::_emitErrorMessage(const QString& msg) void QGCUASFileManager::_emitErrorMessage(const QString& msg)
{ {
qDebug() << "QGCUASFileManager:" << msg; qDebug() << "QGCUASFileManager: Error" << msg;
emit errorMessage(msg); emit errorMessage(msg);
} }
void QGCUASFileManager::_emitStatusMessage(const QString& msg)
{
qDebug() << "QGCUASFileManager: Status" << msg;
emit statusMessage(msg);
}
/// @brief Sends the specified Request out to the UAS. /// @brief Sends the specified Request out to the UAS.
void QGCUASFileManager::_sendRequest(Request* request) void QGCUASFileManager::_sendRequest(Request* request)
{ {
......
...@@ -25,6 +25,7 @@ ...@@ -25,6 +25,7 @@
#define QGCUASFILEMANAGER_H #define QGCUASFILEMANAGER_H
#include <QObject> #include <QObject>
#include <QDir>
#include "UASInterface.h" #include "UASInterface.h"
...@@ -48,7 +49,7 @@ public slots: ...@@ -48,7 +49,7 @@ public slots:
void receiveMessage(LinkInterface* link, mavlink_message_t message); void receiveMessage(LinkInterface* link, mavlink_message_t message);
void nothingMessage(); void nothingMessage();
void listRecursively(const QString &from); void listRecursively(const QString &from);
void downloadPath(const QString& from, const QString& to); void downloadPath(const QString& from, const QDir& downloadDir);
protected: protected:
struct RequestHeader struct RequestHeader
...@@ -109,7 +110,9 @@ protected: ...@@ -109,7 +110,9 @@ protected:
{ {
kCOIdle, // not doing anything kCOIdle, // not doing anything
kCOAck, // waiting for an Ack kCOAck, // waiting for an Ack
kCOList, // waiting for a List response kCOList, // waiting for List response
kCOOpen, // waiting for Open response
kCORead, // waiting for Read response
}; };
...@@ -121,7 +124,11 @@ protected: ...@@ -121,7 +124,11 @@ protected:
void _setupAckTimeout(void); void _setupAckTimeout(void);
void _clearAckTimeout(void); void _clearAckTimeout(void);
void _emitErrorMessage(const QString& msg); void _emitErrorMessage(const QString& msg);
void _emitStatusMessage(const QString& msg);
void _sendRequest(Request* request); void _sendRequest(Request* request);
void _fillRequestWithString(Request* request, const QString& str);
void _openResponse(Request* openAck);
void _readResponse(Request* readAck);
void sendList(); void sendList();
void listDecode(const uint8_t *data, unsigned len); void listDecode(const uint8_t *data, unsigned len);
...@@ -136,8 +143,14 @@ protected: ...@@ -136,8 +143,14 @@ protected:
UASInterface* _mav; UASInterface* _mav;
quint16 _encdata_seq; quint16 _encdata_seq;
unsigned _listOffset; // offset for the current List operation unsigned _listOffset; ///> offset for the current List operation
QString _listPath; // path for the current List operation QString _listPath; ///> path for the current List operation
uint8_t _activeSession; ///> currently active session, 0 for none
uint32_t _readOffset; ///> current read offset
QByteArray _readFileAccumulator; ///> Holds file being downloaded
QDir _readFileDownloadDir; ///> Directory to download file to
QString _readFileDownloadFilename; ///> Filename (no path) for download file
// We give MockMavlinkFileServer friend access so that it can use the data structures and opcodes // We give MockMavlinkFileServer friend access so that it can use the data structures and opcodes
// to build a mock mavlink file server for testing. // to build a mock mavlink file server for testing.
......
...@@ -33,10 +33,10 @@ void QGCUASFileView::listFiles() ...@@ -33,10 +33,10 @@ void QGCUASFileView::listFiles()
void QGCUASFileView::downloadFiles() void QGCUASFileView::downloadFiles()
{ {
QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"), QString dir = QFileDialog::getExistingDirectory(this, tr("Download Directory"),
QDir::homePath(), QDir::homePath(),
QFileDialog::ShowDirsOnly QFileDialog::ShowDirsOnly
| QFileDialog::DontResolveSymlinks); | QFileDialog::DontResolveSymlinks);
// And now download to this location // And now download to this location
_manager->downloadPath(ui->pathLineEdit->text(), dir); _manager->downloadPath(ui->pathLineEdit->text(), QDir(dir));
} }
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment