From e2ce56dc05c33a3cee8e0f8cd103636597952823 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 25 Jun 2014 17:14:35 -0700 Subject: [PATCH] List command is now fully working Also changes for Ack timeouts and unit testing --- src/uas/QGCUASFileManager.cc | 269 ++++++++++++++++++++++++----------- src/uas/QGCUASFileManager.h | 105 ++++++++++---- 2 files changed, 263 insertions(+), 111 deletions(-) diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 64689adaf..5b48b3059 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -1,3 +1,26 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + #include "QGCUASFileManager.h" #include "QGC.h" #include "MAVLinkProtocol.h" @@ -44,84 +67,106 @@ static const quint32 crctab[] = QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : QObject(parent), + _currentOperation(kCOIdle), _mav(uas), _encdata_seq(0) { + bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout())); + Q_ASSERT(connected); } -quint32 -QGCUASFileManager::crc32(const uint8_t *src, unsigned len, unsigned state) +/// @brief Calculates a 32 bit CRC for the specified request. +/// @param request Request to calculate CRC for. request->size must be set correctly. +/// @param state previous crc state +/// @return Calculated CRC +quint32 QGCUASFileManager::crc32(Request* request, unsigned state) { - for (unsigned i = 0; i < len; i++) - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); + uint8_t* data = (uint8_t*)request; + size_t cbData = sizeof(RequestHeader) + request->hdr.size; + + // Always calculate CRC with 0 initial CRC value + quint32 crcSave = request->hdr.crc32; + request->hdr.crc32 = 0; + + for (size_t i=0; i < cbData; i++) + state = crctab[(state ^ data[i]) & 0xff] ^ (state >> 8); + + request->hdr.crc32 = crcSave; + return state; } void QGCUASFileManager::nothingMessage() { - mavlink_message_t message; - - RequestHeader hdr; - hdr.magic = 'f'; - hdr.session = 0; - hdr.crc32 = 0; - hdr.size = 0; - hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); - - _mav->sendMessage(message); - qDebug() << "Sent message!"; + // FIXME: Connect ui correctly } void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) { Q_UNUSED(link); - + if (message.msgid != MAVLINK_MSG_ID_ENCAPSULATED_DATA) { // wtf, not for us return; } - + mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&message, &data); - const RequestHeader *hdr = (const RequestHeader *)&data.data[0]; - - // XXX VALIDATE MESSAGE - - switch (_current_operation) { - case kCOIdle: - // we should not be seeing anything here.. shut the other guy up - qDebug() << "FTP resetting file transfer session"; - sendReset(); - break; - - case kCOList: - if (hdr->opcode == kRspAck) { - listDecode(&hdr->data[0], hdr->size); - } else if (hdr->opcode == kRspNak) { - emit statusMessage(QString("error: ").append(errorString(hdr->data[0]))); - } - break; + Request* request = (Request*)&data.data[0]; - default: - emit statusMessage("message in unexpected state"); + qDebug() << "FTP: opcode:" << request->hdr.opcode; + + // FIXME: Check CRC + + if (request->hdr.opcode == kRspAck) { + _clearAckTimeout(); + + switch (_currentOperation) { + case kCOIdle: + // we should not be seeing anything here.. shut the other guy up + qDebug() << "FTP resetting file transfer session"; + _sendCmdReset(); + break; + + case kCOAck: + // We are expecting an ack back + _clearAckTimeout(); + _currentOperation = kCOIdle; + break; + + case kCOList: + listDecode(&request->data[0], request->hdr.size); + break; + + default: + _emitErrorMessage("Ack received in unexpected state"); + break; + } + } else if (request->hdr.opcode == kRspNak) { + _clearAckTimeout(); + _emitErrorMessage(QString("Nak received, error: ").append(errorString(request->data[0]))); + _currentOperation = kCOIdle; + } else { + // Note that we don't change our operation state. If something goes wrong beyond this, the operation + // will time out. + _emitErrorMessage(tr("Unknown opcode returned server: %1").arg(request->hdr.opcode)); } } void QGCUASFileManager::listRecursively(const QString &from) { - if (_current_operation != kCOIdle) { - // XXX beep and don't do anything + if (_currentOperation != kCOIdle) { + _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); + return; } // clear the text widget emit resetStatusMessages(); // initialise the lister - _list_path = from; - _list_offset = 0; - _current_operation = kCOList; + _listPath = from; + _listOffset = 0; + _currentOperation = kCOList; // and send the initial request sendList(); @@ -134,15 +179,17 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) // parse filenames out of the buffer while (offset < len) { - + const char * ptr = (const char *)data + offset; + // get the length of the name - unsigned nlen = strnlen((const char *)data + offset, len - offset); + unsigned nlen = strnlen(ptr, len - offset); if (nlen < 2) { break; } - QString s((const char *)data + offset + 1); - if (data[0] == 'D') { + // Returned names are prepended with D for directory or F for file + QString s(ptr + 1); + if (*ptr == 'D') { s.append('/'); } @@ -158,52 +205,29 @@ void QGCUASFileManager::listDecode(const uint8_t *data, unsigned len) // we have run out of files to list if (files == 0) { - _current_operation = kCOIdle; + _currentOperation = kCOIdle; } else { // update our state - _list_offset += files; + _listOffset += files; // and send another request sendList(); } } -void QGCUASFileManager::sendReset() -{ - mavlink_message_t message; - - RequestHeader hdr; - hdr.magic = 'f'; - hdr.session = 0; - hdr.opcode = kCmdReset; - hdr.crc32 = 0; - hdr.offset = 0; - hdr.size = 0; - hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length - - _mav->sendMessage(message); -} - void QGCUASFileManager::sendList() { - mavlink_message_t message; - - RequestHeader hdr; - hdr.magic = 'f'; - hdr.session = 0; - hdr.opcode = kCmdList; - hdr.crc32 = 0; - hdr.offset = _list_offset; - strncpy((char *)&hdr.data[0], _list_path.toStdString().c_str(), 200); // XXX should have a real limit here - hdr.size = strlen((const char *)&hdr.data[0]); - - hdr.crc32 = crc32((uint8_t*)&hdr, sizeof(hdr) + hdr.size, 0); - - mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)&hdr); // XXX 250 is a magic length + Request request; - _mav->sendMessage(message); + request.hdr.magic = 'f'; + request.hdr.session = 0; + request.hdr.opcode = kCmdList; + request.hdr.offset = _listOffset; + + strncpy((char *)&request.data[0], _listPath.toStdString().c_str(), sizeof(request.data)); + request.hdr.size = strnlen((const char *)&request.data[0], sizeof(request.data)); + + _sendRequest(&request); } void QGCUASFileManager::downloadPath(const QString &from, const QString &to) @@ -253,7 +277,84 @@ QString QGCUASFileManager::errorString(uint8_t errorCode) return QString("device I/O error"); case kErrPerm: return QString("permission denied"); + case kErrUnknownCommand: + return QString("unknown command"); + case kErrCrc: + return QString("bad crc"); default: - return QString("bad error code"); + return QString("unknown error code"); } } + +/// @brief Sends a command which only requires an opcode and no additional data +/// @param opcode Opcode to send +/// @param newOpState State to put state machine into +/// @return TRUE: command sent, FALSE: command not sent, waiting for previous command to finish +bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState) +{ + if (_currentOperation != kCOIdle) { + // Can't have multiple commands in play at the same time + return false; + } + + Request request; + request.hdr.magic = 'f'; + request.hdr.session = 0; + request.hdr.opcode = opcode; + request.hdr.offset = 0; + request.hdr.size = 0; + + _currentOperation = newOpState; + _setupAckTimeout(); + + _sendRequest(&request); + + return TRUE; +} + +/// @brief Starts the ack timeout timer +void QGCUASFileManager::_setupAckTimeout(void) +{ + qDebug() << "Setting Ack"; + + Q_ASSERT(!_ackTimer.isActive()); + + _ackTimer.setSingleShot(true); + _ackTimer.start(_ackTimerTimeoutMsecs); +} + +/// @brief Clears the ack timeout timer +void QGCUASFileManager::_clearAckTimeout(void) +{ + qDebug() << "Clearing Ack"; + + Q_ASSERT(_ackTimer.isActive()); + + _ackTimer.stop(); +} + +/// @brief Called when ack timeout timer fires +void QGCUASFileManager::_ackTimeout(void) +{ + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Timeout waiting for ack")); +} + +void QGCUASFileManager::_emitErrorMessage(const QString& msg) +{ + qDebug() << "QGCUASFileManager:" << msg; + emit errorMessage(msg); +} + +/// @brief Sends the specified Request out to the UAS. +void QGCUASFileManager::_sendRequest(Request* request) +{ + mavlink_message_t message; + + _setupAckTimeout(); + + request->hdr.crc32 = crc32(request); + // FIXME: Send correct system id instead of harcoded 250 + mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)request); + _mav->sendMessage(message); +} \ No newline at end of file diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 8f8c46205..5c41c0bd1 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -1,7 +1,31 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + #ifndef QGCUASFILEMANAGER_H #define QGCUASFILEMANAGER_H #include + #include "UASInterface.h" class QGCUASFileManager : public QObject @@ -9,17 +33,22 @@ class QGCUASFileManager : public QObject Q_OBJECT public: QGCUASFileManager(QObject* parent, UASInterface* uas); + + /// These methods are only used for testing purposes. + bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); }; + bool _sendCmdTestNoAck(void) { return _sendOpcodeOnlyCmd(kCmdTestNoAck, kCOAck); }; + bool _sendCmdReset(void) { return _sendOpcodeOnlyCmd(kCmdReset, kCOAck); }; signals: void statusMessage(const QString& msg); void resetStatusMessages(); + void errorMessage(const QString& ms); public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); void nothingMessage(); void listRecursively(const QString &from); void downloadPath(const QString& from, const QString& to); -// void timedOut(); protected: struct RequestHeader @@ -30,23 +59,32 @@ protected: uint8_t size; uint32_t crc32; uint32_t offset; - uint8_t data[]; }; + struct Request + { + struct RequestHeader hdr; + // The entire Request must fit into the data member of the mavlink_encapsulated_data_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)]; + }; + enum Opcode { - kCmdNone, // ignored, always acked + 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?) + 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 + kRspNak, + + kCmdTestNoAck, // ignored, ack not sent back, for testing only, should timeout waiting for ack }; enum ErrorCode @@ -61,36 +99,49 @@ protected: kErrNotAppend, kErrTooBig, kErrIO, - kErrPerm + kErrPerm, + kErrUnknownCommand, + kErrCrc }; enum OperationState { kCOIdle, // not doing anything - + kCOAck, // waiting for an Ack kCOList, // waiting for a List response }; - - OperationState _current_operation; - unsigned _retry_counter; - - UASInterface* _mav; - quint16 _encdata_seq; - - unsigned _session_id; // session ID for current session - unsigned _list_offset; // offset for the current List operation - QString _list_path; // path for the current List operation - - void sendTerminate(); - void sendReset(); + + +protected slots: + void _ackTimeout(void); + +protected: + bool _sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState); + void _setupAckTimeout(void); + void _clearAckTimeout(void); + void _emitErrorMessage(const QString& msg); + void _sendRequest(Request* request); void sendList(); void listDecode(const uint8_t *data, unsigned len); - static quint32 crc32(const uint8_t *src, unsigned len, unsigned state); + 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 + static const int _ackTimerTimeoutMsecs = 1000; ///> Timeout in msecs for ack timer + + UASInterface* _mav; + quint16 _encdata_seq; + + unsigned _listOffset; // offset for the current List operation + QString _listPath; // path for the current List operation + + // We give MockMavlinkFileServer friend access so that it can use the data structures and opcodes + // to build a mock mavlink file server for testing. + friend class MockMavlinkFileServer; }; #endif // QGCUASFILEMANAGER_H -- 2.22.0