JoystickInput.cc 10.9 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>
Lorenz Meier's avatar
Lorenz Meier committed
21
#include <QSettings>
pixhawk's avatar
pixhawk committed
22 23 24 25 26 27

/**
 * 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
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),
Lorenz Meier's avatar
Lorenz Meier committed
38 39 40
        autoButtonMapping(-1),
        manualButtonMapping(-1),
        stabilizeButtonMapping(-1),
41 42 43
        joystickName(tr("Unitinialized")),
        joystickButtons(0),
        joystickID(0)
pixhawk's avatar
pixhawk committed
44
{
Lorenz Meier's avatar
Lorenz Meier committed
45 46
    loadSettings();

47
    for (int i = 0; i < 10; i++) {
pixhawk's avatar
pixhawk committed
48 49 50 51
        calibrationPositive[i] = sdlJoystickMax;
        calibrationNegative[i] = sdlJoystickMin;
    }

52
    // Listen for when the active UAS changes so we can change who we're sending data to.
pixhawk's avatar
pixhawk committed
53 54
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));

55 56
    // Start this thread. This allows the Joystick Settings window to work correctly even w/o any UASes connected.
    start();
pixhawk's avatar
pixhawk committed
57 58
}

59 60
JoystickInput::~JoystickInput()
{
Lorenz Meier's avatar
Lorenz Meier committed
61
    storeSettings();
LM's avatar
LM committed
62
    done = true;
Lorenz Meier's avatar
Lorenz Meier committed
63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
}

void JoystickInput::loadSettings()
{
    // Load defaults from settings
    QSettings settings;
    settings.sync();
    settings.beginGroup("QGC_JOYSTICK_INPUT");
    xAxis = (settings.value("X_AXIS_MAPPING", xAxis).toInt());
    yAxis = (settings.value("Y_AXIS_MAPPING", yAxis).toInt());
    thrustAxis = (settings.value("THRUST_AXIS_MAPPING", thrustAxis).toInt());
    yawAxis = (settings.value("YAW_AXIS_MAPPING", yawAxis).toInt());
    autoButtonMapping = (settings.value("AUTO_BUTTON_MAPPING", autoButtonMapping).toInt());
    stabilizeButtonMapping = (settings.value("STABILIZE_BUTTON_MAPPING", stabilizeButtonMapping).toInt());
    manualButtonMapping = (settings.value("MANUAL_BUTTON_MAPPING", manualButtonMapping).toInt());
    settings.endGroup();
}

void JoystickInput::storeSettings()
{
    // Store settings
    QSettings settings;
    settings.beginGroup("QGC_JOYSTICK_INPUT");
    settings.setValue("X_AXIS_MAPPING", xAxis);
    settings.setValue("Y_AXIS_MAPPING", yAxis);
    settings.setValue("THRUST_AXIS_MAPPING", thrustAxis);
    settings.setValue("YAW_AXIS_MAPPING", yawAxis);
    settings.setValue("AUTO_BUTTON_MAPPING", autoButtonMapping);
    settings.setValue("STABILIZE_BUTTON_MAPPING", stabilizeButtonMapping);
    settings.setValue("MANUAL_BUTTON_MAPPING", manualButtonMapping);
    settings.endGroup();
    settings.sync();
95 96 97
}


pixhawk's avatar
pixhawk committed
98 99 100 101
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
102 103
    if (this->uas)
    {
pixhawk's avatar
pixhawk committed
104
        tmp = dynamic_cast<UAS*>(this->uas);
LM's avatar
LM committed
105 106
        if(tmp)
        {
107
            disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int)));
pixhawk's avatar
pixhawk committed
108 109 110 111 112 113
            disconnect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int)));
        }
    }

    this->uas = uas;

114
    if (this->uas)
LM's avatar
LM committed
115
    {
116 117 118 119 120
        tmp = dynamic_cast<UAS*>(this->uas);
        if(tmp) {
            connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int)));
            connect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int)));
        }
121
    }
pixhawk's avatar
pixhawk committed
122 123 124 125 126
}

void JoystickInput::init()
{
    // INITIALIZE SDL Joystick support
127
    if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
pixhawk's avatar
pixhawk committed
128 129 130 131
        printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
    }

    // Enumerate joysticks and select one
132
    joysticksFound = SDL_NumJoysticks();
pixhawk's avatar
pixhawk committed
133

134 135
    // Wait for joysticks if none are connected
    while (joysticksFound == 0 && !done)
LM's avatar
LM committed
136
    {
Lorenz Meier's avatar
Lorenz Meier committed
137
        QGC::SLEEP::msleep(400);
pixhawk's avatar
pixhawk committed
138
        // INITIALIZE SDL Joystick support
LM's avatar
LM committed
139 140
        if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0)
        {
pixhawk's avatar
pixhawk committed
141 142
            printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
        }
143
        joysticksFound = SDL_NumJoysticks();
pixhawk's avatar
pixhawk committed
144
    }
145 146 147 148
    if (done)
    {
        return;
    }
pixhawk's avatar
pixhawk committed
149

150 151
    qDebug() << QString("%1 Input devices found:").arg(joysticksFound);
    for(int i=0; i < joysticksFound; i++ )
LM's avatar
LM committed
152
    {
153 154 155 156 157 158
        qDebug() << QString("\t- %1").arg(SDL_JoystickName(i));
        SDL_Joystick* x = SDL_JoystickOpen(i);
        qDebug() << QString("Number of Axes: %1").arg(QString::number(SDL_JoystickNumAxes(x)));
        qDebug() << QString("Number of Buttons: %1").arg(QString::number(SDL_JoystickNumButtons(x)));
        qDebug() << QString("Number of Balls: %1").arg(QString::number(SDL_JoystickNumBalls(x)));
        SDL_JoystickClose(x);
pixhawk's avatar
pixhawk committed
159 160 161 162
    }

    SDL_JoystickEventState(SDL_ENABLE);

163 164
    // And attach to the default joystick.
    setActiveJoystick(defaultIndex);
165 166 167

    // Make sure active UAS is set
    setActiveUAS(UASManager::instance()->getActiveUAS());
pixhawk's avatar
pixhawk committed
168
}
169

170 171 172 173
void JoystickInput::shutdown()
{
    done = true;
}
pixhawk's avatar
pixhawk committed
174 175 176 177 178 179 180 181

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

182
    forever
LM's avatar
LM committed
183 184 185
    {
        if (done)
        {
186 187 188
            done = false;
            exit();
            return;
LM's avatar
LM committed
189 190 191
        }
        while(SDL_PollEvent(&event))
        {
pixhawk's avatar
pixhawk committed
192 193 194 195

            SDL_JoystickUpdate();

            // Todo check if it would be more beneficial to use the event structure
196
            switch(event.type) {
pixhawk's avatar
pixhawk committed
197 198
            case SDL_KEYDOWN:
                /* handle keyboard stuff here */
199
                //qDebug() << "KEY PRESSED!";
pixhawk's avatar
pixhawk committed
200 201 202 203 204 205 206 207
                break;

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

            case SDL_JOYBUTTONDOWN:  /* Handle Joystick Button Presses */
208
                if ( event.jbutton.button == 0 ) {
209
                    //qDebug() << "BUTTON PRESSED!";
pixhawk's avatar
pixhawk committed
210 211 212 213
                }
                break;

            case SDL_JOYAXISMOTION:  /* Handle Joystick Motion */
214 215
                if ( ( event.jaxis.value < -3200 ) || (event.jaxis.value > 3200 ) ) {
                    if( event.jaxis.axis == 0) {
pixhawk's avatar
pixhawk committed
216 217 218
                        /* Left-right movement code goes here */
                    }

219
                    if( event.jaxis.axis == 1) {
pixhawk's avatar
pixhawk committed
220 221 222 223 224 225
                        /* Up-Down movement code goes here */
                    }
                }
                break;

            default:
226
                //qDebug() << "SDL event occured";
pixhawk's avatar
pixhawk committed
227 228 229 230
                break;
            }
        }

231 232 233 234 235
//        // Display all axes
//        for(int i = 0; i < SDL_JoystickNumAxes(joystick); i++)
//        {
//            qDebug() << "\rAXIS" << i << "is: " << SDL_JoystickGetAxis(joystick, i);
//        }
pixhawk's avatar
pixhawk committed
236 237 238 239 240

        // 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;
241
        thrust = thrust * 2.0f - 1.0f;
pixhawk's avatar
pixhawk committed
242
        // Bound rounding errors
243 244
        if (thrust > 1.0f) thrust = 1.0f;
        if (thrust < -1.0f) thrust = -1.0f;
pixhawk's avatar
pixhawk committed
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
        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);

302 303
        // Emit signals for each button individually
        for (int i = 0; i < SDL_JoystickNumButtons(joystick); i++)
LM's avatar
LM committed
304
        {
305 306 307
            // If the button was down, but now it's up, trigger a buttonPressed event
            quint16 lastButtonState = buttonState & (1 << i);
            if (SDL_JoystickGetButton(joystick, i) && !lastButtonState)
LM's avatar
LM committed
308
            {
pixhawk's avatar
pixhawk committed
309
                emit buttonPressed(i);
310 311 312 313 314 315
                buttonState |= 1 << i;
            }
            else if (!SDL_JoystickGetButton(joystick, i) && lastButtonState)
            {
                emit buttonReleased(i);
                buttonState &= ~(1 << i);
pixhawk's avatar
pixhawk committed
316 317
            }
        }
318 319 320

        // Now signal an update for all UI together.
        emit joystickChanged(y, x, yaw, thrust, xHat, yHat, buttonState);
pixhawk's avatar
pixhawk committed
321 322

        // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms)
323
        QGC::SLEEP::msleep(20);
pixhawk's avatar
pixhawk committed
324 325
    }
}
326

327 328 329 330 331 332 333 334
void JoystickInput::setActiveJoystick(int id)
{
    joystickID = id;
    joystick = SDL_JoystickOpen(joystickID);
    if (joystick)
    {
        joystickName = QString(SDL_JoystickName(joystickID));
        joystickButtons = SDL_JoystickNumButtons(joystick);
335 336
        joystickAxes = SDL_JoystickNumAxes(joystick);
        qDebug() << QString("Switching to joystick '%1' with %2 buttons/%3 axes").arg(joystickName, QString::number(joystickButtons), QString::number(joystickAxes));
337
    }
338
    buttonState = 0;
339 340
}

341 342 343 344
const QString& JoystickInput::getName()
{
    return joystickName;
}