JoystickInput.cc 8.15 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
/*=====================================================================
======================================================================*/

/**
 * @file
 *   @brief Joystick interface
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *   @author Andreas Romer <mavteam@student.ethz.ch>
 *
 */

#include "JoystickInput.h"

#include <QDebug>
#include <limits.h>
#include "UAS.h"
#include "UASManager.h"
19
#include "QGC.h"
20
#include <QMutexLocker>
pixhawk's avatar
pixhawk committed
21 22 23 24 25 26

/**
 * The coordinate frame of the joystick axis is the aeronautical frame like shown on this image:
 * @image html http://pixhawk.ethz.ch/wiki/_media/standards/body-frame.png Aeronautical frame
 */
JoystickInput::JoystickInput() :
LM's avatar
LM committed
27 28 29 30 31 32 33 34 35 36 37
        sdlJoystickMin(-32768.0f),
        sdlJoystickMax(32767.0f),
        defaultIndex(0),
        uas(NULL),
        uasButtonList(QList<int>()),
        done(false),
        thrustAxis(2),
        xAxis(0),
        yAxis(1),
        yawAxis(3),
        joystickName(tr("Unitinialized"))
pixhawk's avatar
pixhawk committed
38
{
39
    for (int i = 0; i < 10; i++) {
pixhawk's avatar
pixhawk committed
40 41 42 43 44 45 46
        calibrationPositive[i] = sdlJoystickMax;
        calibrationNegative[i] = sdlJoystickMin;
    }

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));

    // Enter main loop
47
    //start();
pixhawk's avatar
pixhawk committed
48 49
}

50 51
JoystickInput::~JoystickInput()
{
LM's avatar
LM committed
52
    done = true;
53
    QGC::SLEEP::usleep(50000);
LM's avatar
LM committed
54
    this->deleteLater();
55 56 57
}


pixhawk's avatar
pixhawk committed
58 59 60 61
void JoystickInput::setActiveUAS(UASInterface* uas)
{
    // Only connect / disconnect is the UAS is of a controllable UAS class
    UAS* tmp = 0;
LM's avatar
LM committed
62 63
    if (this->uas)
    {
pixhawk's avatar
pixhawk committed
64
        tmp = dynamic_cast<UAS*>(this->uas);
LM's avatar
LM committed
65 66
        if(tmp)
        {
pixhawk's avatar
pixhawk committed
67 68 69 70 71 72 73 74
            disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double)));
            disconnect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int)));
        }
    }

    this->uas = uas;

    tmp = dynamic_cast<UAS*>(this->uas);
75
    if(tmp) {
pixhawk's avatar
pixhawk committed
76 77 78
        connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double)));
        connect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int)));
    }
LM's avatar
LM committed
79 80
    if (!isRunning())
    {
81 82
        start();
    }
pixhawk's avatar
pixhawk committed
83 84 85 86 87
}

void JoystickInput::init()
{
    // INITIALIZE SDL Joystick support
88
    if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
pixhawk's avatar
pixhawk committed
89 90 91 92 93 94 95
        printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
    }

    // Enumerate joysticks and select one
    int numJoysticks = SDL_NumJoysticks();

    // Wait for joysticks if none is connected
LM's avatar
LM committed
96 97 98
    while (numJoysticks == 0)
    {
        MG::SLEEP::msleep(800);
pixhawk's avatar
pixhawk committed
99
        // INITIALIZE SDL Joystick support
LM's avatar
LM committed
100 101
        if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0)
        {
pixhawk's avatar
pixhawk committed
102 103 104 105 106 107
            printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
        }
        numJoysticks = SDL_NumJoysticks();
    }

    printf("%d Input devices found:\n", numJoysticks);
LM's avatar
LM committed
108 109
    for(int i=0; i < SDL_NumJoysticks(); i++ )
    {
pixhawk's avatar
pixhawk committed
110
        printf("\t- %s\n", SDL_JoystickName(i));
111
        joystickName = QString(SDL_JoystickName(i));
pixhawk's avatar
pixhawk committed
112 113 114 115 116 117 118
    }

    printf("\nOpened %s\n", SDL_JoystickName(defaultIndex));

    SDL_JoystickEventState(SDL_ENABLE);

    joystick = SDL_JoystickOpen(defaultIndex);
119 120 121

    // Make sure active UAS is set
    setActiveUAS(UASManager::instance()->getActiveUAS());
pixhawk's avatar
pixhawk committed
122 123 124 125 126 127 128 129 130 131
}

/**
 * @brief Execute the Joystick process
 */
void JoystickInput::run()
{

    init();

132
    forever
LM's avatar
LM committed
133 134 135 136 137 138 139 140
    {
        if (done)
        {
           done = false;
           exit();
        }
        while(SDL_PollEvent(&event))
        {
pixhawk's avatar
pixhawk committed
141 142 143 144

            SDL_JoystickUpdate();

            // Todo check if it would be more beneficial to use the event structure
145
            switch(event.type) {
pixhawk's avatar
pixhawk committed
146 147
            case SDL_KEYDOWN:
                /* handle keyboard stuff here */
148
                //qDebug() << "KEY PRESSED!";
pixhawk's avatar
pixhawk committed
149 150 151 152 153 154 155 156
                break;

            case SDL_QUIT:
                /* Set whatever flags are necessary to */
                /* end the main loop here */
                break;

            case SDL_JOYBUTTONDOWN:  /* Handle Joystick Button Presses */
157
                if ( event.jbutton.button == 0 ) {
158
                    //qDebug() << "BUTTON PRESSED!";
pixhawk's avatar
pixhawk committed
159 160 161 162
                }
                break;

            case SDL_JOYAXISMOTION:  /* Handle Joystick Motion */
163 164
                if ( ( event.jaxis.value < -3200 ) || (event.jaxis.value > 3200 ) ) {
                    if( event.jaxis.axis == 0) {
pixhawk's avatar
pixhawk committed
165 166 167
                        /* Left-right movement code goes here */
                    }

168
                    if( event.jaxis.axis == 1) {
pixhawk's avatar
pixhawk committed
169 170 171 172 173 174
                        /* Up-Down movement code goes here */
                    }
                }
                break;

            default:
175
                //qDebug() << "SDL event occured";
pixhawk's avatar
pixhawk committed
176 177 178 179 180
                break;
            }
        }

        // Display all axes
LM's avatar
LM committed
181 182
        for(int i = 0; i < SDL_JoystickNumAxes(joystick); i++)
        {
pixhawk's avatar
pixhawk committed
183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252
            //qDebug() << "\rAXIS" << i << "is: " << SDL_JoystickGetAxis(joystick, i);
        }

        // THRUST
        double thrust = ((double)SDL_JoystickGetAxis(joystick, thrustAxis) - calibrationNegative[thrustAxis]) / (calibrationPositive[thrustAxis] - calibrationNegative[thrustAxis]);
        // Has to be inverted for Logitech Wingman
        thrust = 1.0f - thrust;
        // Bound rounding errors
        if (thrust > 1) thrust = 1;
        if (thrust < 0) thrust = 0;
        emit thrustChanged((float)thrust);

        // X Axis
        double x = ((double)SDL_JoystickGetAxis(joystick, xAxis) - calibrationNegative[xAxis]) / (calibrationPositive[xAxis] - calibrationNegative[xAxis]);
        x = 1.0f - x;
        x = x * 2.0f - 1.0f;
        // Bound rounding errors
        if (x > 1.0f) x = 1.0f;
        if (x < -1.0f) x = -1.0f;
        emit xChanged((float)x);

        // Y Axis
        double y = ((double)SDL_JoystickGetAxis(joystick, yAxis) - calibrationNegative[yAxis]) / (calibrationPositive[yAxis] - calibrationNegative[yAxis]);
        y = 1.0f - y;
        y = y * 2.0f - 1.0f;
        // Bound rounding errors
        if (y > 1.0f) y = 1.0f;
        if (y < -1.0f) y = -1.0f;
        emit yChanged((float)y);

        // Yaw Axis

        double yaw = ((double)SDL_JoystickGetAxis(joystick, yawAxis) - calibrationNegative[yawAxis]) / (calibrationPositive[yawAxis] - calibrationNegative[yawAxis]);
        yaw = 1.0f - yaw;
        yaw = yaw * 2.0f - 1.0f;
        // Bound rounding errors
        if (yaw > 1.0f) yaw = 1.0f;
        if (yaw < -1.0f) yaw = -1.0f;
        emit yawChanged((float)yaw);

        // Get joystick hat position, convert it to vector
        int hatPosition = SDL_JoystickGetHat(joystick, 0);

        int xHat,yHat;
        xHat = 0;
        yHat = 0;

        // Build up vectors describing the hat position
        //
        // Coordinate frame for joystick hat:
        //
        //    y
        //    ^
        //    |
        //    |
        //    0 ----> x
        //

        if ((SDL_HAT_UP & hatPosition) > 0) yHat = 1;
        if ((SDL_HAT_DOWN & hatPosition) > 0) yHat = -1;

        if ((SDL_HAT_LEFT & hatPosition) > 0) xHat = -1;
        if ((SDL_HAT_RIGHT & hatPosition) > 0) xHat = 1;

        // Send new values to rest of groundstation
        emit hatDirectionChanged(xHat, yHat);
        emit joystickChanged(y, x, yaw, thrust, xHat, yHat);


        // Display all buttons
LM's avatar
LM committed
253 254
        for(int i = 0; i < SDL_JoystickNumButtons(joystick); i++)
        {
pixhawk's avatar
pixhawk committed
255
            //qDebug() << "BUTTON" << i << "is: " << SDL_JoystickGetAxis(joystick, i);
LM's avatar
LM committed
256 257
            if(SDL_JoystickGetButton(joystick, i))
            {
pixhawk's avatar
pixhawk committed
258 259 260
                emit buttonPressed(i);
                // Check if button is a UAS select button

LM's avatar
LM committed
261 262
                if (uasButtonList.contains(i))
                {
pixhawk's avatar
pixhawk committed
263
                    UASInterface* uas = UASManager::instance()->getUASForId(i);
LM's avatar
LM committed
264 265
                    if (uas)
                    {
pixhawk's avatar
pixhawk committed
266 267 268 269 270 271 272 273
                        UASManager::instance()->setActiveUAS(uas);
                    }
                }
            }

        }

        // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms)
274
        QGC::SLEEP::msleep(20);
pixhawk's avatar
pixhawk committed
275 276 277 278

    }

}
279 280 281 282 283

const QString& JoystickInput::getName()
{
    return joystickName;
}