Joystick.cc 28.9 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

10 11 12 13 14

#include "Joystick.h"
#include "QGC.h"
#include "AutoPilotPlugin.h"
#include "UAS.h"
15 16 17 18
#include "QGCApplication.h"
#include "VideoManager.h"
#include "QGCCameraManager.h"
#include "QGCCameraControl.h"
19 20 21 22

#include <QSettings>

QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog")
23
QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog")
24

25
const char* Joystick::_settingsGroup =                  "Joysticks";
26
const char* Joystick::_calibratedSettingsKey =          "Calibrated2"; // Increment number to force recalibration
27 28 29 30 31
const char* Joystick::_buttonActionSettingsKey =        "ButtonActionName%1";
const char* Joystick::_throttleModeSettingsKey =        "ThrottleMode";
const char* Joystick::_exponentialSettingsKey =         "Exponential";
const char* Joystick::_accumulatorSettingsKey =         "Accumulator";
const char* Joystick::_deadbandSettingsKey =            "Deadband";
32
const char* Joystick::_circleCorrectionSettingsKey =    "Circle_Correction";
33
const char* Joystick::_frequencySettingsKey =           "Frequency";
34
const char* Joystick::_txModeSettingsKey =              nullptr;
35 36 37 38 39
const char* Joystick::_fixedWingTXModeSettingsKey =     "TXMode_FixedWing";
const char* Joystick::_multiRotorTXModeSettingsKey =    "TXMode_MultiRotor";
const char* Joystick::_roverTXModeSettingsKey =         "TXMode_Rover";
const char* Joystick::_vtolTXModeSettingsKey =          "TXMode_VTOL";
const char* Joystick::_submarineTXModeSettingsKey =     "TXMode_Submarine";
40

41 42 43 44
const char* Joystick::_buttonActionArm =                QT_TR_NOOP("Arm");
const char* Joystick::_buttonActionDisarm =             QT_TR_NOOP("Disarm");
const char* Joystick::_buttonActionVTOLFixedWing =      QT_TR_NOOP("VTOL: Fixed Wing");
const char* Joystick::_buttonActionVTOLMultiRotor =     QT_TR_NOOP("VTOL: Multi-Rotor");
45 46 47 48 49 50
const char* Joystick::_buttonActionZoomIn =             QT_TR_NOOP("Zoom In");
const char* Joystick::_buttonActionZoomOut =            QT_TR_NOOP("Zoom Out");
const char* Joystick::_buttonActionNextStream =         QT_TR_NOOP("Next Video Stream");
const char* Joystick::_buttonActionPreviousStream =     QT_TR_NOOP("Previous Video Stream");
const char* Joystick::_buttonActionNextCamera =         QT_TR_NOOP("Next Camera");
const char* Joystick::_buttonActionPreviousCamera =     QT_TR_NOOP("Previous Camera");
51

52 53 54 55 56 57 58
const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
    "RollAxis",
    "PitchAxis",
    "YawAxis",
    "ThrottleAxis"
};

59 60
int Joystick::_transmitterMode = 2;

61
Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager)
Gregory Dymarek's avatar
Gregory Dymarek committed
62
    : _exitThread(false)
63 64 65
    , _name(name)
    , _axisCount(axisCount)
    , _buttonCount(buttonCount)
66 67 68
    , _hatCount(hatCount)
    , _hatButtonCount(4*hatCount)
    , _totalButtonCount(_buttonCount+_hatButtonCount)
69
    , _calibrationMode(false)
70 71 72
    , _rgAxisValues(nullptr)
    , _rgCalibration(nullptr)
    , _rgButtonValues(nullptr)
73 74
    , _lastButtonBits(0)
    , _throttleMode(ThrottleModeCenterZero)
75
    , _negativeThrust(false)
76
    , _exponential(0)
77
    , _accumulator(false)
Gregory Dymarek's avatar
Gregory Dymarek committed
78
    , _deadband(false)
79
    , _circleCorrection(true)
80
    , _frequency(25.0f)
81
    , _activeVehicle(nullptr)
82
    , _pollingStartedForCalibration(false)
83
    , _multiVehicleManager(multiVehicleManager)
84
{
85

86 87
    _rgAxisValues = new int[_axisCount];
    _rgCalibration = new Calibration_t[_axisCount];
88
    _rgButtonValues = new bool[_totalButtonCount];
89 90

    for (int i=0; i<_axisCount; i++) {
91 92
        _rgAxisValues[i] = 0;
    }
93
    for (int i=0; i<_totalButtonCount; i++) {
94 95
        _rgButtonValues[i] = false;
    }
96

97 98
    _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());

99
    _loadSettings();
100 101

    connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
102 103 104 105
}

Joystick::~Joystick()
{
106 107 108 109
    // Crash out of the thread if it is still running
    terminate();
    wait();

Jacob Walser's avatar
Jacob Walser committed
110 111 112
    delete[] _rgAxisValues;
    delete[] _rgCalibration;
    delete[] _rgButtonValues;
113 114
}

115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131
void Joystick::_setDefaultCalibration(void) {
    QSettings   settings;
    settings.beginGroup(_settingsGroup);
    settings.beginGroup(_name);
    _calibrated = settings.value(_calibratedSettingsKey, false).toBool();

    // Only set default calibrations if we do not have a calibration for this gamecontroller
    if(_calibrated) return;

    for(int axis = 0; axis < _axisCount; axis++) {
        Joystick::Calibration_t calibration;
        _rgCalibration[axis] = calibration;
    }

    _rgCalibration[1].reversed = true;
    _rgCalibration[3].reversed = true;

132 133 134 135 136
    // Default TX Mode 2 axis assignments for gamecontrollers
    _rgFunctionAxis[rollFunction]       = 2;
    _rgFunctionAxis[pitchFunction]      = 3;
    _rgFunctionAxis[yawFunction]        = 0;
    _rgFunctionAxis[throttleFunction]   = 1;
137

138
    _exponential = 0;
139 140
    _accumulator = false;
    _deadband = false;
141
    _circleCorrection = false;
142
    _frequency = 25.0f;
143 144 145 146 147 148
    _throttleMode = ThrottleModeCenterZero;
    _calibrated = true;

    _saveSettings();
}

149
void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle)
150 151 152 153 154 155 156 157 158 159 160 161 162
{
    if(activeVehicle) {
        if(activeVehicle->fixedWing()) {
            _txModeSettingsKey = _fixedWingTXModeSettingsKey;
        } else if(activeVehicle->multiRotor()) {
            _txModeSettingsKey = _multiRotorTXModeSettingsKey;
        } else if(activeVehicle->rover()) {
            _txModeSettingsKey = _roverTXModeSettingsKey;
        } else if(activeVehicle->vtol()) {
            _txModeSettingsKey = _vtolTXModeSettingsKey;
        } else if(activeVehicle->sub()) {
            _txModeSettingsKey = _submarineTXModeSettingsKey;
        } else {
163
            _txModeSettingsKey = nullptr;
164 165 166
            qWarning() << "No valid joystick TXmode settings key for selected vehicle";
            return;
        }
167
    } else {
168
        _txModeSettingsKey = nullptr;
169 170
    }
}
171

172 173 174 175 176
void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
{
    _updateTXModeSettingsKey(activeVehicle);

    if(activeVehicle) {
177 178 179 180 181 182 183 184
        QSettings settings;
        settings.beginGroup(_settingsGroup);
        int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();

        setTXMode(mode);
    }
}

185 186 187
void Joystick::_loadSettings(void)
{
    QSettings   settings;
188

189
    settings.beginGroup(_settingsGroup);
190

191 192 193 194
    Vehicle* activeVehicle = _multiVehicleManager->activeVehicle();

    if(_txModeSettingsKey && activeVehicle)
        _transmitterMode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
195

196
    settings.beginGroup(_name);
197

198 199
    bool badSettings = false;
    bool convertOk;
200

201
    qCDebug(JoystickLog) << "_loadSettings " << _name;
202

203
    _calibrated = settings.value(_calibratedSettingsKey, false).toBool();
204
    _exponential = settings.value(_exponentialSettingsKey, 0).toFloat();
205
    _accumulator = settings.value(_accumulatorSettingsKey, false).toBool();
Gregory Dymarek's avatar
Gregory Dymarek committed
206
    _deadband = settings.value(_deadbandSettingsKey, false).toBool();
207
    _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool();
208
    _frequency = settings.value(_frequencySettingsKey, 25.0f).toFloat();
209

210 211
    _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk);
    badSettings |= !convertOk;
212

213
    qCDebug(JoystickLog) << "_loadSettings calibrated:txmode:throttlemode:exponential:deadband:badsettings" << _calibrated << _transmitterMode << _throttleMode << _exponential << _deadband << badSettings;
214

215 216 217 218
    QString minTpl  ("Axis%1Min");
    QString maxTpl  ("Axis%1Max");
    QString trimTpl ("Axis%1Trim");
    QString revTpl  ("Axis%1Rev");
219
    QString deadbndTpl  ("Axis%1Deadbnd");
220

221
    for (int axis=0; axis<_axisCount; axis++) {
222
        Calibration_t* calibration = &_rgCalibration[axis];
223

224 225
        calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk);
        badSettings |= !convertOk;
226

227 228
        calibration->min = settings.value(minTpl.arg(axis), -32768).toInt(&convertOk);
        badSettings |= !convertOk;
229

230
        calibration->max = settings.value(maxTpl.arg(axis), 32767).toInt(&convertOk);
231
        badSettings |= !convertOk;
232

233 234 235
        calibration->deadband = settings.value(deadbndTpl.arg(axis), 0).toInt(&convertOk);
        badSettings |= !convertOk;

236
        calibration->reversed = settings.value(revTpl.arg(axis), false).toBool();
237

238

239
        qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings;
240
    }
241

242 243
    for (int function=0; function<maxFunction; function++) {
        int functionAxis;
244

245 246
        functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk);
        badSettings |= !convertOk || (functionAxis == -1);
247

248
        _rgFunctionAxis[function] = functionAxis;
249

250 251
        qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings;
    }
252

253 254 255 256
    // FunctionAxis mappings are always stored in TX mode 2
    // Remap to stored TX mode in settings
    _remapAxes(2, _transmitterMode, _rgFunctionAxis);

Don Gagne's avatar
Don Gagne committed
257
    for (int button=0; button<_totalButtonCount; button++) {
258
        _rgButtonActions << settings.value(QString(_buttonActionSettingsKey).arg(button), QString()).toString();
Don Gagne's avatar
Don Gagne committed
259
        qCDebug(JoystickLog) << "_loadSettings button:action" << button << _rgButtonActions[button];
260
    }
261

262 263 264 265 266 267 268 269 270
    if (badSettings) {
        _calibrated = false;
        settings.setValue(_calibratedSettingsKey, false);
    }
}

void Joystick::_saveSettings(void)
{
    QSettings settings;
271

272
    settings.beginGroup(_settingsGroup);
273 274 275

    // Transmitter mode is static
    // Save the mode we are using
276 277
    if(_txModeSettingsKey)
        settings.setValue(_txModeSettingsKey, _transmitterMode);
278

279
    settings.beginGroup(_name);
280

281
    settings.setValue(_calibratedSettingsKey, _calibrated);
282
    settings.setValue(_exponentialSettingsKey, _exponential);
283
    settings.setValue(_accumulatorSettingsKey, _accumulator);
Gregory Dymarek's avatar
Gregory Dymarek committed
284
    settings.setValue(_deadbandSettingsKey, _deadband);
285
    settings.setValue(_circleCorrectionSettingsKey, _circleCorrection);
286
    settings.setValue(_frequencySettingsKey, _frequency);
287
    settings.setValue(_throttleModeSettingsKey, _throttleMode);
288

289
    qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode;
290 291 292 293 294

    QString minTpl  ("Axis%1Min");
    QString maxTpl  ("Axis%1Max");
    QString trimTpl ("Axis%1Trim");
    QString revTpl  ("Axis%1Rev");
295
    QString deadbndTpl  ("Axis%1Deadbnd");
296

297
    for (int axis=0; axis<_axisCount; axis++) {
298
        Calibration_t* calibration = &_rgCalibration[axis];
299

300 301 302 303
        settings.setValue(trimTpl.arg(axis), calibration->center);
        settings.setValue(minTpl.arg(axis), calibration->min);
        settings.setValue(maxTpl.arg(axis), calibration->max);
        settings.setValue(revTpl.arg(axis), calibration->reversed);
304
        settings.setValue(deadbndTpl.arg(axis), calibration->deadband);
305

306
        qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed:deadband"
307 308 309 310 311
                                << _name
                                << axis
                                << calibration->min
                                << calibration->max
                                << calibration->center
312 313
                                << calibration->reversed
                                << calibration->deadband;
314
    }
315

316 317 318 319 320
    // Always save function Axis mappings in TX Mode 2
    // Write mode 2 mappings without changing mapping currently in use
    int temp[maxFunction];
    _remapAxes(_transmitterMode, 2, temp);

321
    for (int function=0; function<maxFunction; function++) {
322
        settings.setValue(_rgFunctionSettingsKey[function], temp[function]);
323 324
        qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function];
    }
325

Don Gagne's avatar
Don Gagne committed
326
    for (int button=0; button<_totalButtonCount; button++) {
327 328 329 330 331
        settings.setValue(QString(_buttonActionSettingsKey).arg(button), _rgButtonActions[button]);
        qCDebug(JoystickLog) << "_saveSettings button:action" << button << _rgButtonActions[button];
    }
}

332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367
// Relative mappings of axis functions between different TX modes
int Joystick::_mapFunctionMode(int mode, int function) {

    static const int mapping[][4] = {
        { 2, 1, 0, 3 },
        { 2, 3, 0, 1 },
        { 0, 1, 2, 3 },
        { 0, 3, 2, 1 }};

    return mapping[mode-1][function];
}

// Remap current axis functions from current TX mode to new TX mode
void Joystick::_remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]) {
    int temp[maxFunction];

    for(int function = 0; function < maxFunction; function++) {
        temp[_mapFunctionMode(newMode, function)] = _rgFunctionAxis[_mapFunctionMode(currentMode, function)];
    }

    for(int function = 0; function < maxFunction; function++) {
        newMapping[function] = temp[function];
    }

}

void Joystick::setTXMode(int mode) {
    if(mode > 0 && mode <= 4) {
        _remapAxes(_transmitterMode, mode, _rgFunctionAxis);
        _transmitterMode = mode;
        _saveSettings();
    } else {
        qCWarning(JoystickLog) << "Invalid mode:" << mode;
    }
}

368
/// Adjust the raw axis value to the -1:1 range given calibration information
369
float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDeadbands)
370 371 372 373
{
    float valueNormalized;
    float axisLength;
    float axisBasis;
374

375 376 377 378 379 380 381 382 383
    if (value > calibration.center) {
        axisBasis = 1.0f;
        valueNormalized = value - calibration.center;
        axisLength =  calibration.max - calibration.center;
    } else {
        axisBasis = -1.0f;
        valueNormalized = calibration.center - value;
        axisLength =  calibration.center - calibration.min;
    }
384

385 386
    float axisPercent;

387
    if (withDeadbands) {
388 389 390 391 392 393 394
        if (valueNormalized>calibration.deadband) {
            axisPercent = (valueNormalized - calibration.deadband) / (axisLength - calibration.deadband);
        } else if (valueNormalized<-calibration.deadband) {
            axisPercent = (valueNormalized + calibration.deadband) / (axisLength - calibration.deadband);
        } else {
            axisPercent = 0.f;
        }
395 396 397
    }
    else {
        axisPercent = valueNormalized / axisLength;
398
    }
399

400
    float correctedValue = axisBasis * axisPercent;
401

402 403 404
    if (calibration.reversed) {
        correctedValue *= -1.0f;
    }
405

406
#if 0
407
    qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:deadband:basis:normalized:length"
408 409 410 411 412
                            << correctedValue
                            << value
                            << calibration.min
                            << calibration.max
                            << calibration.center
413
                            << calibration.reversed
414
                            << calibration.deadband
415 416 417 418 419
                            << axisBasis
                            << valueNormalized
                            << axisLength;
#endif

420
    return std::max(-1.0f, std::min(correctedValue, 1.0f));
421 422 423 424 425
}


void Joystick::run(void)
{
Gregory Dymarek's avatar
Gregory Dymarek committed
426
    _open();
427

428
    while (!_exitThread) {
Gregory Dymarek's avatar
Gregory Dymarek committed
429
    _update();
430 431 432

        // Update axes
        for (int axisIndex=0; axisIndex<_axisCount; axisIndex++) {
Gregory Dymarek's avatar
Gregory Dymarek committed
433
            int newAxisValue = _getAxis(axisIndex);
434 435 436 437
            // Calibration code requires signal to be emitted even if value hasn't changed
            _rgAxisValues[axisIndex] = newAxisValue;
            emit rawAxisValueChanged(axisIndex, newAxisValue);
        }
438

439 440
        // Update buttons
        for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) {
Gregory Dymarek's avatar
Gregory Dymarek committed
441
            bool newButtonValue = _getButton(buttonIndex);
442 443 444 445 446
            if (newButtonValue != _rgButtonValues[buttonIndex]) {
                _rgButtonValues[buttonIndex] = newButtonValue;
                emit rawButtonPressedChanged(buttonIndex, newButtonValue);
            }
        }
447 448 449 450 451 452 453 454 455 456 457 458 459 460 461

        // Update hat - append hat buttons to the end of the normal button list
        int numHatButtons = 4;
        for (int hatIndex=0; hatIndex<_hatCount; hatIndex++) {
            for (int hatButtonIndex=0; hatButtonIndex<numHatButtons; hatButtonIndex++) {
                // Create new index value that includes the normal button list
                int rgButtonValueIndex = hatIndex*numHatButtons + hatButtonIndex + _buttonCount;
                // Get hat value from joystick
                bool newButtonValue = _getHat(hatIndex,hatButtonIndex);
                if (newButtonValue != _rgButtonValues[rgButtonValueIndex]) {
                    _rgButtonValues[rgButtonValueIndex] = newButtonValue;
                    emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue);
                }
            }
        }
462

463
        if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) {
464
            int     axis = _rgFunctionAxis[rollFunction];
465
            float   roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
466

467
                    axis = _rgFunctionAxis[pitchFunction];
468
            float   pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
469

470
                    axis = _rgFunctionAxis[yawFunction];
471
            float   yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
472

473
                    axis = _rgFunctionAxis[throttleFunction];
474
            float   throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband);
475

476 477 478 479 480 481 482 483 484
            if ( _accumulator ) {
                static float throttle_accu = 0.f;

                throttle_accu += throttle*(40/1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time)

                throttle_accu = std::max(static_cast<float>(-1.f), std::min(throttle_accu, static_cast<float>(1.f)));
                throttle = throttle_accu;
            }

485 486 487 488 489 490 491 492 493 494 495 496 497
            if ( _circleCorrection ) {
                float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
                float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
                float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
                float throttle_limited = std::max(static_cast<float>(-M_PI_4), std::min(throttle, static_cast<float>(M_PI_4)));

                // Map from unit circle to linear range and limit
                roll =      std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f));
                pitch =     std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f));
                yaw =       std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f));
                throttle =  std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f));
            }

498
            if ( _exponential != 0 ) {
499
                // Exponential (0% to -50% range like most RC radios)
nanthony21's avatar
nanthony21 committed
500
                //_exponential is set by a slider in joystickConfig.qml
501 502

                // Calculate new RPY with exponential applied
503 504 505
                roll =      -_exponential*powf(roll,3) + (1+_exponential)*roll;
                pitch =     -_exponential*powf(pitch,3) + (1+_exponential)*pitch;
                yaw =       -_exponential*powf(yaw,3) + (1+_exponential)*yaw;
506
            }
507

508
            // Adjust throttle to 0:1 range
509
            if (_throttleMode == ThrottleModeCenterZero && _activeVehicle->supportsThrottleModeCenterZero()) {
510
                if (!_activeVehicle->supportsNegativeThrust() || !_negativeThrust) {
511 512
                    throttle = std::max(0.0f, throttle);
                }
513
            } else {
514 515
                throttle = (throttle + 1.0f) / 2.0f;
            }
516

517
            // Set up button pressed information
518

519 520
            quint16 newButtonBits = 0;      // New set of button which are down
            quint16 buttonPressedBits = 0;  // Buttons pressed for manualControl signal
521

522
            for (int buttonIndex=0; buttonIndex<_totalButtonCount; buttonIndex++) {
523
                quint16 buttonBit = 1 << buttonIndex;
524

525 526
                if (!_rgButtonValues[buttonIndex]) {
                    // Button up, just record it
527 528 529
                    newButtonBits |= buttonBit;
                } else {
                    if (_lastButtonBits & buttonBit) {
530
                        // Button was up last time through, but is now down which indicates a button press
531
                        qCDebug(JoystickLog) << "button triggered" << buttonIndex;
532

533 534 535
                        QString buttonAction =_rgButtonActions[buttonIndex];
                        if (!buttonAction.isEmpty()) {
                            _buttonAction(buttonAction);
536 537
                        }
                    }
538 539

                    // Mark the button as pressed as long as its pressed
540
                    buttonPressedBits |= buttonBit;
541 542
                }
            }
543

544
            _lastButtonBits = newButtonBits;
545

546
            qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle" << name() << roll << -pitch << yaw << throttle;
547

Don Gagne's avatar
Don Gagne committed
548
            // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub.
549
            emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode());
550
        }
551

552 553 554
        // Sleep. Update rate of joystick is by default 25 Hz
        int mswait = (int)(1000.0f / _frequency);
        QGC::SLEEP::msleep(mswait);
555
    }
556

Gregory Dymarek's avatar
Gregory Dymarek committed
557
    _close();
558 559
}

560
void Joystick::startPolling(Vehicle* vehicle)
561
{
562 563 564 565 566 567
    if (vehicle) {

        // If a vehicle is connected, disconnect it
        if (_activeVehicle) {
            UAS* uas = _activeVehicle->uas();
            disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
568
        }
569 570

        // Always set up the new vehicle
571
        _activeVehicle = vehicle;
572

573 574 575 576 577
        // If joystick is not calibrated, disable it
        if ( !_calibrated ) {
            vehicle->setJoystickEnabled(false);
        }

Jacob Walser's avatar
Jacob Walser committed
578 579 580
        // Update qml in case of joystick transition
        emit calibratedChanged(_calibrated);

581 582 583 584 585 586 587 588 589 590 591 592 593
        // Only connect the new vehicle if it wants joystick data
        if (vehicle->joystickEnabled()) {
            _pollingStartedForCalibration = false;

            UAS* uas = _activeVehicle->uas();
            connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
            // FIXME: ****
            //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
        }
    }


    if (!isRunning()) {
594 595
        _exitThread = false;
        start();
596 597 598 599 600
    }
}

void Joystick::stopPolling(void)
{
601
    if (isRunning()) {
602 603 604

        if (_activeVehicle && _activeVehicle->joystickEnabled()) {
            UAS* uas = _activeVehicle->uas();
Jacob Walser's avatar
Jacob Walser committed
605 606
            // Neutral attitude controls
            // emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode());
607 608
            disconnect(this, &Joystick::manualControl,          uas, &UAS::setExternalControlSetpoint);
        }
Don Gagne's avatar
Don Gagne committed
609 610
        // FIXME: ****
        //disconnect(this, &Joystick::buttonActionTriggered,  uas, &UAS::triggerAction);
611

612
        _exitThread = true;
Jacob Walser's avatar
Jacob Walser committed
613
    }
614 615 616 617
}

void Joystick::setCalibration(int axis, Calibration_t& calibration)
{
618
    if (!_validAxis(axis)) {
619 620 621
        qCWarning(JoystickLog) << "Invalid axis index" << axis;
        return;
    }
622

623 624 625 626 627 628 629 630
    _calibrated = true;
    _rgCalibration[axis] = calibration;
    _saveSettings();
    emit calibratedChanged(_calibrated);
}

Joystick::Calibration_t Joystick::getCalibration(int axis)
{
631
    if (!_validAxis(axis)) {
632 633
        qCWarning(JoystickLog) << "Invalid axis index" << axis;
    }
634

635 636 637 638 639
    return _rgCalibration[axis];
}

void Joystick::setFunctionAxis(AxisFunction_t function, int axis)
{
640
    if (!_validAxis(axis)) {
641 642 643 644 645 646
        qCWarning(JoystickLog) << "Invalid axis index" << axis;
        return;
    }

    _calibrated = true;
    _rgFunctionAxis[function] = axis;
647

648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663
    _saveSettings();
    emit calibratedChanged(_calibrated);
}

int Joystick::getFunctionAxis(AxisFunction_t function)
{
    if (function < 0 || function >= maxFunction) {
        qCWarning(JoystickLog) << "Invalid function" << function;
    }

    return _rgFunctionAxis[function];
}

QStringList Joystick::actions(void)
{
    QStringList list;
664
    list << _buttonActionArm << _buttonActionDisarm;
665 666 667
    if (_activeVehicle) {
        list << _activeVehicle->flightModes();
    }
668
    list << _buttonActionVTOLFixedWing << _buttonActionVTOLMultiRotor;
669 670 671
    list << _buttonActionZoomIn << _buttonActionZoomOut;
    list << _buttonActionNextStream << _buttonActionPreviousStream;
    list << _buttonActionNextCamera << _buttonActionPreviousCamera;
672 673 674
    return list;
}

Don Gagne's avatar
Don Gagne committed
675
void Joystick::setButtonAction(int button, const QString& action)
676
{
677
    if (!_validButton(button)) {
678 679 680
        qCWarning(JoystickLog) << "Invalid button index" << button;
        return;
    }
681

Don Gagne's avatar
Don Gagne committed
682
    qDebug() << "setButtonAction" << action;
683

684 685 686 687 688
    _rgButtonActions[button] = action;
    _saveSettings();
    emit buttonActionsChanged(buttonActions());
}

Don Gagne's avatar
Don Gagne committed
689
QString Joystick::getButtonAction(int button)
690
{
691
    if (!_validButton(button)) {
692 693
        qCWarning(JoystickLog) << "Invalid button index" << button;
    }
694

695 696 697 698 699 700
    return _rgButtonActions[button];
}

QVariantList Joystick::buttonActions(void)
{
    QVariantList list;
701

Don Gagne's avatar
Don Gagne committed
702
    for (int button=0; button<_totalButtonCount; button++) {
703 704
        list += QVariant::fromValue(_rgButtonActions[button]);
    }
705

706 707 708 709 710 711 712 713 714 715 716 717 718 719
    return list;
}

int Joystick::throttleMode(void)
{
    return _throttleMode;
}

void Joystick::setThrottleMode(int mode)
{
    if (mode < 0 || mode >= ThrottleModeMax) {
        qCWarning(JoystickLog) << "Invalid throttle mode" << mode;
        return;
    }
720

721
    _throttleMode = (ThrottleMode_t)mode;
722 723 724 725 726

    if (_throttleMode == ThrottleModeDownZero) {
        setAccumulator(false);
    }

727 728 729 730
    _saveSettings();
    emit throttleModeChanged(_throttleMode);
}

731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746
bool Joystick::negativeThrust(void)
{
    return _negativeThrust;
}

void Joystick::setNegativeThrust(bool allowNegative)
{
    if (_negativeThrust == allowNegative) {
        return;
    }
    _negativeThrust = allowNegative;

    _saveSettings();
    emit negativeThrustChanged(_negativeThrust);
}

747
float Joystick::exponential(void)
748 749 750 751
{
    return _exponential;
}

752
void Joystick::setExponential(float expo)
753 754 755 756 757 758 759
{
    _exponential = expo;

    _saveSettings();
    emit exponentialChanged(_exponential);
}

760 761 762 763 764 765 766 767 768 769 770 771 772
bool Joystick::accumulator(void)
{
    return _accumulator;
}

void Joystick::setAccumulator(bool accu)
{
    _accumulator = accu;

    _saveSettings();
    emit accumulatorChanged(_accumulator);
}

Gregory Dymarek's avatar
Gregory Dymarek committed
773 774 775 776 777 778 779 780 781 782 783 784
bool Joystick::deadband(void)
{
    return _deadband;
}

void Joystick::setDeadband(bool deadband)
{
    _deadband = deadband;

    _saveSettings();
}

785 786 787 788 789 790 791 792 793 794 795 796 797
bool Joystick::circleCorrection(void)
{
    return _circleCorrection;
}

void Joystick::setCircleCorrection(bool circleCorrection)
{
    _circleCorrection = circleCorrection;

    _saveSettings();
    emit circleCorrectionChanged(_circleCorrection);
}

798 799 800 801 802 803 804 805 806 807 808 809 810 811 812
float Joystick::frequency()
{
    return _frequency;
}

void Joystick::setFrequency(float val)
{
    //-- Arbitrary limits
    if(val < 0.25f)  val = 0.25f;
    if(val > 100.0f) val = 100.0f;
    _frequency = val;
    _saveSettings();
    emit frequencyChanged();
}

813
void Joystick::setCalibrationMode(bool calibrating)
814
{
815
    _calibrationMode = calibrating;
816

817
    if (calibrating && !isRunning()) {
818
        _pollingStartedForCalibration = true;
819
        startPolling(_multiVehicleManager->activeVehicle());
820
    }
821 822
    else if (_pollingStartedForCalibration) {
        stopPolling();
Don Gagne's avatar
Don Gagne committed
823
    }
824 825
}

826

Don Gagne's avatar
Don Gagne committed
827 828
void Joystick::_buttonAction(const QString& action)
{
829 830 831 832
    if (!_activeVehicle || !_activeVehicle->joystickEnabled()) {
        return;
    }

833
    if (action == _buttonActionArm) {
Don Gagne's avatar
Don Gagne committed
834
        _activeVehicle->setArmed(true);
835
    } else if (action == _buttonActionDisarm) {
Don Gagne's avatar
Don Gagne committed
836
        _activeVehicle->setArmed(false);
837 838 839 840
    } else if (action == _buttonActionVTOLFixedWing) {
        _activeVehicle->setVtolInFwdFlight(true);
    } else if (action == _buttonActionVTOLMultiRotor) {
        _activeVehicle->setVtolInFwdFlight(false);
841 842
    } else if (_activeVehicle->flightModes().contains(action)) {
        _activeVehicle->setFlightMode(action);
843 844 845 846 847 848
    } else if(action == _buttonActionZoomIn || action == _buttonActionZoomOut) {
        emit stepZoom(action == _buttonActionZoomIn ? 1 : -1);
    } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) {
        emit stepStream(action == _buttonActionNextStream ? 1 : -1);
    } else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) {
        emit stepCamera(action == _buttonActionNextCamera ? 1 : -1);
Don Gagne's avatar
Don Gagne committed
849 850 851 852 853
    } else {
        qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
    }
}

854 855 856 857 858 859 860
bool Joystick::_validAxis(int axis)
{
    return axis >= 0 && axis < _axisCount;
}

bool Joystick::_validButton(int button)
{
861
    return button >= 0 && button < _totalButtonCount;
862 863
}