JoystickInput.cc 26.3 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9
/*=====================================================================
======================================================================*/

/**
 * @file
 *   @brief Joystick interface
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *   @author Andreas Romer <mavteam@student.ethz.ch>
10
 *   @author Julian Oes <julian@oes.ch
pixhawk's avatar
pixhawk committed
11 12 13 14 15 16 17 18 19
 *
 */

#include "JoystickInput.h"

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

using namespace std;
pixhawk's avatar
pixhawk committed
27 28 29 30 31 32

/**
 * 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() :
33 34
    sdlJoystickMin(-32768.0f),
    sdlJoystickMax(32767.0f),
35
    isEnabled(false),
36
    isCalibrating(false),
37
    done(false),
38
    uas(NULL),
39 40
    autopilotType(0),
    systemType(0),
41
    uasCanReverse(false),
42 43 44 45 46 47
    rollAxis(-1),
    pitchAxis(-1),
    yawAxis(-1),
    throttleAxis(-1),
    joystickName(""),
    joystickID(-1),
48
    joystickNumButtons(0)
pixhawk's avatar
pixhawk committed
49
{
50 51 52

    // Make sure we initialize with the correct UAS.
    setActiveUAS(UASManager::instance()->getActiveUAS());
53

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

58 59
JoystickInput::~JoystickInput()
{
60 61
    storeGeneralSettings();
    storeJoystickSettings();
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
void JoystickInput::loadGeneralSettings()
{
    // Load defaults from settings
    QSettings settings;
    settings.sync();

    // Deal with settings specific to the JoystickInput
    settings.beginGroup("JOYSTICK_INPUT");
    isEnabled = settings.value("ENABLED", false).toBool();
74
    joystickName = settings.value("JOYSTICK_NAME", "").toString();
Thomas Gubler's avatar
Thomas Gubler committed
75
    mode = (JOYSTICK_MODE)settings.value("JOYSTICK_MODE", JOYSTICK_MODE_ATTITUDE).toInt();
76 77 78
    settings.endGroup();
}

79 80 81 82
/**
 * @brief Restores settings for the current joystick from saved settings file.
 * Assumes that both joystickName & joystickNumAxes are correct.
 */
83
void JoystickInput::loadJoystickSettings()
Lorenz Meier's avatar
Lorenz Meier committed
84
{
85 86 87
    // Load defaults from settings
    QSettings settings;
    settings.sync();
88 89

    // Now for the current joystick
90 91 92 93 94 95
    settings.beginGroup(joystickName);
    rollAxis = (settings.value("ROLL_AXIS_MAPPING", -1).toInt());
    pitchAxis = (settings.value("PITCH_AXIS_MAPPING", -1).toInt());
    yawAxis = (settings.value("YAW_AXIS_MAPPING", -1).toInt());
    throttleAxis = (settings.value("THROTTLE_AXIS_MAPPING", -1).toInt());

96 97 98 99
    // Clear out and then restore the (AUTOPILOT, SYSTEM) mapping for joystick settings
    joystickSettings.clear();
    int autopilots = settings.beginReadArray("AUTOPILOTS");
    for (int i = 0; i < autopilots; i++)
100
    {
101 102 103 104
        settings.setArrayIndex(i);
        int autopilotType = settings.value("AUTOPILOT_TYPE", 0).toInt();
        int systems = settings.beginReadArray("SYSTEMS");
        for (int j = 0; j < systems; j++)
105
        {
106 107 108 109 110 111
            settings.setArrayIndex(j);
            int systemType = settings.value("SYSTEM_TYPE", 0).toInt();

            // Now that both the autopilot and system type are available, update some references.
            QMap<int, bool>* joystickAxesInverted = &joystickSettings[autopilotType][systemType].axesInverted;
            QMap<int, bool>* joystickAxesLimited = &joystickSettings[autopilotType][systemType].axesLimited;
112 113
            QMap<int, float>* joystickAxesMinRange = &joystickSettings[autopilotType][systemType].axesMaxRange;
            QMap<int, float>* joystickAxesMaxRange = &joystickSettings[autopilotType][systemType].axesMinRange;
114 115 116 117 118 119 120 121 122 123 124 125
            QMap<int, int>* joystickButtonActions = &joystickSettings[autopilotType][systemType].buttonActions;

            // Read back the joystickAxesInverted QList one element at a time.
            int axesStored = settings.beginReadArray("AXES_INVERTED");
            for (int k = 0; k < axesStored; k++)
            {
                settings.setArrayIndex(k);
                int index = settings.value("INDEX", 0).toInt();
                bool inverted = settings.value("INVERTED", false).toBool();
                joystickAxesInverted->insert(index, inverted);
            }
            settings.endArray();
126

127 128 129 130 131 132 133 134 135 136 137
            // Read back the joystickAxesLimited QList one element at a time.
            axesStored = settings.beginReadArray("AXES_LIMITED");
            for (int k = 0; k < axesStored; k++)
            {
                settings.setArrayIndex(k);
                int index = settings.value("INDEX", 0).toInt();
                bool limited = settings.value("LIMITED", false).toBool();
                joystickAxesLimited->insert(index, limited);
            }
            settings.endArray();

138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159
            // Read back the joystickAxesMinRange QList one element at a time.
            axesStored = settings.beginReadArray("AXES_MIN_RANGE");
            for (int k = 0; k < axesStored; k++)
            {
                settings.setArrayIndex(k);
                int index = settings.value("INDEX", 0).toInt();
                float min = settings.value("MIN_RANGE", false).toFloat();
                joystickAxesMinRange->insert(index, min);
            }
            settings.endArray();

            // Read back the joystickAxesMaxRange QList one element at a time.
            axesStored = settings.beginReadArray("AXES_MAX_RANGE");
            for (int k = 0; k < axesStored; k++)
            {
                settings.setArrayIndex(k);
                int index = settings.value("INDEX", 0).toInt();
                float max = settings.value("MAX_RANGE", false).toFloat();
                joystickAxesMaxRange->insert(index, max);
            }
            settings.endArray();

160 161 162 163 164 165 166 167 168 169
            // Read back the button->action mapping.
            int buttonsStored = settings.beginReadArray("BUTTONS_ACTIONS");
            for (int k = 0; k < buttonsStored; k++)
            {
                settings.setArrayIndex(k);
                int index = settings.value("INDEX", 0).toInt();
                int action = settings.value("ACTION", 0).toInt();
                joystickButtonActions->insert(index, action);
            }
            settings.endArray();
170
        }
171
        settings.endArray();
172 173
    }
    settings.endArray();
174

175
    settings.endGroup();
176 177 178 179 180 181 182 183 184 185

    emit joystickSettingsChanged();
}

void JoystickInput::storeGeneralSettings() const
{
    QSettings settings;
    settings.beginGroup("JOYSTICK_INPUT");
    settings.setValue("ENABLED", isEnabled);
    settings.setValue("JOYSTICK_NAME", joystickName);
Thomas Gubler's avatar
Thomas Gubler committed
186
    settings.setValue("JOYSTICK_MODE", mode);
187 188
    settings.endGroup();
    settings.sync();
Lorenz Meier's avatar
Lorenz Meier committed
189 190
}

191
void JoystickInput::storeJoystickSettings() const
Lorenz Meier's avatar
Lorenz Meier committed
192
{
193 194 195 196 197 198 199
    // Store settings
    QSettings settings;
    settings.beginGroup(joystickName);
    settings.setValue("ROLL_AXIS_MAPPING", rollAxis);
    settings.setValue("PITCH_AXIS_MAPPING", pitchAxis);
    settings.setValue("YAW_AXIS_MAPPING", yawAxis);
    settings.setValue("THROTTLE_AXIS_MAPPING", throttleAxis);
200 201 202 203
    settings.beginWriteArray("AUTOPILOTS");
    QMapIterator<int, QMap<int, JoystickSettings> > i(joystickSettings);
    int autopilotIndex = 0;
    while (i.hasNext())
204
    {
205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224
        i.next();
        settings.setArrayIndex(autopilotIndex++);

        int autopilotType = i.key();
        settings.setValue("AUTOPILOT_TYPE", autopilotType);

        settings.beginWriteArray("SYSTEMS");
        QMapIterator<int, JoystickSettings> j(i.value());
        int systemIndex = 0;
        while (j.hasNext())
        {
            j.next();
            settings.setArrayIndex(systemIndex++);

            int systemType = j.key();
            settings.setValue("SYSTEM_TYPE", systemType);

            // Now that both the autopilot and system type are available, update some references.
            QMapIterator<int, bool> joystickAxesInverted(joystickSettings[autopilotType][systemType].axesInverted);
            QMapIterator<int, bool> joystickAxesLimited(joystickSettings[autopilotType][systemType].axesLimited);
225 226
            QMapIterator<int, float> joystickAxesMinRange(joystickSettings[autopilotType][systemType].axesMaxRange);
            QMapIterator<int, float> joystickAxesMaxRange(joystickSettings[autopilotType][systemType].axesMinRange);
227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245
            QMapIterator<int, int> joystickButtonActions(joystickSettings[autopilotType][systemType].buttonActions);

            settings.beginWriteArray("AXES_INVERTED");
            int k = 0;
            while (joystickAxesInverted.hasNext())
            {
                joystickAxesInverted.next();
                int inverted = joystickAxesInverted.value();
                // Only save this axes' inversion status if it's not the default
                if (inverted)
                {
                    settings.setArrayIndex(k++);
                    int index = joystickAxesInverted.key();
                    settings.setValue("INDEX", index);
                    settings.setValue("INVERTED", inverted);
                }
            }
            settings.endArray();

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
            settings.beginWriteArray("AXES_MIN_RANGE");
            k = 0;
            while (joystickAxesMinRange.hasNext())
            {
                joystickAxesMinRange.next();
                float min = joystickAxesMinRange.value();
                settings.setArrayIndex(k++);
                int index = joystickAxesMinRange.key();
                settings.setValue("INDEX", index);
                settings.setValue("MIN_RANGE", min);
            }
            settings.endArray();

            settings.beginWriteArray("AXES_MAX_RANGE");
            k = 0;
            while (joystickAxesMaxRange.hasNext())
            {
                joystickAxesMaxRange.next();
                float max = joystickAxesMaxRange.value();
                settings.setArrayIndex(k++);
                int index = joystickAxesMaxRange.key();
                settings.setValue("INDEX", index);
                settings.setValue("MAX_RANGE", max);
            }
            settings.endArray();

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
            settings.beginWriteArray("AXES_LIMITED");
            k = 0;
            while (joystickAxesLimited.hasNext())
            {
                joystickAxesLimited.next();
                int limited = joystickAxesLimited.value();
                if (limited)
                {
                    settings.setArrayIndex(k++);
                    int index = joystickAxesLimited.key();
                    settings.setValue("INDEX", index);
                    settings.setValue("LIMITED", limited);
                }
            }
            settings.endArray();

            settings.beginWriteArray("BUTTONS_ACTIONS");
            k = 0;
            while (joystickButtonActions.hasNext())
            {
                joystickButtonActions.next();
                int action = joystickButtonActions.value();
                if (action != -1)
                {
                    settings.setArrayIndex(k++);
                    int index = joystickButtonActions.key();
                    settings.setValue("INDEX", index);
                    settings.setValue("ACTION", action);
                }
            }
            settings.endArray();
        }
        settings.endArray(); // SYSTEMS
305
    }
306
    settings.endArray(); // AUTOPILOTS
307 308
    settings.endGroup();
    settings.sync();
309 310
}

pixhawk's avatar
pixhawk committed
311 312
void JoystickInput::setActiveUAS(UASInterface* uas)
{
313 314 315 316 317 318
    // Do nothing if the UAS hasn't changed.
    if (uas == this->uas)
    {
        return;
    }

pixhawk's avatar
pixhawk committed
319 320
    // Only connect / disconnect is the UAS is of a controllable UAS class
    UAS* tmp = 0;
LM's avatar
LM committed
321 322
    if (this->uas)
    {
pixhawk's avatar
pixhawk committed
323
        tmp = dynamic_cast<UAS*>(this->uas);
LM's avatar
LM committed
324 325
        if(tmp)
        {
326
            disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8)));
327
            disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
pixhawk's avatar
pixhawk committed
328
        }
329
        uasCanReverse = false;
pixhawk's avatar
pixhawk committed
330 331
    }

332 333 334 335 336 337
    // Save any settings for the last UAS
    if (joystickID > -1)
    {
        storeJoystickSettings();
    }

pixhawk's avatar
pixhawk committed
338 339
    this->uas = uas;

340
    if (this->uas && (tmp = dynamic_cast<UAS*>(this->uas)))
LM's avatar
LM committed
341
    {
342 343
        connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), tmp, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8)));
        qDebug() << "connected joystick";
344 345 346 347 348 349 350 351
        connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
        uasCanReverse = tmp->systemCanReverse();

        // Update the joystick settings for a new UAS.
        autopilotType = uas->getAutopilotType();
        systemType = uas->getSystemType();
    }

352 353 354 355
    // Make sure any UI elements know we've updated the UAS. The UASManager signal is re-emitted here so that UI elements know to
    // update their UAS-specific UI.
    emit activeUASSet(uas);

356 357 358 359
    // Load any joystick-specific settings now that the UAS has changed.
    if (joystickID > -1)
    {
        loadJoystickSettings();
360
    }
pixhawk's avatar
pixhawk committed
361 362
}

363 364 365 366 367 368
void JoystickInput::setEnabled(bool enabled)
{
    this->isEnabled = enabled;
    storeJoystickSettings();
}

pixhawk's avatar
pixhawk committed
369 370
void JoystickInput::init()
{
371
    // Initialize SDL Joystick support and detect number of joysticks.
372
    if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
pixhawk's avatar
pixhawk committed
373 374 375 376
        printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
    }

    // Enumerate joysticks and select one
377
    numJoysticks = SDL_NumJoysticks();
pixhawk's avatar
pixhawk committed
378

379 380 381
    // If no joysticks are connected, there's nothing we can do, so just keep
    // going back to sleep every second unless the program quits.
    while (!numJoysticks && !done)
LM's avatar
LM committed
382
    {
383
        QGC::SLEEP::sleep(1);
pixhawk's avatar
pixhawk committed
384
    }
385 386 387 388
    if (done)
    {
        return;
    }
pixhawk's avatar
pixhawk committed
389

390 391 392 393
    // Now that we've detected a joystick, load in the joystick-agnostic settings.
    loadGeneralSettings();

    // Enumerate all found devices
394
    qDebug() << QString("%1 Input devices found:").arg(numJoysticks);
395
    int activeJoystick = 0;
396
    for(int i=0; i < numJoysticks; i++ )
LM's avatar
LM committed
397
    {
398
        QString name = SDL_JoystickName(i);
399
        qDebug() << QString("\t%1").arg(name);
400 401 402 403 404 405 406 407

        // If we've matched this joystick to what was last opened, note it.
        // Note: The way this is implemented the LAST joystick of a given name will be opened.
        if (name == joystickName)
        {
            activeJoystick = i;
        }

408
        SDL_Joystick* x = SDL_JoystickOpen(i);
409 410
        qDebug() << QString("\tNumber of Axes: %1").arg(QString::number(SDL_JoystickNumAxes(x)));
        qDebug() << QString("\tNumber of Buttons: %1").arg(QString::number(SDL_JoystickNumButtons(x)));
411
        SDL_JoystickClose(x);
pixhawk's avatar
pixhawk committed
412 413
    }

414 415 416
    // Set the active joystick based on name, if a joystick was found in the saved settings, otherwise
    // default to the first one.
    setActiveJoystick(activeJoystick);
417

418
    // Now make sure we know what the current UAS is and track changes to it.
419
    setActiveUAS(UASManager::instance()->getActiveUAS());
420
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
pixhawk's avatar
pixhawk committed
421
}
422

423 424 425 426
void JoystickInput::shutdown()
{
    done = true;
}
pixhawk's avatar
pixhawk committed
427 428 429

/**
 * @brief Execute the Joystick process
430 431
 * Note that the SDL procedure is polled. This is because connecting and disconnecting while the event checker is running
 * fails as of SDL 1.2. It is therefore much easier to just poll for the joystick we want to sample.
pixhawk's avatar
pixhawk committed
432 433 434 435 436
 */
void JoystickInput::run()
{
    init();

437
    forever
LM's avatar
LM committed
438 439 440
    {
        if (done)
        {
441 442 443
            done = false;
            exit();
            return;
LM's avatar
LM committed
444
        }
pixhawk's avatar
pixhawk committed
445

446 447
        // Poll the joystick for new values.
        SDL_JoystickUpdate();
pixhawk's avatar
pixhawk committed
448

449
        // Emit all necessary signals for all axes.
450
        for (int i = 0; i < joystickNumAxes; i++)
451 452 453
        {
            // First emit the uncalibrated values for each axis based on their ID.
            // This is generally not used for controlling a vehicle, but a UI representation, so it being slightly off is fine.
454
            // Here we map the joystick axis value into the initial range of [0:1].
455
            float axisValue = SDL_JoystickGetAxis(joystick, i);
456 457 458 459 460 461 462 463 464 465 466 467 468 469 470

            // during calibration save min and max values
            if (isCalibrating)
            {
                if (joystickSettings[autopilotType][systemType].axesMinRange.value(i) > axisValue)
                {
                    joystickSettings[autopilotType][systemType].axesMinRange[i] = axisValue;
                }

                if (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) < axisValue)
                {
                    joystickSettings[autopilotType][systemType].axesMaxRange[i] = axisValue;
                }
            }

471
            if (joystickSettings[autopilotType][systemType].axesInverted[i])
472
            {
473 474 475
                axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMinRange.value(i)) /
                        (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) -
                         joystickSettings[autopilotType][systemType].axesMinRange.value(i));
476 477 478
            }
            else
            {
479 480 481
                axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMaxRange.value(i)) /
                        (joystickSettings[autopilotType][systemType].axesMinRange.value(i) -
                         joystickSettings[autopilotType][systemType].axesMaxRange.value(i));
482
            }
483
            axisValue = 1.0f - axisValue;
484

485
            // For non-throttle axes or if the UAS can reverse, go ahead and convert this into the range [-1:1].
486 487 488 489 490

            //if (uasCanReverse || throttleAxis != i)
            // don't take into account if UAS can reverse. This means to reverse position but not throttle
            // therefore deactivated for now
            if (throttleAxis != i)
491 492
            {
                axisValue = axisValue * 2.0f - 1.0f;
493
            }
494
            // Otherwise if this vehicle can only go forward, scale it to [0:1].
495
            else if (throttleAxis == i && joystickSettings[autopilotType][systemType].axesLimited.value(i))
496
            {
497
                axisValue = (axisValue);
498 499 500 501
                if (axisValue < 0.0f)
                {
                    axisValue = 0.0f;
                }
502
            }
503 504 505 506

            // Bound rounding errors
            if (axisValue > 1.0f) axisValue = 1.0f;
            if (axisValue < -1.0f) axisValue = -1.0f;
507 508 509 510 511
            if (joystickAxes[i] != axisValue)
            {
                joystickAxes[i] = axisValue;
                emit axisValueChanged(i, axisValue);
            }
512
        }
pixhawk's avatar
pixhawk committed
513 514

        // Build up vectors describing the hat position
515
        int hatPosition = SDL_JoystickGetHat(joystick, 0);
516
        qint8 newYHat = 0;
517 518
        if ((SDL_HAT_UP & hatPosition) > 0) newYHat = 1;
        if ((SDL_HAT_DOWN & hatPosition) > 0) newYHat = -1;
519
        qint8 newXHat = 0;
520 521 522 523 524 525
        if ((SDL_HAT_LEFT & hatPosition) > 0) newXHat = -1;
        if ((SDL_HAT_RIGHT & hatPosition) > 0) newXHat = 1;
        if (newYHat != yHat || newXHat != xHat)
        {
            xHat = newXHat;
            yHat = newYHat;
526
            emit hatDirectionChanged(newXHat, newYHat);
527
        }
pixhawk's avatar
pixhawk committed
528

529
        // Emit signals for each button individually
530
        for (int i = 0; i < joystickNumButtons; i++)
LM's avatar
LM committed
531
        {
532
            // If the button was down, but now it's up, trigger a buttonPressed event
533
            quint16 lastButtonState = joystickButtons & (1 << i);
534
            if (SDL_JoystickGetButton(joystick, i) && !lastButtonState)
LM's avatar
LM committed
535
            {
pixhawk's avatar
pixhawk committed
536
                emit buttonPressed(i);
537
                joystickButtons |= 1 << i;
538 539 540 541
            }
            else if (!SDL_JoystickGetButton(joystick, i) && lastButtonState)
            {
                emit buttonReleased(i);
542 543 544 545
                if (isEnabled && joystickSettings[autopilotType][systemType].buttonActions.contains(i))
                {
                    emit actionTriggered(joystickSettings[autopilotType][systemType].buttonActions.value(i));
                }
546
                joystickButtons &= ~(1 << i);
pixhawk's avatar
pixhawk committed
547 548
            }
        }
549 550

        // Now signal an update for all UI together.
551 552
        if (isEnabled)
        {
553 554 555 556
            float roll = rollAxis > -1?joystickAxes[rollAxis]:numeric_limits<float>::quiet_NaN();
            float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:numeric_limits<float>::quiet_NaN();
            float yaw = yawAxis > -1?joystickAxes[yawAxis]:numeric_limits<float>::quiet_NaN();
            float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:numeric_limits<float>::quiet_NaN();
557
            emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons, mode);
558
        }
pixhawk's avatar
pixhawk committed
559

560 561
        // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms)
        QGC::SLEEP::msleep(40);
pixhawk's avatar
pixhawk committed
562 563
    }
}
564

565 566
void JoystickInput::setActiveJoystick(int id)
{
567
    // If we already had a joystick, close that one before opening a new one.
568 569
    if (joystick && SDL_JoystickOpened(joystickID))
    {
570
        storeJoystickSettings();
571 572 573 574 575
        SDL_JoystickClose(joystick);
        joystick = NULL;
        joystickID = -1;
    }

576
    joystickID = id;
577
    joystick = SDL_JoystickOpen(joystickID);
578
    if (joystick && SDL_JoystickOpened(joystickID))
579
    {
580
        // Update joystick configuration.
581
        joystickName = QString(SDL_JoystickName(joystickID));
582 583 584
        joystickNumButtons = SDL_JoystickNumButtons(joystick);
        joystickNumAxes = SDL_JoystickNumAxes(joystick);

585
        // Restore saved settings for this joystick.
586
        loadJoystickSettings();
587

588 589
        // Update cached joystick axes values.
        // Also emit any signals for currently-triggering events
590
        joystickAxes.clear();
591
        for (int i = 0; i < joystickNumAxes; i++)
592
        {
593
            joystickAxes.append(numeric_limits<float>::quiet_NaN());
594
        }
595 596 597

        // Update cached joystick button values.
        // Emit signals for any button events.
598 599 600 601 602 603
        joystickButtons = 0;
    }
    else
    {
        joystickNumButtons = 0;
        joystickNumAxes = 0;
604
    }
605

606 607 608
    // Specify that a new joystick has been selected, so that any UI elements can update.
    emit newJoystickSelected();
    // And then trigger an update of this new UI.
609
    emit joystickSettingsChanged();
610 611
}

612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643
void JoystickInput::setCalibrating(bool active)
{
    if (active)
    {
        setEnabled(false);
        isCalibrating = true;

        // set range small so that limits can be re-found
        for (int i = 0; i < joystickNumAxes; i++)
        {
            joystickSettings[autopilotType][systemType].axesMinRange[i] = -10.0f;
            joystickSettings[autopilotType][systemType].axesMaxRange[i] = 10.0f;
        }

    } else {

        // store calibration values
        storeJoystickSettings();

        qDebug() << "Calibration result:";
        for (int i = 0; i < joystickNumAxes; i++)
        {
            qDebug() << i << ": " <<
                        joystickSettings[autopilotType][systemType].axesMinRange[i] <<
                        " - " <<
                        joystickSettings[autopilotType][systemType].axesMaxRange[i];
        }
        setEnabled(true);
        isCalibrating = false;
    }
}

644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676
void JoystickInput::setAxisMapping(int axis, JOYSTICK_INPUT_MAPPING newMapping)
{
    switch (newMapping)
    {
        case JOYSTICK_INPUT_MAPPING_ROLL:
            rollAxis = axis;
            break;
        case JOYSTICK_INPUT_MAPPING_PITCH:
            pitchAxis = axis;
            break;
        case JOYSTICK_INPUT_MAPPING_YAW:
            yawAxis = axis;
            break;
        case JOYSTICK_INPUT_MAPPING_THROTTLE:
            throttleAxis = axis;
            break;
        case JOYSTICK_INPUT_MAPPING_NONE:
        default:
            if (rollAxis == axis)
            {
                rollAxis = -1;
            }
            if (pitchAxis == axis)
            {
                pitchAxis = -1;
            }
            if (yawAxis == axis)
            {
                yawAxis = -1;
            }
            if (throttleAxis == axis)
            {
                throttleAxis = -1;
677
                joystickSettings[autopilotType][systemType].axesLimited.remove(axis);
678 679 680
            }
            break;
    }
681
    storeJoystickSettings();
682 683 684 685
}

void JoystickInput::setAxisInversion(int axis, bool inverted)
{
686
    if (axis < joystickNumAxes)
687
    {
688 689
        joystickSettings[autopilotType][systemType].axesInverted[axis] = inverted;
        storeJoystickSettings();
690 691 692
    }
}

693 694
void JoystickInput::setAxisRangeLimit(int axis, bool limitRange)
{
695
    if (axis < joystickNumAxes)
696
    {
697 698
        joystickSettings[autopilotType][systemType].axesLimited[axis] = limitRange;
        storeJoystickSettings();
699 700 701
    }
}

702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719
void JoystickInput::setAxisRangeLimitMin(int axis, float min)
{
    if (axis < joystickNumAxes)
    {
        joystickSettings[autopilotType][systemType].axesMinRange[axis] = min;
        storeJoystickSettings();
    }
}

void JoystickInput::setAxisRangeLimitMax(int axis, float max)
{
    if (axis < joystickNumAxes)
    {
        joystickSettings[autopilotType][systemType].axesMaxRange[axis] = max;
        storeJoystickSettings();
    }
}

720
void JoystickInput::setButtonAction(int button, int action)
721
{
722
    if (button < joystickNumButtons)
723
    {
724 725 726 727 728 729 730 731 732 733
        joystickSettings[autopilotType][systemType].buttonActions[button] = action;
        storeJoystickSettings();
    }
}

float JoystickInput::getCurrentValueForAxis(int axis) const
{
    if (axis < joystickNumAxes)
    {
        return joystickAxes.at(axis);
734 735
    }
    return 0.0f;
736
}
737

738
bool JoystickInput::getInvertedForAxis(int axis) const
739
{
740
    if (axis < joystickNumAxes)
741
    {
742
        return joystickSettings[autopilotType][systemType].axesInverted.value(axis);
743 744 745 746
    }
    return false;
}

747
bool JoystickInput::getRangeLimitForAxis(int axis) const
748
{
749
    if (axis < joystickNumAxes)
750
    {
751
        return joystickSettings[autopilotType][systemType].axesLimited.value(axis);
752 753 754
    }
    return false;
}
755

756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773
float JoystickInput::getAxisRangeLimitMinForAxis(int axis) const
{
    if (axis < joystickNumAxes)
    {
        return joystickSettings[autopilotType][systemType].axesMinRange.value(axis);
    }
    return sdlJoystickMin;
}

float JoystickInput::getAxisRangeLimitMaxForAxis(int axis) const
{
    if (axis < joystickNumAxes)
    {
        return joystickSettings[autopilotType][systemType].axesMaxRange.value(axis);
    }
    return sdlJoystickMax;
}

774 775 776 777 778 779 780 781
int JoystickInput::getActionForButton(int button) const
{
    if (button < joystickNumButtons && joystickSettings[autopilotType][systemType].buttonActions.contains(button))
    {
        return joystickSettings[autopilotType][systemType].buttonActions.value(button);
    }
    return -1;
}