Mouse6dofInput.cpp 10 KB
Newer Older
1 2 3 4 5 6 7 8 9 10
/**
 * @file
 *   @brief 3dConnexion 3dMouse interface for QGroundControl
 *
 *   @author Matthias Krebs <makrebs@student.ethz.ch>
 *
 */

#include "Mouse6dofInput.h"
#include "UAS.h"
11
#include "MultiVehicleManager.h"
Don Gagne's avatar
Don Gagne committed
12
#include "QGCMessageBox.h"
13

Don Gagne's avatar
Don Gagne committed
14
#ifdef QGC_MOUSE_ENABLED_LINUX
15 16
#include <QX11Info>
#include <X11/Xlib.h>
17
#ifdef Success
18
#undef Success              // Eigen library doesn't work if Success is defined
19
#endif //Success
20 21 22 23
extern "C"
{
#include "xdrvlib.h"
}
Don Gagne's avatar
Don Gagne committed
24
#endif // QGC_MOUSE_ENABLED_LINUX
25

Don Gagne's avatar
Don Gagne committed
26
#ifdef QGC_MOUSE_ENABLED_WIN
27
Mouse6dofInput::Mouse6dofInput(Mouse3DInput* mouseInput) :
28
    mouse3DMax(0.075),   // TODO: check maximum value fot plugged device
29 30
    uas(NULL),
    done(false),
31
    mouseActive(false),
32 33
    translationActive(true),
    rotationActive(true),
34 35 36 37 38 39 40
    xValue(0.0),
    yValue(0.0),
    zValue(0.0),
    aValue(0.0),
    bValue(0.0),
    cValue(0.0)
{
41 42
    connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &Mouse6dofInput::_activeVehicleChanged);
    
43 44
    // Connect 3DxWare SDK MotionEvent
    connect(mouseInput, SIGNAL(Move3d(std::vector<float>&)), this, SLOT(motion3DMouse(std::vector<float>&)));
45 46
    connect(mouseInput, SIGNAL(On3dmouseKeyDown(int)), this, SLOT(button3DMouseDown(int)));
    //connect(mouseInput, SIGNAL(On3dmouseKeyUp(int)), this, SLOT(FUNCTION(int)));
47 48

}
Don Gagne's avatar
Don Gagne committed
49
#endif //QGC_MOUSE_ENABLED_WIN
50

Don Gagne's avatar
Don Gagne committed
51
#ifdef QGC_MOUSE_ENABLED_LINUX
52 53
Mouse6dofInput::Mouse6dofInput(QWidget* parent) :
    mouse3DMax(350.0),   // TODO: check maximum value fot plugged device
54 55 56
    uas(NULL),
    done(false),
    mouseActive(false),
57 58
    translationActive(true),
    rotationActive(true),
59 60 61 62 63 64 65
    xValue(0.0),
    yValue(0.0),
    zValue(0.0),
    aValue(0.0),
    bValue(0.0),
    cValue(0.0)
{
66
    connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &Mouse6dofInput::_activeVehicleChanged);
67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87

    if (!mouseActive)
    {
//        // man visudo --> then you can omit giving password (success not guarantied..)
//        qDebug() << "Starting 3DxWare Daemon for 3dConnexion 3dMouse";
//        QString processProgramm = "gksudo";
//        QStringList processArguments;
//        processArguments << "/etc/3DxWare/daemon/3dxsrv -d usb";
//        process3dxDaemon = new QProcess();
//        process3dxDaemon->start(processProgramm, processArguments);
//    //    process3dxDaemon->waitForFinished();
//    //    {
//    //        qDebug() << "... continuing without 3DxWare. May not be initialized properly!";
//    //        qDebug() << "Try in terminal as user root:" << processArguments.last();
//    //    }

        Display *display = QX11Info::display();
        if(!display)
        {
            qDebug() << "Cannot open display!" << endl;
        }
88
        if ( !MagellanInit( display, parent->winId() ) )
89
        {
Don Gagne's avatar
Don Gagne committed
90 91
            QGCMessageBox::critical(tr("No 3DxWare driver is running."),
                                    tr("Enter in Terminal 'sudo /etc/3DxWare/daemon/3dxsrv -d usb' and then restart QGroundControl."));
92 93 94 95 96 97 98 99 100 101 102 103 104
            qDebug() << "No 3DxWare driver is running!";
            return;
        }
        else
        {
            qDebug() << "Initialized 3dMouse";
            mouseActive = true;
        }
    }
    else
    {
        qDebug() << "3dMouse already initialized..";
    }
105

106
}
Don Gagne's avatar
Don Gagne committed
107
#endif //QGC_MOUSE_ENABLED_LINUX
108

109

110 111 112 113 114
Mouse6dofInput::~Mouse6dofInput()
{
    done = true;
}

115
void Mouse6dofInput::_activeVehicleChanged(Vehicle* vehicle)
116 117 118
{
    if (this->uas)
    {
119 120 121
        disconnect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), uas, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
        // Todo: disconnect button mapping
        uas = NULL;
122 123
    }

124 125 126 127 128
    if (vehicle) {
        uas = vehicle->uas();
        
        connect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), uas, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
            // Todo: connect button mapping
129
    }
130
    
131 132 133 134 135 136 137 138 139
    if (!isRunning())
    {
        start();
    }
}

void Mouse6dofInput::init()
{
    // Make sure active UAS is set
Don Gagne's avatar
Don Gagne committed
140
    _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle());
141 142 143 144 145 146 147 148 149 150 151 152 153
}

void Mouse6dofInput::run()
{
    init();

    forever
    {
        if (done)
        {
           done = false;
           exit();
        }
154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177

        if (mouseActive)
        {
            // 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);
        }
178 179 180 181 182 183

        // Sleep, update rate of 3d mouse is approx. 50 Hz (1000 ms / 50 = 20 ms)
        QGC::SLEEP::msleep(20);
    }
}

Don Gagne's avatar
Don Gagne committed
184
#ifdef QGC_MOUSE_ENABLED_WIN
185 186 187
void Mouse6dofInput::motion3DMouse(std::vector<float> &motionData)
{
    if (motionData.size() < 6) return;
188 189
    mouseActive = true;

190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209
    if (translationActive)
    {
        xValue = (double)1.0e2f*motionData[ 1 ] / mouse3DMax;
        yValue = (double)1.0e2f*motionData[ 0 ] / mouse3DMax;
        zValue = (double)1.0e2f*motionData[ 2 ] / mouse3DMax;
    }else{
        xValue = 0;
        yValue = 0;
        zValue = 0;
    }
    if (rotationActive)
    {
        aValue = (double)1.0e2f*motionData[ 4 ] / mouse3DMax;
        bValue = (double)1.0e2f*motionData[ 3 ] / mouse3DMax;
        cValue = (double)1.0e2f*motionData[ 5 ] / mouse3DMax;
    }else{
        aValue = 0;
        bValue = 0;
        cValue = 0;
    }
210

211
    //qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
212
}
Don Gagne's avatar
Don Gagne committed
213
#endif //QGC_MOUSE_ENABLED_WIN
214

Don Gagne's avatar
Don Gagne committed
215
#ifdef QGC_MOUSE_ENABLED_WIN
216 217 218 219 220 221 222 223 224 225 226 227 228 229
void Mouse6dofInput::button3DMouseDown(int button)
{
    switch(button)
    {
    case 1:
    {
            rotationActive = !rotationActive;
            emit mouseRotationActiveChanged(rotationActive);
            qDebug() << "Changed 3DMouse Rotation to " << (bool)rotationActive;
        break;
    }
    case 2:
    {
            translationActive = !translationActive;
230
            emit mouseTranslationActiveChanged(translationActive);
231 232 233 234 235 236 237
            qDebug() << "Changed 3DMouse Translation to" << (bool)translationActive;
        break;
    }
    default:
        break;
    }
}
Don Gagne's avatar
Don Gagne committed
238
#endif //QGC_MOUSE_ENABLED_WIN
239

Don Gagne's avatar
Don Gagne committed
240
#ifdef QGC_MOUSE_ENABLED_LINUX
241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319
void Mouse6dofInput::handleX11Event(XEvent *event)
{
    //qDebug("XEvent occured...");
    if (!mouseActive)
    {
        qDebug() << "3dMouse not initialized. Cancelled handling X11event for 3dMouse";
        return;
    }

    MagellanFloatEvent MagellanEvent;

    Display *display = QX11Info::display();
    if(!display)
    {
        qDebug() << "Cannot open display!" << endl;
    }

    switch (event->type)
    {
    case ClientMessage:
      switch( MagellanTranslateEvent( display, event, &MagellanEvent, 1.0, 1.0 ) )
      {
        case MagellanInputMotionEvent :
             MagellanRemoveMotionEvents( display );
             for (int i = 0; i < 6; i++) {  // Saturation
                 MagellanEvent.MagellanData[i] = (abs(MagellanEvent.MagellanData[i]) < mouse3DMax) ? MagellanEvent.MagellanData[i] : (mouse3DMax*MagellanEvent.MagellanData[i]/abs(MagellanEvent.MagellanData[i]));
             }

             // Check whether translational motions are enabled
             if (translationActive)
             {
                 xValue = MagellanEvent.MagellanData[ MagellanZ ] / mouse3DMax;
                 yValue = MagellanEvent.MagellanData[ MagellanX ] / mouse3DMax;
                 zValue = - MagellanEvent.MagellanData[ MagellanY ] / mouse3DMax;
             }else{
                 xValue = 0;
                 yValue = 0;
                 zValue = 0;
             }
             // Check whether rotational motions are enabled
             if (rotationActive)
             {
                 aValue = MagellanEvent.MagellanData[ MagellanC ] / mouse3DMax;
                 bValue = MagellanEvent.MagellanData[ MagellanA ] / mouse3DMax;
                 cValue = - MagellanEvent.MagellanData[ MagellanB ] / mouse3DMax;
             }else{
                 aValue = 0;
                 bValue = 0;
                 cValue = 0;
             }
             //qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
        break;

        case MagellanInputButtonPressEvent :
            qDebug() << "MagellanInputButtonPressEvent called with button " << MagellanEvent.MagellanButton;
            switch (MagellanEvent.MagellanButton)
            {
            case 1:
            {
                    rotationActive = !rotationActive;
                    emit mouseRotationActiveChanged(rotationActive);
                    qDebug() << "Changed 3DMouse Rotation to " << (bool)rotationActive;
                break;
            }
            case 2:
            {
                    translationActive = !translationActive;
                    emit mouseTranslationActiveChanged(translationActive);
                    qDebug() << "Changed 3DMouse Translation to" << (bool)translationActive;
                break;
            }
            default:
                break;
            }
        default:
            break;
        }
    }
}
Don Gagne's avatar
Don Gagne committed
320
#endif //QGC_MOUSE_ENABLED_LINUX