Implementation of 6DOF 3DMouse input (Windows only)

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

qgroundcontrol.pro

@@ -624,3 +624,16 @@ win32-msvc2008|win32-msvc2010|linux {
     LIBS += -Llibs/thirdParty/libxbee/lib \
         -llibxbee
 }
+
+#win32-msvc2008|win32-msvc2010 {
+    message("Including support for 3DxWare for Windows system.")
+    SOURCES  += libs/thirdParty/3DMouse/win/MouseParameters.cpp \
+                libs/thirdParty/3DMouse/win/Mouse3DInput.cpp \
+                src/input/Mouse6dofInput.cpp
+    HEADERS  += libs/thirdParty/3DMouse/win/I3dMouseParams.h \
+                libs/thirdParty/3DMouse/win/MouseParameters.h \
+                libs/thirdParty/3DMouse/win/Mouse3DInput.h \
+                src/input/Mouse6dofInput.h
+    INCLUDEPATH += libs/thirdParty/3DMouse/win
+    DEFINES += MOUSE_ENABLED_WIN
+#}

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

src/uas/UAS.cc

@@ -2458,6 +2458,11 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
     }
 }
 
+void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
+{
+// TODO: send mavlink 6DOF manual control message
+}
+
 /**
 * @return the type of the system
 */

src/uas/UAS.h

@@ -573,6 +573,9 @@ public slots:
     /** @brief Receive a button pressed event from an input device, e.g. joystick */
     void receiveButton(int buttonIndex);
 
+    /** @brief Set the values for the 6dof manual control of the vehicle */
+    void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);
+
     /** @brief Add a link associated with this robot */
     void addLink(LinkInterface* link);
     /** @brief Remove a link associated with this robot */

src/ui/MainWindow.cc

@@ -182,6 +182,13 @@ MainWindow::MainWindow(QWidget *parent):
     joystickWidget = 0;
     joystick = new JoystickInput();
 
+#ifdef MOUSE_ENABLED_WIN
+    emit initStatusChanged("Initializing 3D mouse interface.");
+
+    mouseInput = new Mouse3DInput(this);
+    mouse = new Mouse6dofInput(mouseInput);
+#endif //MOUSE_ENABLED_WIN
+
     // Connect link
     if (autoReconnect)
     {

src/ui/MainWindow.h

@@ -54,6 +54,7 @@ This file is part of the QGROUNDCONTROL project
 #include "HUD.h"
 #include "JoystickWidget.h"
 #include "input/JoystickInput.h"
+#include "input/Mouse6dofInput.h"
 #include "DebugConsole.h"
 #include "ParameterInterface.h"
 #include "XMLCommProtocolWidget.h"
@@ -371,6 +372,12 @@ protected:
 
     JoystickInput* joystick;
 
+#ifdef MOUSE_ENABLED_WIN
+    /** 3d Mouse support (WIN only) **/
+    Mouse3DInput* mouseInput;               ///< 3dConnexion 3dMouse SDK
+    Mouse6dofInput* mouse;                  ///< Implementation for 3dMouse input
+#endif // MOUSE_ENABLED_WIN
+
     /** User interface actions **/
     QAction* connectUASAct;
     QAction* disconnectUASAct;