List command is now fully working

Also changes for Ack timeouts and unit testing

src/uas/QGCUASFileManager.cc

@@ -1,3 +1,26 @@
+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ 
+ 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 <http://www.gnu.org/licenses/>.
+ 
+ ======================================================================*/
+
 #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);
+}

src/uas/QGCUASFileManager.h

@@ -1,7 +1,31 @@
+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ 
+ 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 <http://www.gnu.org/licenses/>.
+ 
+ ======================================================================*/
+
 #ifndef QGCUASFILEMANAGER_H
 #define QGCUASFILEMANAGER_H
 
 #include <QObject>
+
 #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 <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?)
+            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?)
 
             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