OP-15 Threaded version of USB QIODevice, not tested yet...

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@410 ebee16cc-31ac-478f-84a7-5cbb03baadba

ground/src/plugins/rawhid/rawhid.cpp

@@ -29,27 +29,237 @@
 
 #include "rawhid_const.h"
 
+#include <QtGlobal>
+#include <QMutexLocker>
+#include <QWaitCondition>
+
 //timeout value used when we want to return directly without waiting
-static const int IMMEDIATE_READ_TIMEOUT = 10;
-static const int IMMEDIATE_WRITE_TIMEOUT = 50;
+static const int READ_TIMEOUT = 200;
+static const int READ_SIZE = 64;
 
-#if 0
-static const int MAX_RX_LENGTH  = 63;
-static const int MAX_TX_LENGTH  = 63;
+static const int WRITE_TIMEOUT = 200;
+static const int WRITE_SIZE = 64;
 
-static const int READ_TIMEOUT   = 200;
-static const int WRITE_TIMEOUT  = 100;
-#endif
 
-RawHID::RawHID()
-    :QIODevice()
+
+
+/**
+*   Thread to desynchronize reading from the device
+*/
+class RawHIDReadThread : public QThread
 {
+public:
+    RawHIDReadThread(RawHID *hid);
+    virtual ~RawHIDReadThread();
+
+    /** Return the data read so far without waiting */
+    int getReadData(char *data, int size);
+
+    /** return the bytes buffered */
+    qint64 getBytesAvailable();
+
+protected:
+    void run();
+
+    /** QByteArray might not be the most efficient way to implement
+    a circular buffer but it's good enough and very simple */
+    QByteArray m_readBuffer;
+
+    /** A mutex to protect read buffer */
+    QMutex m_readBufMtx;
+
+    RawHID *m_hid;
+
+    pjrc_rawhid *hiddev;
+    int hidno;
+
+    bool m_running;
+};
+
+
+/**
+*  This class is nearly the same than RawHIDReadThread but for writing
+*/
+class RawHIDWriteThread : public QThread
+{
+public:
+    RawHIDWriteThread(RawHID *hid);
+    virtual ~RawHIDWriteThread();
+
+    /** Add some data to be written without waiting */
+    int pushDataToWrite(const char *data, int size);
+
+    /** Return the number of bytes buffered */
+    qint64 getBytesToWrite();
+
+protected:
+    void run();
+
+    /** QByteArray might not be the most efficient way to implement
+    a circular buffer but it's good enough and very simple */
+    QByteArray m_writeBuffer;
+
+    /** A mutex to protect read buffer */
+    QMutex m_writeBufMtx;
+
+    /** Synchronize task with data arival */
+    QWaitCondition m_newDataToWrite;
+
+    RawHID *m_hid;
+
+    pjrc_rawhid *hiddev;
+    int hidno;
+
+    bool m_running;
+};
+
+
+
+RawHIDReadThread::RawHIDReadThread(RawHID *hid)
+    : m_hid(hid),
+    hiddev(&hid->dev),
+    hidno(hid->m_deviceNo),
+    m_running(true)
+{
+}
+
+RawHIDReadThread::~RawHIDReadThread()
+{
+    m_running = false;
+    //wait for the thread to terminate
+    if(wait(1000) == false)
+        qDebug() << "Cannot terminate RawHIDReadThread";
+}
+
+void RawHIDReadThread::run()
+{
+    while(m_running)
+    {
+        //here we use a temporary buffer so we don't need to lock
+        //the mutex while we are reading from the device
+        char buffer[READ_SIZE] = {0};
+        int ret = hiddev->receive(hidno, buffer, READ_SIZE, READ_TIMEOUT);
+
+        if(ret > 0) //read some data
+        {
+            m_readBufMtx.lock();
+            m_readBuffer.append(buffer, ret);
+            m_readBufMtx.unlock();
+
+            emit m_hid->readyRead();
+        }
+        else if(ret == 0) //nothing read
+        {
+        }
+        else // < 0 => error
+        {
+        }
+    }
+}
+
+int RawHIDReadThread::getReadData(char *data, int size)
+{
+    QMutexLocker lock(&m_readBufMtx);
+
+    size = qMin(size, m_readBuffer.size());
+
+    memcpy(data, m_readBuffer.constData(), size);
+    m_readBuffer.remove(0, size);
+
+    return size;
+}
+
+qint64 RawHIDReadThread::getBytesAvailable()
+{
+    QMutexLocker lock(&m_readBufMtx);
+    return m_readBuffer.size();
+}
+
+RawHIDWriteThread::RawHIDWriteThread(RawHID *hid)
+    : m_hid(hid),
+    hiddev(&hid->dev),
+    hidno(hid->m_deviceNo),
+    m_running(true)
+{
+}
+
+RawHIDWriteThread::~RawHIDWriteThread()
+{
+    m_running = false;
+    //wait for the thread to terminate
+    if(wait(1000) == false)
+        qDebug() << "Cannot terminate RawHIDReadThread";
+}
+
+void RawHIDWriteThread::run()
+{
+    qDebug() << "Write thread started";
+    while(m_running)
+    {
+        char buffer[WRITE_SIZE] = {0};
+        m_writeBufMtx.lock();
+        int size = qMin(WRITE_SIZE, m_writeBuffer.size());
+        while(size <= 0)
+        {
+            //wait on new data to write condition, the timeout
+            //enable the thread to shutdown properly
+            m_newDataToWrite.wait(&m_writeBufMtx, 200);
+            if(!m_running)
+                return;
+            else
+                size = qMin(WRITE_SIZE, m_writeBuffer.size());
+        }
+
+        //make a temporary copy so we don't need to lock the mutex
+        //during actual device access
+        memcpy(buffer, m_writeBuffer.constData(), size);
+        m_writeBuffer.remove(0, size);
+        m_writeBufMtx.unlock();
+
+        qDebug() << "Data to write";
+
+        int ret = hiddev->send(hidno, buffer, size, WRITE_TIMEOUT);
+
+        if(ret > 0)
+        {
+            //only remove the size actually written to the device
+            m_writeBufMtx.lock();
+            m_writeBuffer.remove(0, ret);
+            m_writeBufMtx.unlock();
+
+            emit m_hid->bytesWritten(ret);
+        }
+        else if(ret < 0) // < 0 => error
+        {
+        }
+        else
+        {
+            qDebug() << "No data written to device ??";
+        }
+    }
+}
+
+int RawHIDWriteThread::pushDataToWrite(const char *data, int size)
+{
+    QMutexLocker lock(&m_writeBufMtx);
+
+    m_writeBuffer.append(data, size);
+    m_newDataToWrite.wakeOne(); //signal that new data arrived
+    return size;
+}
+
+qint64 RawHIDWriteThread::getBytesToWrite()
+{
+    QMutexLocker lock(&m_writeBufMtx);
+    return m_writeBuffer.size();
 }
 
 RawHID::RawHID(const QString &deviceName)
     :QIODevice(),
     serialNumber(deviceName),
-    m_deviceNo(-1)
+    m_deviceNo(-1),
+    m_readThread(NULL),
+    m_writeThread(NULL)
 {
     //find the device the user want to open and close the other
     int opened = dev.open(MAX_DEVICES, VID, PID, USAGE_PAGE, USAGE);
@@ -65,18 +275,32 @@ RawHID::RawHID(const QString &deviceName)
             dev.close(i);
     }
 
-    //didn't find the device we are trying to open??
+    //didn't find the device we are trying to open (shouldnt happen)
     if(m_deviceNo < 0)
+    {
         qDebug() << "Error: cannot open device " << deviceName;
+        return;
+    }
+
+    m_readThread = new RawHIDReadThread(this);
+    m_writeThread = new RawHIDWriteThread(this);
+
+    m_readThread->start();
+    m_writeThread->start();
 }
 
 RawHID::~RawHID()
 {
+    if(m_readThread)
+        delete m_readThread;
+
+    if(m_writeThread)
+        delete m_writeThread;
 }
 
 bool RawHID::open(OpenMode mode)
 {
-    if(m_deviceNo)
+    if(m_deviceNo < 0)
         return false;
 
     QIODevice::open(mode);
@@ -95,15 +319,23 @@ bool RawHID::isSequential() const
     return true;
 }
 
+qint64 RawHID::bytesAvailable() const
+{
+    return m_readThread->getBytesAvailable() + QIODevice::bytesAvailable();
+}
+
+qint64 RawHID::bytesToWrite() const
+{
+    return m_writeThread->getBytesToWrite() + QIODevice::bytesToWrite();
+}
 
 qint64 RawHID::readData(char *data, qint64 maxSize)
 {
-    return dev.receive(m_deviceNo, data, maxSize, IMMEDIATE_READ_TIMEOUT);
+    return m_readThread->getReadData(data, maxSize);
 }
 
 qint64 RawHID::writeData(const char *data, qint64 maxSize)
 {
-    return dev.send(m_deviceNo, const_cast<char*>(data), maxSize, IMMEDIATE_WRITE_TIMEOUT);
+    return m_writeThread->pushDataToWrite(data, maxSize);
 }
 
-

ground/src/plugins/rawhid/rawhid.h

@@ -29,16 +29,27 @@
 #define RAWHID_H
 
 #include "rawhid_global.h"
+
+#include <QThread>
 #include <QIODevice>
+#include <QMutex>
+#include <QByteArray>
 
 #include "pjrc_rawhid.h"
 
+//helper classes
+class RawHIDReadThread;
+class RawHIDWriteThread;
+
 /**
 *   The actual IO device that will be used to communicate
 *   with the board.
 */
 class RAWHID_EXPORT RawHID : public QIODevice
 {
+    friend class RawHIDReadThread;
+    friend class RawHIDWriteThread;
+
 public:
     RawHID();
     RawHID(const QString &deviceName);
@@ -51,10 +62,16 @@ public:
 protected:
     virtual qint64 readData(char *data, qint64 maxSize);
     virtual qint64 writeData(const char *data, qint64 maxSize);
+    virtual qint64 bytesAvailable() const;
+    virtual qint64 bytesToWrite() const;
 
     QString serialNumber;
+
     int m_deviceNo;
     pjrc_rawhid dev;
+
+    RawHIDReadThread *m_readThread;
+    RawHIDWriteThread *m_writeThread;
 };
 
 #endif // RAWHID_H

ground/src/plugins/rawhid/rawhidplugin.cpp

@@ -48,7 +48,7 @@ RawHIDEnumerationThread::~RawHIDEnumerationThread()
     m_running = false;
     //wait for the thread to terminate
     if(wait(1000) == false)
-        qDebug() << "Cannot terminate rawhid thread";
+        qDebug() << "Cannot terminate RawHIDEnumerationThread";
 }
 
 void RawHIDEnumerationThread::run()
@@ -67,7 +67,7 @@ void RawHIDEnumerationThread::run()
             }
         }
 
-        msleep(500); //update available devices twice per second
+        msleep(500); //update available devices twice per second (doesn't need more)
     }
 }
 
@@ -135,6 +135,43 @@ QString RawHIDConnection::shortName()
 
 
 
+//usb hid test thread
+//temporary...
+RawHIDTestThread::RawHIDTestThread()
+{
+    Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager();
+    QObject::connect(cm, SIGNAL(deviceConnected(QIODevice *)),
+                     this, SLOT(onDeviceConnect(QIODevice *)));
+    QObject::connect(cm, SIGNAL(deviceDisconnected()),
+                     this, SLOT(onDeviceDisconnect()));
+}
+
+void RawHIDTestThread::onDeviceConnect(QIODevice *dev)
+{
+    this->dev = dev;
+    dev->open(QIODevice::ReadWrite);
+    QObject::connect(dev, SIGNAL(readyRead()),
+                     this, SLOT(onReadyRead()));
+
+    QObject::connect(dev, SIGNAL(bytesWritten(qint64)),
+                     this, SLOT(onBytesWritten(qint64)));
+    dev->write("Hello raw hid device\n");
+}
+
+void RawHIDTestThread::onDeviceDisconnect()
+{
+    dev->close();
+}
+
+void RawHIDTestThread::onReadyRead()
+{
+    qDebug() << "Rx:" << dev->readLine(32);
+}
+
+void RawHIDTestThread::onBytesWritten(qint64 sz)
+{
+    qDebug() << "Sent " << sz << " bytes";
+}
 
 RawHIDPlugin::RawHIDPlugin()
 {
@@ -148,6 +185,9 @@ RawHIDPlugin::~RawHIDPlugin()
 void RawHIDPlugin::extensionsInitialized()
 {
     addAutoReleasedObject(new RawHIDConnection);
+
+    //temp for test
+    //addAutoReleasedObject(new RawHIDTestThread);
 }
 
 bool RawHIDPlugin::initialize(const QStringList & arguments, QString * errorString)

ground/src/plugins/rawhid/rawhidplugin.h

@@ -108,4 +108,21 @@ public:
 };
 
 
+//usb hid test thread
+#include "coreplugin/icore.h"
+#include "coreplugin/connectionmanager.h"
+class RawHIDTestThread: public QObject
+{
+    Q_OBJECT
+public:
+    RawHIDTestThread();
+protected slots:
+    void onDeviceConnect(QIODevice *);
+    void onDeviceDisconnect();
+    void onReadyRead();
+    void onBytesWritten(qint64 sz);
+protected:
+    QIODevice *dev;
+};
+
 #endif // RAWHIDPLUGIN_H