Implementation of 6DOF 3DMouse input (Windows only)

Implementation of 6DOF 3DMouse input (Windows only). Not tested yet.

src/input/Mouse6dofInput.cpp

+/**
+ * @file
+ *   @brief 3dConnexion 3dMouse interface for QGroundControl
+ *
+ *   @author Matthias Krebs <makrebs@student.ethz.ch>
+ *
+ */
+
+#include "Mouse6dofInput.h"
+#include "UAS.h"
+#include "UASManager.h"
+
+Mouse6dofInput::Mouse6dofInput(Mouse3DInput* mouseInput) :
+    mouse3DMax(0.01),   // TODO: check maximum value fot plugged device
+    uas(NULL),
+    done(false),
+    xValue(0.0),
+    yValue(0.0),
+    zValue(0.0),
+    aValue(0.0),
+    bValue(0.0),
+    cValue(0.0)
+{
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+    // Connect 3DxWare SDK MotionEvent
+    connect(mouseInput, SIGNAL(Move3d(std::vector<float>&)), this, SLOT(motion3DMouse(std::vector<float>&)));
+    // TODO: Connect button mapping
+    //connect(mouseInput, SIGNAL(On3dmouseKeyDown(int)), this, SLOT);
+    //connect(mouseInput, SIGNAL(On3dmouseKeyUp(int)), this, SLOT);
+
+}
+
+Mouse6dofInput::~Mouse6dofInput()
+{
+    done = true;
+}
+
+void Mouse6dofInput::setActiveUAS(UASInterface* uas)
+{
+    // Only connect / disconnect is the UAS is of a controllable UAS class
+    UAS* tmp = 0;
+    if (this->uas)
+    {
+        tmp = dynamic_cast<UAS*>(this->uas);
+        if(tmp)
+        {
+            disconnect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
+            // Todo: disconnect button mapping
+        }
+    }
+
+    this->uas = uas;
+
+    tmp = dynamic_cast<UAS*>(this->uas);
+    if(tmp) {
+                connect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
+                // Todo: disconnect button mapping
+    }
+    if (!isRunning())
+    {
+        start();
+    }
+}
+
+void Mouse6dofInput::init()
+{
+    // Make sure active UAS is set
+    setActiveUAS(UASManager::instance()->getActiveUAS());
+}
+
+void Mouse6dofInput::run()
+{
+    init();
+
+    forever
+    {
+        if (done)
+        {
+           done = false;
+           exit();
+        }
+        // Bound x value
+        if (xValue > 1.0) xValue = 1.0;
+        if (xValue < -1.0) xValue = -1.0;
+        // Bound x value
+        if (yValue > 1.0) yValue = 1.0;
+        if (yValue < -1.0) yValue = -1.0;
+        // Bound x value
+        if (zValue > 1.0) zValue = 1.0;
+        if (zValue < -1.0) zValue = -1.0;
+        // Bound x value
+        if (aValue > 1.0) aValue = 1.0;
+        if (aValue < -1.0) aValue = -1.0;
+        // Bound x value
+        if (bValue > 1.0) bValue = 1.0;
+        if (bValue < -1.0) bValue = -1.0;
+        // Bound x value
+        if (cValue > 1.0) cValue = 1.0;
+        if (cValue < -1.0) cValue = -1.0;
+
+        emit mouse6dofChanged(xValue, yValue, zValue, aValue, bValue, cValue);
+
+        // Sleep, update rate of 3d mouse is approx. 50 Hz (1000 ms / 50 = 20 ms)
+        QGC::SLEEP::msleep(20);
+    }
+}
+
+void Mouse6dofInput::motion3DMouse(std::vector<float> &motionData)
+{
+    if (motionData.size() < 6) return;
+    xValue = (double)1.0e2f*motionData[ 1 ] / mouse3DMax;
+    yValue = (double)1.0e2f*motionData[ 0 ] / mouse3DMax;
+    zValue = (double)1.0e2f*motionData[ 2 ] / mouse3DMax;
+    aValue = (double)1.0e2f*motionData[ 4 ] / mouse3DMax;
+    bValue = (double)1.0e2f*motionData[ 3 ] / mouse3DMax;
+    cValue = (double)1.0e2f*motionData[ 5 ] / mouse3DMax;
+
+    qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
+}

src/input/Mouse6dofInput.h

+/**
+ * @file
+ *   @brief Definition of 3dConnexion 3dMouse interface for QGroundControl
+ *
+ *   @author Matthias Krebs <makrebs@student.ethz.ch>
+ *
+ */
+
+#ifndef MOUSE6DOFINPUT_H
+#define MOUSE6DOFINPUT_H
+
+#include <QThread>
+#include "Mouse3DInput.h"
+
+#include "UASInterface.h"
+
+class Mouse6dofInput : public QThread
+{
+    Q_OBJECT
+
+public:
+    Mouse6dofInput(Mouse3DInput* mouseInput);
+    ~Mouse6dofInput();
+    void run();
+
+    const double mouse3DMax;
+
+protected:
+    void init();
+
+    Mouse3DInput* mouse3D;
+    UASInterface* uas;
+    bool done;
+
+    double xValue;
+    double yValue;
+    double zValue;
+    double aValue;
+    double bValue;
+    double cValue;
+
+signals:
+    /**
+     * @brief Input of the 3d mouse has changed
+     *
+     * @param x Input x direction, range [-1, 1]
+     * @param y Input y direction, range [-1, 1]
+     * @param z Input z direction, range [-1, 1]
+     * @param a Input x rotation,  range [-1, 1]
+     * @param b Input y rotation,  range [-1, 1]
+     * @param c Input z rotation,  range [-1, 1]
+     */
+    void mouse6dofChanged(double x, double y, double z, double a, double b, double c);
+
+public slots:
+    void setActiveUAS(UASInterface* uas);
+    void motion3DMouse(std::vector<float> &motionData);
+
+};
+
+#endif // MOUSE6DOFINPUT_H