From 23c77aefcb04cd2115486b328b6f463845603d08 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:13:02 -0700 Subject: [PATCH] Mock mavlink ftp server --- src/qgcunittest/MockMavlinkFileServer.cc | 141 ++++++++++++++++------- src/qgcunittest/MockMavlinkFileServer.h | 55 ++------- src/qgcunittest/MockMavlinkInterface.h | 1 + 3 files changed, 113 insertions(+), 84 deletions(-) diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index 2dc67a8c6..b17aa8989 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -25,63 +25,124 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) { + QGCUASFileManager::Request ackResponse; + QString path; + Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); - mavlink_encapsulated_data_t data; - mavlink_msg_encapsulated_data_decode(&message, &data); - const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; + mavlink_encapsulated_data_t requestEncapsulatedData; + mavlink_msg_encapsulated_data_decode(&message, &requestEncapsulatedData); + QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestEncapsulatedData.data[0]; - // FIXME: Check CRC + // Validate CRC + if (request->hdr.crc32 != QGCUASFileManager::crc32(request)) { + _sendNak(QGCUASFileManager::kErrCrc); + } - switch (hdr->opcode) { - case kCmdNone: + switch (request->hdr.opcode) { + case QGCUASFileManager::kCmdTestNoAck: + // ignored, ack not sent back, for testing only + break; + + case QGCUASFileManager::kCmdReset: + // terminates all sessions + // Fall through to send back Ack + + case QGCUASFileManager::kCmdNone: // ignored, always acked + ackResponse.hdr.magic = 'f'; + ackResponse.hdr.opcode = QGCUASFileManager::kRspAck; + ackResponse.hdr.session = 0; + ackResponse.hdr.crc32 = 0; + ackResponse.hdr.size = 0; + _emitResponse(&ackResponse); + break; + + case QGCUASFileManager::kCmdList: + // We only support root path + path = (char *)&request->data[0]; + 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; - RequestHeader ackHdr; - ackHdr.magic = 'f'; - ackHdr.opcode = kRspAck; - ackHdr.session = 0; - ackHdr.crc32 = 0; - ackHdr.size = 0; - // FIXME: Add CRC - //ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + 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; + } - mavlink_message_t ackMessage; - mavlink_msg_encapsulated_data_pack(250, 0, &ackMessage, 0 /*_encdata_seq*/, (uint8_t*)&ackHdr); - emit messageReceived(NULL, ackMessage); + _emitResponse(&ackResponse); + break; + + // Remainder of commands are NYI - case kCmdTerminate: + case QGCUASFileManager::kCmdTerminate: // releases sessionID, closes file - case kCmdReset: - // terminates all sessions - case kCmdList: - // list files in from - case kCmdOpen: + case QGCUASFileManager::kCmdOpen: // opens for reading, returns - case kCmdRead: + case QGCUASFileManager::kCmdRead: // reads bytes from in - case kCmdCreate: + case QGCUASFileManager::kCmdCreate: // creates for writing, returns - case kCmdWrite: + case QGCUASFileManager::kCmdWrite: // appends bytes at in - case kCmdRemove: + case QGCUASFileManager::kCmdRemove: // remove file (only if created by server?) default: // nack for all NYI opcodes - - RequestHeader nakHdr; - nakHdr.opcode = kRspNak; - nakHdr.magic = 'f'; - nakHdr.session = 0; - nakHdr.crc32 = 0; - nakHdr.size = 0; - // FIXME: Add CRC - //ackHdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_message_t nakMessage; - mavlink_msg_encapsulated_data_pack(250, 0, &nakMessage, 0 /*_encdata_seq*/, (uint8_t*)&nakHdr); - emit messageReceived(NULL, nakMessage); + _sendNak(QGCUASFileManager::kErrUnknownCommand); break; } } + +void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) +{ + QGCUASFileManager::Request nakResponse; + + nakResponse.hdr.magic = 'f'; + nakResponse.hdr.opcode = QGCUASFileManager::kRspNak; + nakResponse.hdr.session = 0; + nakResponse.hdr.size = sizeof(nakResponse.data[0]); + nakResponse.data[0] = error; + + _emitResponse(&nakResponse); +} + +void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request) +{ + mavlink_message_t mavlinkMessage; + + request->hdr.crc32 = QGCUASFileManager::crc32(request); + + mavlink_msg_encapsulated_data_pack(250, 0, &mavlinkMessage, 0 /*_encdata_seq*/, (uint8_t*)request); + + emit messageReceived(NULL, mavlinkMessage); +} \ No newline at end of file diff --git a/src/qgcunittest/MockMavlinkFileServer.h b/src/qgcunittest/MockMavlinkFileServer.h index bca48ffc1..0bccd3d4a 100644 --- a/src/qgcunittest/MockMavlinkFileServer.h +++ b/src/qgcunittest/MockMavlinkFileServer.h @@ -25,6 +25,10 @@ #define MOCKMAVLINKFILESERVER_H #include "MockMavlinkInterface.h" +#include "QGCUASFileManager.h" + + +#include class MockMavlinkFileServer : public MockMavlinkInterface { @@ -32,54 +36,17 @@ class MockMavlinkFileServer : public MockMavlinkInterface public: MockMavlinkFileServer(void) { }; - virtual void sendMessage(mavlink_message_t message); - -private: - // FIXME: These should be in a mavlink header somewhere shouldn't they? - struct RequestHeader - { - uint8_t magic; - uint8_t session; - uint8_t opcode; - uint8_t size; - uint32_t crc32; - uint32_t offset; - uint8_t data[]; - }; + void setFileList(QStringList& fileList) { _fileList = fileList; } - enum Opcode - { - kCmdNone, // ignored, always acked - kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) - - kRspAck, - kRspNak - }; + // From MockMavlinkInterface + virtual void sendMessage(mavlink_message_t message); - enum ErrorCode - { - kErrNone, - kErrNoRequest, - kErrNoSession, - kErrSequence, - kErrNotDir, - kErrNotFile, - kErrEOF, - kErrNotAppend, - kErrTooBig, - kErrIO, - kErrPerm - }; +private: + void _sendNak(QGCUASFileManager::ErrorCode error); + void _emitResponse(QGCUASFileManager::Request* request); - + QStringList _fileList; }; #endif diff --git a/src/qgcunittest/MockMavlinkInterface.h b/src/qgcunittest/MockMavlinkInterface.h index 12ef447e4..5aaab3d6a 100644 --- a/src/qgcunittest/MockMavlinkInterface.h +++ b/src/qgcunittest/MockMavlinkInterface.h @@ -37,6 +37,7 @@ public: virtual void sendMessage(mavlink_message_t message) = 0; signals: + // link argument will always be NULL void messageReceived(LinkInterface* link, mavlink_message_t message); }; -- 2.22.0