Skip to content

Q
qgroundcontrol
Commit 158bfc91 authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Merge pull request #849 from DonLakeFlyer/FTP 

Modified to use new FILE_TRANSFER_PROTOCOL message
parents 3e0ca9f6 85e81a84
Hide whitespace changes
Inline Side-by-side
Showing with 119 additions and 83 deletions
src/qgcunittest/MockMavlinkFileServer.cc
View file @ 158bfc91
......@@ -45,8 +45,10 @@ const MockMavlinkFileServer::FileTestCase MockMavlinkFileServer::rgFileTestCases
// We only support a single fixed session
const uint8_t MockMavlinkFileServer::_sessionId = 1;
MockMavlinkFileServer::MockMavlinkFileServer(void) :
_errMode(errModeNone)
MockMavlinkFileServer::MockMavlinkFileServer(uint8_t systemIdQGC, uint8_t systemIdServer) :
_errMode(errModeNone),
_systemIdServer(systemIdServer),
_systemIdQGC(systemIdQGC)
{
}
......@@ -74,7 +76,6 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request, ui
return;
}
ackResponse.hdr.magic = 'f';
ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
ackResponse.hdr.session = 0;
ackResponse.hdr.offset = request->hdr.offset;
......@@ -133,7 +134,6 @@ void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request, ui
return;
}
response.hdr.magic = 'f';
response.hdr.opcode = QGCUASFileManager::kRspAck;
response.hdr.session = _sessionId;
......@@ -183,7 +183,6 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request, ui
// We should always have written something, otherwise there is something wrong with the code above
Q_ASSERT(cDataBytes);
response.hdr.magic = 'f';
response.hdr.session = _sessionId;
response.hdr.size = cDataBytes;
response.hdr.offset = request->hdr.offset;
......@@ -214,15 +213,15 @@ 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_FILE_TRANSFER_PROTOCOL);
mavlink_encapsulated_data_t requestEncapsulatedData;
mavlink_msg_encapsulated_data_decode(&message, &requestEncapsulatedData);
QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestEncapsulatedData.data[0];
mavlink_file_transfer_protocol_t requestFileTransferProtocol;
mavlink_msg_file_transfer_protocol_decode(&message, &requestFileTransferProtocol);
QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestFileTransferProtocol.payload[0];
Q_ASSERT(request->hdr.magic == QGCUASFileManager::kProtocolMagic);
uint16_t incomingSeqNumber = requestEncapsulatedData.seqnr;
Q_ASSERT(requestFileTransferProtocol.target_system == _systemIdServer);
uint16_t incomingSeqNumber = request->hdr.seqNumber;
uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber);
if (_errMode == errModeNoResponse) {
......@@ -236,6 +235,7 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
// Validate CRC
if (request->hdr.crc32 != QGCUASFileManager::crc32(request)) {
qDebug() << "Bad CRC received - opcode:" << request->hdr.opcode << "expected:" << request->hdr.crc32 << "actual:" << QGCUASFileManager::crc32(request);
_sendNak(QGCUASFileManager::kErrCrc, outgoingSeqNumber);
}
......@@ -250,7 +250,6 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
case QGCUASFileManager::kCmdNone:
// ignored, always acked
ackResponse.hdr.magic = 'f';
ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
ackResponse.hdr.session = 0;
ackResponse.hdr.crc32 = 0;
......@@ -294,7 +293,6 @@ void MockMavlinkFileServer::_sendAck(uint16_t seqNumber)
{
QGCUASFileManager::Request ackResponse;
ackResponse.hdr.magic = 'f';
ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
ackResponse.hdr.session = 0;
ackResponse.hdr.size = 0;
......@@ -307,7 +305,6 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error, uint16_
{
QGCUASFileManager::Request nakResponse;
nakResponse.hdr.magic = 'f';
nakResponse.hdr.opcode = QGCUASFileManager::kRspNak;
nakResponse.hdr.session = 0;
nakResponse.hdr.size = 1;
......@@ -321,14 +318,21 @@ void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request, u
{
mavlink_message_t mavlinkMessage;
request->hdr.magic = QGCUASFileManager::kProtocolMagic;
request->hdr.seqNumber = seqNumber;
request->hdr.crc32 = QGCUASFileManager::crc32(request);
if (_errMode == errModeBadCRC) {
// Return a bad CRC
request->hdr.crc32++;
}
mavlink_msg_encapsulated_data_pack(250, 50, &mavlinkMessage, seqNumber, (uint8_t*)request);
mavlink_msg_file_transfer_protocol_pack(_systemIdServer, // System ID
0, // Component ID
&mavlinkMessage, // Mavlink Message to pack into
0, // Target network
_systemIdQGC, // QGC Target System ID
0, // Target component
(uint8_t*)request); // Payload
emit messageReceived(NULL, mavlinkMessage);
}
......
src/qgcunittest/MockMavlinkFileServer.h
View file @ 158bfc91
......@@ -40,7 +40,10 @@ class MockMavlinkFileServer : public MockMavlinkInterface
Q_OBJECT
public:
MockMavlinkFileServer(void);
/// @brief Constructor for MockMavlinkFileServer
/// @param System ID for QGroundControl App
/// @pqram System ID for this Server
MockMavlinkFileServer(uint8_t systemIdQGC, uint8_t systemIdServer);
/// @brief Sets the list of files returned by the List command. Prepend names with F or D
/// to indicate (F)ile or (D)irectory.
......@@ -103,6 +106,8 @@ private:
static const uint8_t _sessionId;
uint8_t _readFileLength; ///< Length of active file being read
ErrorMode_t _errMode; ///< Currently set error mode, as specified by setErrorMode
const uint8_t _systemIdServer; ///< System ID for server
const uint8_t _systemIdQGC; ///< QGC System ID
};
#endif
src/qgcunittest/QGCUASFileManagerTest.cc
View file @ 158bfc91
......@@ -31,6 +31,7 @@
/// @author Don Gagne <don@thegagnes.com>
QGCUASFileManagerUnitTest::QGCUASFileManagerUnitTest(void) :
_mockFileServer(_systemIdQGC, _systemIdServer),
_fileManager(NULL),
_multiSpy(NULL)
{
......@@ -40,6 +41,7 @@ QGCUASFileManagerUnitTest::QGCUASFileManagerUnitTest(void) :
// Called once before all test cases are run
void QGCUASFileManagerUnitTest::initTestCase(void)
{
_mockUAS.setMockSystemId(_systemIdServer);
_mockUAS.setMockMavlinkPlugin(&_mockFileServer);
}
......@@ -48,19 +50,16 @@ void QGCUASFileManagerUnitTest::init(void)
{
Q_ASSERT(_multiSpy == NULL);
_fileManager = new QGCUASFileManager(NULL, &_mockUAS);
_fileManager = new QGCUASFileManager(NULL, &_mockUAS, _systemIdQGC);
Q_CHECK_PTR(_fileManager);
// Reset any internal state back to normal
_mockFileServer.setErrorMode(MockMavlinkFileServer::errModeNone);
_fileListReceived.clear();
bool connected = connect(&_mockFileServer, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), _fileManager, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
Q_ASSERT(connected);
Q_UNUSED(connected); // Silent release build compiler warning
connect(&_mockFileServer, &MockMavlinkFileServer::messageReceived, _fileManager, &QGCUASFileManager::receiveMessage);
connected = connect(_fileManager, SIGNAL(listEntry(const QString&)), this, SLOT(listEntry(const QString&)));
Q_ASSERT(connected);
connect(_fileManager, &QGCUASFileManager::listEntry, this, &QGCUASFileManagerUnitTest::listEntry);
_rgSignals[listEntrySignalIndex] = SIGNAL(listEntry(const QString&));
_rgSignals[listCompleteSignalIndex] = SIGNAL(listComplete(void));
......
src/qgcunittest/QGCUASFileManagerTest.h
View file @ 158bfc91
......@@ -80,6 +80,10 @@ private:
downloadFileCompleteSignalMask = 1 << downloadFileCompleteSignalIndex,
errorMessageSignalMask = 1 << errorMessageSignalIndex,
};
static const uint8_t _systemIdQGC = 255;
static const uint8_t _systemIdServer = 128;
MockUAS _mockUAS;
MockMavlinkFileServer _mockFileServer;
......
src/uas/QGCUASFileManager.cc
View file @ 158bfc91
......@@ -24,6 +24,7 @@
#include "QGCUASFileManager.h"
#include "QGC.h"
#include "MAVLinkProtocol.h"
#include "MainWindow.h"
#include <QFile>
#include <QDir>
......@@ -66,16 +67,20 @@ static const quint32 crctab[] =
};
QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) :
QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas, uint8_t unitTestSystemIdQGC) :
QObject(parent),
_currentOperation(kCOIdle),
_mav(uas),
_lastOutgoingSeqNumber(0),
_activeSession(0)
_activeSession(0),
_systemIdQGC(unitTestSystemIdQGC)
{
bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout()));
Q_ASSERT(connected);
Q_UNUSED(connected); // Silence retail unused variable error
connect(&_ackTimer, &QTimer::timeout, this, &QGCUASFileManager::_ackTimeout);
_systemIdServer = _mav->getUASID();
// Make sure we don't have bad structure packing
Q_ASSERT(sizeof(RequestHeader) == 16);
}
/// @brief Calculates a 32 bit CRC for the specified request.
......@@ -90,6 +95,9 @@ quint32 QGCUASFileManager::crc32(Request* request, unsigned state)
// Always calculate CRC with 0 initial CRC value
quint32 crcSave = request->hdr.crc32;
request->hdr.crc32 = 0;
request->hdr.padding[0] = 0;
request->hdr.padding[1] = 0;
request->hdr.padding[2] = 0;
for (size_t i=0; i < cbData; i++)
state = crctab[(state ^ data[i]) & 0xff] ^ (state >> 8);
......@@ -182,6 +190,7 @@ void QGCUASFileManager::_readAckResponse(Request* readAck)
request.hdr.session = _activeSession;
request.hdr.opcode = kCmdRead;
request.hdr.offset = _readOffset;
request.hdr.size = 0;
_sendRequest(&request);
} else {
......@@ -252,28 +261,25 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me
{
Q_UNUSED(link);
if (message.msgid != MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
// wtf, not for us
return;
}
// XXX: hack to prevent files from videostream to interfere
// FIXME: magic number
if (message.compid != 50) {
// receiveMessage is signalled will all mavlink messages so we need to filter everything else out but ours.
if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
return;
}
mavlink_encapsulated_data_t data;
mavlink_msg_encapsulated_data_decode(&message, &data);
Request* request = (Request*)&data.data[0];
if (request->hdr.magic != kProtocolMagic) {
mavlink_file_transfer_protocol_t data;
mavlink_msg_file_transfer_protocol_decode(&message, &data);
// Make sure we are the target system
if (data.target_system != _systemIdQGC) {
qDebug() << "Received MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL with possibly incorrect system id" << _systemIdQGC;
return;
}
Request* request = (Request*)&data.payload[0];
_clearAckTimeout();
uint16_t incomingSeqNumber = data.seqnr;
uint16_t incomingSeqNumber = request->hdr.seqNumber;
// Make sure we have a good CRC
quint32 expectedCRC = crc32(request);
......@@ -381,10 +387,10 @@ void QGCUASFileManager::_sendListCommand(void)
{
Request request;
request.hdr.magic = 'f';
request.hdr.session = 0;
request.hdr.opcode = kCmdList;
request.hdr.offset = _listOffset;
request.hdr.size = 0;
_fillRequestWithString(&request, _listPath);
......@@ -416,10 +422,10 @@ void QGCUASFileManager::downloadPath(const QString& from, const QDir& downloadDi
_currentOperation = kCOOpen;
Request request;
request.hdr.magic = 'f';
request.hdr.session = 0;
request.hdr.opcode = kCmdOpen;
request.hdr.offset = 0;
request.hdr.size = 0;
_fillRequestWithString(&request, from);
_sendRequest(&request);
}
......@@ -470,7 +476,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS
}
Request request;
request.hdr.magic = 'f';
request.hdr.session = 0;
request.hdr.opcode = opcode;
request.hdr.offset = 0;
......@@ -523,6 +528,7 @@ void QGCUASFileManager::_sendTerminateCommand(void)
Request request;
request.hdr.session = _activeSession;
request.hdr.opcode = kCmdTerminate;
request.hdr.size = 0;
_sendRequest(&request);
}
......@@ -547,10 +553,22 @@ void QGCUASFileManager::_sendRequest(Request* request)
_lastOutgoingSeqNumber++;
request->hdr.magic = kProtocolMagic;
request->hdr.seqNumber = _lastOutgoingSeqNumber;
request->hdr.crc32 = crc32(request);
// FIXME: Send correct system id instead of harcoded 250
// FIXME: What about the component id? Should it be set to something specific.
mavlink_msg_encapsulated_data_pack(250, 0, &message, _lastOutgoingSeqNumber, (uint8_t*)request);
if (_systemIdQGC == 0) {
_systemIdQGC = MainWindow::instance()->getMAVLink()->getSystemId();
}
Q_ASSERT(_mav);
mavlink_msg_file_transfer_protocol_pack(_systemIdQGC, // QGC System ID
0, // QGC Component ID
&message, // Mavlink Message to pack into
0, // Target network
_systemIdServer, // Target system
0, // Target component
(uint8_t*)request); // Payload
_mav->sendMessage(message);
}
src/uas/QGCUASFileManager.h
View file @ 158bfc91
......@@ -33,7 +33,7 @@ class QGCUASFileManager : public QObject
{
Q_OBJECT
public:
QGCUASFileManager(QObject* parent, UASInterface* uas);
QGCUASFileManager(QObject* parent, UASInterface* uas, uint8_t unitTestSystemIdQGC = 0);
/// These methods are only used for testing purposes.
bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); };
......@@ -74,26 +74,29 @@ public slots:
void downloadPath(const QString& from, const QDir& downloadDir);
protected:
static const uint8_t kProtocolMagic = 'f';
/// @brief This is the fixed length portion of the protocol data. Trying to pack structures across differing compilers is
/// questionable, so we pad the structure ourselves to 32 bit alignment which should get us what we want.
struct RequestHeader
{
uint8_t magic; ///> Magic byte 'f' to idenitfy FTP protocol
uint8_t session; ///> Session id for read and write commands
uint8_t opcode; ///> Command opcode
uint8_t size; ///> Size of data
uint32_t crc32; ///> CRC for entire Request structure, with crc32 set to 0
uint32_t offset; ///> Offsets for List and Read commands
uint16_t seqNumber; ///< sequence number for message
uint8_t session; ///< Session id for read and write commands
uint8_t opcode; ///< Command opcode
uint8_t size; ///< Size of data
uint8_t padding[3];
uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0
uint32_t offset; ///< Offsets for List and Read commands
};
struct Request
{
struct RequestHeader hdr;
// We use a union here instead of just casting (uint32_t)&data[0] to not break strict aliasing rules
// We use a union here instead of just casting (uint32_t)&payload[0] to not break strict aliasing rules
union {
// The entire Request must fit into the data member of the mavlink_encapsulated_data_t structure. We use as many leftover bytes
// The entire Request must fit into the payload member of the mavlink_file_transfer_protocol_t structure. We use as many leftover bytes
// after we use up space for the RequestHeader for the data portion of the Request.
uint8_t data[sizeof(((mavlink_encapsulated_data_t*)0)->data) - sizeof(RequestHeader)];
uint8_t data[sizeof(((mavlink_file_transfer_protocol_t*)0)->payload) - sizeof(RequestHeader)];
// File length returned by Open command
uint32_t openFileLength;
......@@ -103,22 +106,22 @@ protected:
enum Opcode
{
// Commands
kCmdNone, ///> ignored, always acked
kCmdTerminate, ///> releases sessionID, closes file
kCmdReset, ///> terminates all sessions
kCmdList, ///> list files in <path> from <offset>
kCmdOpen, ///> opens <path> for reading, returns <session>
kCmdRead, ///> reads <size> bytes from <offset> in <session>
kCmdCreate, ///> creates <path> for writing, returns <session>
kCmdWrite, ///> appends <size> bytes at <offset> in <session>
kCmdRemove, ///> remove file (only if created by server?)
kCmdNone, ///< ignored, always acked
kCmdTerminate, ///< releases sessionID, closes file
kCmdReset, ///< terminates all sessions
kCmdList, ///< list files in <path> from <offset>
kCmdOpen, ///< opens <path> for reading, returns <session>
kCmdRead, ///< reads <size> bytes from <offset> in <session>
kCmdCreate, ///< creates <path> for writing, returns <session>
kCmdWrite, ///< appends <size> bytes at <offset> in <session>
kCmdRemove, ///< remove file (only if created by server?)
// Responses
kRspAck, ///> positive acknowledgement of previous command
kRspNak, ///> negative acknowledgement of previous command
kRspAck, ///< positive acknowledgement of previous command
kRspNak, ///< negative acknowledgement of previous command
// Used for testing only, not part of protocol
kCmdTestNoAck, // ignored, ack not sent back, should timeout waiting for ack
kCmdTestNoAck, ///< ignored, ack not sent back, should timeout waiting for ack
};
enum ErrorCode
......@@ -170,21 +173,24 @@ protected:
static quint32 crc32(Request* request, unsigned state = 0);
static QString errorString(uint8_t errorCode);
OperationState _currentOperation; ///> Current operation of state machine
QTimer _ackTimer; ///> Used to signal a timeout waiting for an ack
OperationState _currentOperation; ///< Current operation of state machine
QTimer _ackTimer; ///< Used to signal a timeout waiting for an ack
UASInterface* _mav;
uint16_t _lastOutgoingSeqNumber; ///< Sequence number sent in last outgoing packet
unsigned _listOffset; ///> offset for the current List operation
QString _listPath; ///> path for the current List operation
unsigned _listOffset; ///< offset 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
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
uint8_t _systemIdQGC; ///< System ID for QGC
uint8_t _systemIdServer; ///< System ID for server
// We give MockMavlinkFileServer friend access so that it can use the data structures and opcodes
// to build a mock mavlink file server for testing.
......
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