Joystick.cc 47.1 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9
 *
 * 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

#include <QSettings>

Gus Grubba's avatar
Gus Grubba committed
22
QGC_LOGGING_CATEGORY(JoystickLog,       "JoystickLog")
23
QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog")
24

25
const char* Joystick::_settingsGroup =                  "Joysticks";
Gus Grubba's avatar
Gus Grubba committed
26
const char* Joystick::_calibratedSettingsKey =          "Calibrated4"; // Increment number to force recalibration
Gus Grubba's avatar
Gus Grubba committed
27 28
const char* Joystick::_buttonActionNameKey =            "ButtonActionName%1";
const char* Joystick::_buttonActionRepeatKey =          "ButtonActionRepeat%1";
29 30 31 32
const char* Joystick::_throttleModeSettingsKey =        "ThrottleMode";
const char* Joystick::_exponentialSettingsKey =         "Exponential";
const char* Joystick::_accumulatorSettingsKey =         "Accumulator";
const char* Joystick::_deadbandSettingsKey =            "Deadband";
33
const char* Joystick::_circleCorrectionSettingsKey =    "Circle_Correction";
Gus Grubba's avatar
Gus Grubba committed
34 35
const char* Joystick::_axisFrequencySettingsKey =       "AxisFrequency";
const char* Joystick::_buttonFrequencySettingsKey =     "ButtonFrequency";
36
const char* Joystick::_txModeSettingsKey =              nullptr;
37 38 39 40 41
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";
Gus Grubba's avatar
Gus Grubba committed
42
const char* Joystick::_gimbalSettingsKey =              "GimbalEnabled";
43

Gus Grubba's avatar
Gus Grubba committed
44
const char* Joystick::_buttonActionNone =               QT_TR_NOOP("No Action");
45 46
const char* Joystick::_buttonActionArm =                QT_TR_NOOP("Arm");
const char* Joystick::_buttonActionDisarm =             QT_TR_NOOP("Disarm");
Gus Grubba's avatar
Gus Grubba committed
47
const char* Joystick::_buttonActionToggleArm =          QT_TR_NOOP("Toggle Arm");
48 49
const char* Joystick::_buttonActionVTOLFixedWing =      QT_TR_NOOP("VTOL: Fixed Wing");
const char* Joystick::_buttonActionVTOLMultiRotor =     QT_TR_NOOP("VTOL: Multi-Rotor");
Gus Grubba's avatar
Gus Grubba committed
50 51 52 53
const char* Joystick::_buttonActionContinuousZoomIn =   QT_TR_NOOP("Continuous Zoom In");
const char* Joystick::_buttonActionContinuousZoomOut =  QT_TR_NOOP("Continuous Zoom Out");
const char* Joystick::_buttonActionStepZoomIn =         QT_TR_NOOP("Step Zoom In");
const char* Joystick::_buttonActionStepZoomOut =        QT_TR_NOOP("Step Zoom Out");
54 55 56 57
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");
Gus Grubba's avatar
Gus Grubba committed
58 59 60 61
const char* Joystick::_buttonActionTriggerCamera =      QT_TR_NOOP("Trigger Camera");
const char* Joystick::_buttonActionStartVideoRecord =   QT_TR_NOOP("Start Recording Video");
const char* Joystick::_buttonActionStopVideoRecord =    QT_TR_NOOP("Stop Recording Video");
const char* Joystick::_buttonActionToggleVideoRecord =  QT_TR_NOOP("Toggle Recording Video");
Gus Grubba's avatar
Gus Grubba committed
62 63 64 65 66
const char* Joystick::_buttonActionGimbalDown =         QT_TR_NOOP("Gimbal Down");
const char* Joystick::_buttonActionGimbalUp =           QT_TR_NOOP("Gimbal Up");
const char* Joystick::_buttonActionGimbalLeft =         QT_TR_NOOP("Gimbal Left");
const char* Joystick::_buttonActionGimbalRight =        QT_TR_NOOP("Gimbal Right");
const char* Joystick::_buttonActionGimbalCenter =       QT_TR_NOOP("Gimbal Center");
67
const char* Joystick::_buttonActionEmergencyStop =      QT_TR_NOOP("Emergency Stop");
68

69 70 71 72
const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
    "RollAxis",
    "PitchAxis",
    "YawAxis",
Gus Grubba's avatar
Gus Grubba committed
73 74 75
    "ThrottleAxis",
    "GimbalPitchAxis",
    "GimbalYawAxis"
76 77
};

78 79
int Joystick::_transmitterMode = 2;

Gus Grubba's avatar
Gus Grubba committed
80 81 82 83 84 85 86
AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString name)
    : QObject(parent)
    , action(name)
{
}

AssignableButtonAction::AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_)
Gus Grubba's avatar
Gus Grubba committed
87 88 89 90 91 92
    : QObject(parent)
    , _action(action_)
    , _repeat(canRepeat_)
{
}

93
Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager)
Gus Grubba's avatar
Gus Grubba committed
94
    : _name(name)
95 96
    , _axisCount(axisCount)
    , _buttonCount(buttonCount)
97
    , _hatCount(hatCount)
Gus Grubba's avatar
Gus Grubba committed
98
    , _hatButtonCount(4 * hatCount)
99
    , _totalButtonCount(_buttonCount+_hatButtonCount)
100
    , _multiVehicleManager(multiVehicleManager)
101
{
Gus Grubba's avatar
Gus Grubba committed
102 103 104
    _rgAxisValues   = new int[static_cast<size_t>(_axisCount)];
    _rgCalibration  = new Calibration_t[static_cast<size_t>(_axisCount)];
    _rgButtonValues = new uint8_t[static_cast<size_t>(_totalButtonCount)];
Gus Grubba's avatar
Gus Grubba committed
105
    for (int i = 0; i < _axisCount; i++) {
106 107
        _rgAxisValues[i] = 0;
    }
Gus Grubba's avatar
Gus Grubba committed
108
    for (int i = 0; i < _totalButtonCount; i++) {
Gus Grubba's avatar
Gus Grubba committed
109
        _rgButtonValues[i] = BUTTON_UP;
Gus Grubba's avatar
Gus Grubba committed
110
        _buttonActionArray.append(nullptr);
111
    }
112
    _buildActionList(_multiVehicleManager->activeVehicle());
Gus Grubba's avatar
Gus Grubba committed
113 114 115
    _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
    _loadSettings();
    connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
116 117 118 119
}

Joystick::~Joystick()
{
120 121 122
    // Crash out of the thread if it is still running
    terminate();
    wait();
Jacob Walser's avatar
Jacob Walser committed
123 124 125
    delete[] _rgAxisValues;
    delete[] _rgCalibration;
    delete[] _rgButtonValues;
126
    _assignableButtonActions.clearAndDeleteContents();
Gus Grubba's avatar
Gus Grubba committed
127 128 129 130 131
    for (int button = 0; button < _totalButtonCount; button++) {
        if(_buttonActionArray[button]) {
            _buttonActionArray[button]->deleteLater();
        }
    }
132 133
}

134
void Joystick::_setDefaultCalibration(void) {
Gus Grubba's avatar
Gus Grubba committed
135
    QSettings settings;
136 137 138 139 140 141 142 143 144 145 146 147 148 149 150
    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;

151 152 153 154 155
    // Default TX Mode 2 axis assignments for gamecontrollers
    _rgFunctionAxis[rollFunction]       = 2;
    _rgFunctionAxis[pitchFunction]      = 3;
    _rgFunctionAxis[yawFunction]        = 0;
    _rgFunctionAxis[throttleFunction]   = 1;
156

Gus Grubba's avatar
Gus Grubba committed
157 158 159 160 161 162
    _rgFunctionAxis[gimbalPitchFunction]= 4;
    _rgFunctionAxis[gimbalYawFunction]  = 5;

    _exponential    = 0;
    _accumulator    = false;
    _deadband       = false;
Gus Grubba's avatar
Gus Grubba committed
163 164
    _axisFrequency  = 25.0f;
    _buttonFrequency= 5.0f;
Gus Grubba's avatar
Gus Grubba committed
165 166
    _throttleMode   = ThrottleModeDownZero;
    _calibrated     = true;
167
    _circleCorrection = false;
168 169 170 171

    _saveSettings();
}

172
void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle)
173 174 175 176 177 178 179 180 181 182 183 184 185
{
    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 {
186
            _txModeSettingsKey = nullptr;
187 188 189
            qWarning() << "No valid joystick TXmode settings key for selected vehicle";
            return;
        }
190
    } else {
191
        _txModeSettingsKey = nullptr;
192 193
    }
}
194

195 196 197 198
void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
{
    _updateTXModeSettingsKey(activeVehicle);
    if(activeVehicle) {
199 200 201 202 203 204 205
        QSettings settings;
        settings.beginGroup(_settingsGroup);
        int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
        setTXMode(mode);
    }
}

Gus Grubba's avatar
Gus Grubba committed
206
void Joystick::_loadSettings()
207
{
208
    QSettings settings;
209
    settings.beginGroup(_settingsGroup);
210 211 212 213
    Vehicle* activeVehicle = _multiVehicleManager->activeVehicle();

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

215
    settings.beginGroup(_name);
216

217 218
    bool badSettings = false;
    bool convertOk;
219

220
    qCDebug(JoystickLog) << "_loadSettings " << _name;
221

222 223 224 225
    _calibrated     = settings.value(_calibratedSettingsKey, false).toBool();
    _exponential    = settings.value(_exponentialSettingsKey, 0).toFloat();
    _accumulator    = settings.value(_accumulatorSettingsKey, false).toBool();
    _deadband       = settings.value(_deadbandSettingsKey, false).toBool();
Gus Grubba's avatar
Gus Grubba committed
226 227
    _axisFrequency  = settings.value(_axisFrequencySettingsKey, 25.0f).toFloat();
    _buttonFrequency= settings.value(_buttonFrequencySettingsKey, 5.0f).toFloat();
228
    _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool();
Gus Grubba's avatar
Gus Grubba committed
229
    _gimbalEnabled  = settings.value(_gimbalSettingsKey, false).toBool();
230

231
    _throttleMode   = static_cast<ThrottleMode_t>(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk));
232
    badSettings |= !convertOk;
233

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

236 237 238 239
    QString minTpl      ("Axis%1Min");
    QString maxTpl      ("Axis%1Max");
    QString trimTpl     ("Axis%1Trim");
    QString revTpl      ("Axis%1Rev");
240
    QString deadbndTpl  ("Axis%1Deadbnd");
241

242
    for (int axis = 0; axis < _axisCount; axis++) {
243
        Calibration_t* calibration = &_rgCalibration[axis];
244

245 246
        calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk);
        badSettings |= !convertOk;
247

248 249
        calibration->min = settings.value(minTpl.arg(axis), -32768).toInt(&convertOk);
        badSettings |= !convertOk;
250

251
        calibration->max = settings.value(maxTpl.arg(axis), 32767).toInt(&convertOk);
252
        badSettings |= !convertOk;
253

254 255 256
        calibration->deadband = settings.value(deadbndTpl.arg(axis), 0).toInt(&convertOk);
        badSettings |= !convertOk;

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

259
        qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings;
260
    }
261

Gus Grubba's avatar
Gus Grubba committed
262
    int workingAxis = 0;
Gus Grubba's avatar
Gus Grubba committed
263
    for (int function = 0; function < maxFunction; function++) {
264 265
        int functionAxis;
        functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk);
Gus Grubba's avatar
Gus Grubba committed
266 267 268 269
        badSettings |= !convertOk || (functionAxis >= _axisCount);
        if(functionAxis >= 0) {
            workingAxis++;
        }
Gus Grubba's avatar
Gus Grubba committed
270 271 272
        if(functionAxis < _axisCount) {
            _rgFunctionAxis[function] = functionAxis;
        }
273 274
        qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings;
    }
Gus Grubba's avatar
Gus Grubba committed
275
    badSettings |= workingAxis < 4;
276

277 278 279 280
    // FunctionAxis mappings are always stored in TX mode 2
    // Remap to stored TX mode in settings
    _remapAxes(2, _transmitterMode, _rgFunctionAxis);

Gus Grubba's avatar
Gus Grubba committed
281 282
    for (int button = 0; button < _totalButtonCount; button++) {
        QString a = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString();
Gus Grubba's avatar
Gus Grubba committed
283
        if(!a.isEmpty() && _findAssignableButtonAction(a) >= 0 && a != _buttonActionNone) {
Gus Grubba's avatar
Gus Grubba committed
284 285 286
            if(_buttonActionArray[button]) {
                _buttonActionArray[button]->deleteLater();
            }
Gus Grubba's avatar
Gus Grubba committed
287
            AssignedButtonAction* ap = new AssignedButtonAction(this, a);
Gus Grubba's avatar
Gus Grubba committed
288
            ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool();
Gus Grubba's avatar
Gus Grubba committed
289
            _buttonActionArray[button] = ap;
Gus Grubba's avatar
Gus Grubba committed
290 291
            _buttonActionArray[button]->buttonTime.start();
            qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat;
Gus Grubba's avatar
Gus Grubba committed
292
        }
293
    }
294

295 296 297 298 299 300
    if (badSettings) {
        _calibrated = false;
        settings.setValue(_calibratedSettingsKey, false);
    }
}

Gus Grubba's avatar
Gus Grubba committed
301
void Joystick::_saveButtonSettings()
302 303
{
    QSettings settings;
Gus Grubba's avatar
Gus Grubba committed
304 305 306 307 308 309
    settings.beginGroup(_settingsGroup);
    settings.beginGroup(_name);
    for (int button = 0; button < _totalButtonCount; button++) {
        if(_buttonActionArray[button]) {
            settings.setValue(QString(_buttonActionNameKey).arg(button),        _buttonActionArray[button]->action);
            settings.setValue(QString(_buttonActionRepeatKey).arg(button),      _buttonActionArray[button]->repeat);
Gus Grubba's avatar
Gus Grubba committed
310
            qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button <<  _buttonActionArray[button]->action << _buttonActionArray[button]->repeat;
Gus Grubba's avatar
Gus Grubba committed
311 312 313
        }
    }
}
314

Gus Grubba's avatar
Gus Grubba committed
315 316 317
void Joystick::_saveSettings()
{
    QSettings settings;
318
    settings.beginGroup(_settingsGroup);
319 320 321

    // Transmitter mode is static
    // Save the mode we are using
322
    if(_txModeSettingsKey)
Gus Grubba's avatar
Gus Grubba committed
323
        settings.setValue(_txModeSettingsKey,       _transmitterMode);
324

325
    settings.beginGroup(_name);
Gus Grubba's avatar
Gus Grubba committed
326 327 328 329 330 331 332 333
    settings.setValue(_calibratedSettingsKey,       _calibrated);
    settings.setValue(_exponentialSettingsKey,      _exponential);
    settings.setValue(_accumulatorSettingsKey,      _accumulator);
    settings.setValue(_deadbandSettingsKey,         _deadband);
    settings.setValue(_axisFrequencySettingsKey,    _axisFrequency);
    settings.setValue(_buttonFrequencySettingsKey,  _buttonFrequency);
    settings.setValue(_throttleModeSettingsKey,     _throttleMode);
    settings.setValue(_gimbalSettingsKey,           _gimbalEnabled);
334
    settings.setValue(_circleCorrectionSettingsKey, _circleCorrection);
335

336
    qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode;
337

338 339 340 341
    QString minTpl      ("Axis%1Min");
    QString maxTpl      ("Axis%1Max");
    QString trimTpl     ("Axis%1Trim");
    QString revTpl      ("Axis%1Rev");
342
    QString deadbndTpl  ("Axis%1Deadbnd");
343

344
    for (int axis = 0; axis < _axisCount; axis++) {
345 346 347 348 349
        Calibration_t* calibration = &_rgCalibration[axis];
        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);
350
        settings.setValue(deadbndTpl.arg(axis), calibration->deadband);
351
        qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed:deadband"
352 353 354 355 356
                                << _name
                                << axis
                                << calibration->min
                                << calibration->max
                                << calibration->center
357 358
                                << calibration->reversed
                                << calibration->deadband;
359
    }
360

361 362 363 364
    // 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);
365
    for (int function = 0; function < maxFunction; function++) {
366
        settings.setValue(_rgFunctionSettingsKey[function], temp[function]);
367 368
        qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function];
    }
Gus Grubba's avatar
Gus Grubba committed
369
    _saveButtonSettings();
370 371
}

372 373
// Relative mappings of axis functions between different TX modes
int Joystick::_mapFunctionMode(int mode, int function) {
374 375 376 377 378
    static const int mapping[][6] = {
        { yawFunction, pitchFunction, rollFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction },
        { yawFunction, throttleFunction, rollFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction },
        { rollFunction, pitchFunction, yawFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction },
        { rollFunction, throttleFunction, yawFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction }};
379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402
    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;
    }
}

403
/// Adjust the raw axis value to the -1:1 range given calibration information
404
float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDeadbands)
405 406 407 408
{
    float valueNormalized;
    float axisLength;
    float axisBasis;
409

410 411 412 413 414 415 416 417 418
    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;
    }
419

420 421
    float axisPercent;

422
    if (withDeadbands) {
423 424 425 426 427 428 429
        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;
        }
430 431 432
    }
    else {
        axisPercent = valueNormalized / axisLength;
433
    }
434

435
    float correctedValue = axisBasis * axisPercent;
436

437 438 439
    if (calibration.reversed) {
        correctedValue *= -1.0f;
    }
440

441
#if 0
442
    qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:deadband:basis:normalized:length"
443 444 445 446 447
                            << correctedValue
                            << value
                            << calibration.min
                            << calibration.max
                            << calibration.center
448
                            << calibration.reversed
449
                            << calibration.deadband
450 451 452 453 454
                            << axisBasis
                            << valueNormalized
                            << axisLength;
#endif

455
    return std::max(-1.0f, std::min(correctedValue, 1.0f));
456 457 458
}


Gus Grubba's avatar
Gus Grubba committed
459
void Joystick::run()
460
{
Gus Grubba's avatar
Gus Grubba committed
461
    //-- Joystick thread
Gregory Dymarek's avatar
Gregory Dymarek committed
462
    _open();
Gus Grubba's avatar
Gus Grubba committed
463 464 465 466 467 468 469
    //-- Reset timers
    _axisTime.start();
    for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
        if(_buttonActionArray[buttonIndex]) {
            _buttonActionArray[buttonIndex]->buttonTime.start();
        }
    }
470
    while (!_exitThread) {
Gus Grubba's avatar
Gus Grubba committed
471 472 473 474 475 476 477
        _update();
        _handleButtons();
        _handleAxis();
        QGC::SLEEP::msleep(20);
    }
    _close();
}
478

Gus Grubba's avatar
Gus Grubba committed
479 480
void Joystick::_handleButtons()
{
481
    int lastBbuttonValues[256];
Gus Grubba's avatar
Gus Grubba committed
482 483 484
    //-- Update button states
    for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) {
        bool newButtonValue = _getButton(buttonIndex);
485 486
        if(buttonIndex < 256)
            lastBbuttonValues[buttonIndex] = _rgButtonValues[buttonIndex];
Gus Grubba's avatar
Gus Grubba committed
487 488 489 490 491 492
        if (newButtonValue && _rgButtonValues[buttonIndex] == BUTTON_UP) {
            _rgButtonValues[buttonIndex] = BUTTON_DOWN;
            emit rawButtonPressedChanged(buttonIndex, newButtonValue);
        } else if (!newButtonValue && _rgButtonValues[buttonIndex] != BUTTON_UP) {
            _rgButtonValues[buttonIndex] = BUTTON_UP;
            emit rawButtonPressedChanged(buttonIndex, newButtonValue);
493
        }
Gus Grubba's avatar
Gus Grubba committed
494 495 496 497 498 499 500 501 502 503 504 505 506 507 508
    }
    //-- 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] == BUTTON_UP) {
                _rgButtonValues[rgButtonValueIndex] = BUTTON_DOWN;
                emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue);
            } else if (!newButtonValue && _rgButtonValues[rgButtonValueIndex] != BUTTON_UP) {
                _rgButtonValues[rgButtonValueIndex] = BUTTON_UP;
                emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue);
509 510
            }
        }
Gus Grubba's avatar
Gus Grubba committed
511
    }
512
    //-- Process button press/release
Gus Grubba's avatar
Gus Grubba committed
513
    for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
Gus Grubba's avatar
Gus Grubba committed
514
        if(_rgButtonValues[buttonIndex] == BUTTON_DOWN || _rgButtonValues[buttonIndex] == BUTTON_REPEAT) {
Gus Grubba's avatar
Gus Grubba committed
515
            if(_buttonActionArray[buttonIndex]) {
Gus Grubba's avatar
Gus Grubba committed
516 517 518 519 520 521
                QString buttonAction = _buttonActionArray[buttonIndex]->action;
                if(buttonAction.isEmpty() || buttonAction == _buttonActionNone)
                    continue;
                if(!_buttonActionArray[buttonIndex]->repeat) {
                    //-- This button just went down
                    if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) {
522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544
                        // Check for a multi-button action
                        QList<int> rgButtons = { buttonIndex };
                        bool executeButtonAction = true;
                        for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) {
                            if (multiIndex != buttonIndex) {
                                if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action == buttonAction) {
                                    // We found a multi-button action
                                    if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) {
                                        // So far so good
                                        rgButtons.append(multiIndex);
                                        continue;
                                    } else {
                                        // We are missing a press we need
                                        executeButtonAction = false;
                                        break;
                                    }
                                }
                            }
                        }
                        if (executeButtonAction) {
                            qCDebug(JoystickLog) << "Action triggered" << rgButtons << buttonAction;
                            _executeButtonAction(buttonAction, true);
                        }
Gus Grubba's avatar
Gus Grubba committed
545 546 547
                    }
                } else {
                    //-- Process repeat buttons
Gus Grubba's avatar
Gus Grubba committed
548
                    int buttonDelay = static_cast<int>(1000.0f / _buttonFrequency);
Gus Grubba's avatar
Gus Grubba committed
549 550
                    if(_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) {
                        _buttonActionArray[buttonIndex]->buttonTime.start();
Gus Grubba's avatar
Gus Grubba committed
551
                        qCDebug(JoystickLog) << "Repeat button triggered" << buttonIndex << buttonAction;
552
                        _executeButtonAction(buttonAction, true);
Gus Grubba's avatar
Gus Grubba committed
553
                    }
554 555
                }
            }
Gus Grubba's avatar
Gus Grubba committed
556 557
            //-- Flag it as processed
            _rgButtonValues[buttonIndex] = BUTTON_REPEAT;
558 559 560 561 562 563 564 565 566 567 568 569 570
        } else if(_rgButtonValues[buttonIndex] == BUTTON_UP) {
            //-- Button up transition
            if(buttonIndex < 256) {
                if(lastBbuttonValues[buttonIndex] == BUTTON_DOWN || lastBbuttonValues[buttonIndex] == BUTTON_REPEAT) {
                    if(_buttonActionArray[buttonIndex]) {
                        QString buttonAction = _buttonActionArray[buttonIndex]->action;
                        if(buttonAction.isEmpty() || buttonAction == _buttonActionNone)
                            continue;
                        qCDebug(JoystickLog) << "Button up" << buttonIndex << buttonAction;
                        _executeButtonAction(buttonAction, false);
                    }
                }
            }
571
        }
Gus Grubba's avatar
Gus Grubba committed
572 573
    }
}
574

Gus Grubba's avatar
Gus Grubba committed
575 576 577
void Joystick::_handleAxis()
{
    //-- Get frequency
Gus Grubba's avatar
Gus Grubba committed
578
    int axisDelay = static_cast<int>(1000.0f / _axisFrequency);
Gus Grubba's avatar
Gus Grubba committed
579 580 581 582 583 584 585 586 587 588
    //-- Check elapsed time since last run
    if(_axisTime.elapsed() > axisDelay) {
        _axisTime.start();
        //-- Update axis
        for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) {
            int newAxisValue = _getAxis(axisIndex);
            // Calibration code requires signal to be emitted even if value hasn't changed
            _rgAxisValues[axisIndex] = newAxisValue;
            emit rawAxisValueChanged(axisIndex, newAxisValue);
        }
589
        if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) {
590
            int     axis = _rgFunctionAxis[rollFunction];
Gus Grubba's avatar
Gus Grubba committed
591
            float   roll = _adjustRange(_rgAxisValues[axis],    _rgCalibration[axis], _deadband);
592

593
                    axis = _rgFunctionAxis[pitchFunction];
Gus Grubba's avatar
Gus Grubba committed
594
            float   pitch = _adjustRange(_rgAxisValues[axis],   _rgCalibration[axis], _deadband);
595

596
                    axis = _rgFunctionAxis[yawFunction];
Gus Grubba's avatar
Gus Grubba committed
597
            float   yaw = _adjustRange(_rgAxisValues[axis],     _rgCalibration[axis],_deadband);
598

599
                    axis = _rgFunctionAxis[throttleFunction];
Gus Grubba's avatar
Gus Grubba committed
600
            float   throttle = _adjustRange(_rgAxisValues[axis],_rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband);
601

Gus Grubba's avatar
Gus Grubba committed
602 603
            float   gimbalPitch = 0.0f;
            float   gimbalYaw   = 0.0f;
Gus Grubba's avatar
Gus Grubba committed
604

Gus Grubba's avatar
Gus Grubba committed
605 606 607 608 609 610 611 612 613
            if(_axisCount > 4) {
                axis = _rgFunctionAxis[gimbalPitchFunction];
                gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
            }

            if(_axisCount > 5) {
                axis = _rgFunctionAxis[gimbalYawFunction];
                gimbalYaw = _adjustRange(_rgAxisValues[axis],   _rgCalibration[axis],_deadband);
            }
Gus Grubba's avatar
Gus Grubba committed
614

Gus Grubba's avatar
Gus Grubba committed
615
            if (_accumulator) {
616
                static float throttle_accu = 0.f;
Gus Grubba's avatar
Gus Grubba committed
617
                throttle_accu += throttle * (40 / 1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time)
618 619 620 621
                throttle_accu = std::max(static_cast<float>(-1.f), std::min(throttle_accu, static_cast<float>(1.f)));
                throttle = throttle_accu;
            }

Gus Grubba's avatar
Gus Grubba committed
622
            if (_circleCorrection) {
Gus Grubba's avatar
Gus Grubba committed
623 624 625 626
                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)));
627 628

                // Map from unit circle to linear range and limit
Gus Grubba's avatar
Gus Grubba committed
629 630 631
                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));
632 633 634
                throttle =  std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f));
            }

Gus Grubba's avatar
Gus Grubba committed
635
            if ( _exponential < -0.01f) {
636
                // Exponential (0% to -50% range like most RC radios)
Gus Grubba's avatar
Gus Grubba committed
637
                // _exponential is set by a slider in joystickConfigAdvanced.qml
638
                // Calculate new RPY with exponential applied
Gus Grubba's avatar
Gus Grubba committed
639
                roll =      -_exponential*powf(roll, 3) + (1+_exponential)*roll;
640
                pitch =     -_exponential*powf(pitch,3) + (1+_exponential)*pitch;
Gus Grubba's avatar
Gus Grubba committed
641
                yaw =       -_exponential*powf(yaw,  3) + (1+_exponential)*yaw;
642
            }
643

644
            // Adjust throttle to 0:1 range
645
            if (_throttleMode == ThrottleModeCenterZero && _activeVehicle->supportsThrottleModeCenterZero()) {
646
                if (!_activeVehicle->supportsNegativeThrust() || !_negativeThrust) {
647 648
                    throttle = std::max(0.0f, throttle);
                }
649
            } else {
650 651
                throttle = (throttle + 1.0f) / 2.0f;
            }
Gus Grubba's avatar
Gus Grubba committed
652 653
            qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw;
            // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits)
Gus Grubba's avatar
Gus Grubba committed
654 655 656
            // Set up button bitmap
            quint64 buttonPressedBits = 0;  // Buttons pressed for manualControl signal
            for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
Gus Grubba's avatar
Gus Grubba committed
657
                quint64 buttonBit = static_cast<quint64>(1LL << buttonIndex);
Gus Grubba's avatar
Gus Grubba committed
658
                if (_rgButtonValues[buttonIndex] != BUTTON_UP) {
659
                    // Mark the button as pressed as long as its pressed
660
                    buttonPressedBits |= buttonBit;
661 662
                }
            }
Gus Grubba's avatar
Gus Grubba committed
663 664 665 666 667 668 669
            uint16_t shortButtons = static_cast<uint16_t>(buttonPressedBits & 0xFFFF);
            emit manualControl(roll, -pitch, yaw, throttle, shortButtons, _activeVehicle->joystickMode());
            if(_activeVehicle && _axisCount > 4 && _gimbalEnabled) {
                //-- TODO: There is nothing consuming this as there are no messages to handle gimbal
                //   the way MANUAL_CONTROL handles the other channels.
                emit manualControlGimbal((gimbalPitch + 1.0f) / 2.0f * 90.0f, gimbalYaw * 180.0f);
            }
670 671 672 673
        }
    }
}

674
void Joystick::startPolling(Vehicle* vehicle)
675
{
676 677 678 679 680
    if (vehicle) {
        // If a vehicle is connected, disconnect it
        if (_activeVehicle) {
            UAS* uas = _activeVehicle->uas();
            disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
Gus Grubba's avatar
Gus Grubba committed
681 682 683 684 685 686
            disconnect(this, &Joystick::setArmed,           _activeVehicle, &Vehicle::setArmed);
            disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
            disconnect(this, &Joystick::setFlightMode,      _activeVehicle, &Vehicle::setFlightMode);
            disconnect(this, &Joystick::gimbalPitchStep,    _activeVehicle, &Vehicle::gimbalPitchStep);
            disconnect(this, &Joystick::gimbalYawStep,      _activeVehicle, &Vehicle::gimbalYawStep);
            disconnect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
687
            disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
688
            disconnect(this, &Joystick::emergencyStop,      _activeVehicle, &Vehicle::emergencyStop);
689
        }
690
        // Always set up the new vehicle
691
        _activeVehicle = vehicle;
692 693 694 695
        // If joystick is not calibrated, disable it
        if ( !_calibrated ) {
            vehicle->setJoystickEnabled(false);
        }
Jacob Walser's avatar
Jacob Walser committed
696 697
        // Update qml in case of joystick transition
        emit calibratedChanged(_calibrated);
698 699
        // Build action list
        _buildActionList(vehicle);
700 701 702 703 704
        // 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);
Gus Grubba's avatar
Gus Grubba committed
705 706 707 708 709 710
            connect(this, &Joystick::setArmed,           _activeVehicle, &Vehicle::setArmed);
            connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
            connect(this, &Joystick::setFlightMode,      _activeVehicle, &Vehicle::setFlightMode);
            connect(this, &Joystick::gimbalPitchStep,    _activeVehicle, &Vehicle::gimbalPitchStep);
            connect(this, &Joystick::gimbalYawStep,      _activeVehicle, &Vehicle::gimbalYawStep);
            connect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
711
            connect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
712
            connect(this, &Joystick::emergencyStop,      _activeVehicle, &Vehicle::emergencyStop);
713 714 715 716 717
            // FIXME: ****
            //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
        }
    }
    if (!isRunning()) {
718 719
        _exitThread = false;
        start();
720 721 722 723 724
    }
}

void Joystick::stopPolling(void)
{
725
    if (isRunning()) {
726 727
        if (_activeVehicle && _activeVehicle->joystickEnabled()) {
            UAS* uas = _activeVehicle->uas();
Jacob Walser's avatar
Jacob Walser committed
728 729
            // Neutral attitude controls
            // emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode());
Gus Grubba's avatar
Gus Grubba committed
730 731 732 733 734 735 736
            disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
            disconnect(this, &Joystick::setArmed,           _activeVehicle, &Vehicle::setArmed);
            disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
            disconnect(this, &Joystick::setFlightMode,      _activeVehicle, &Vehicle::setFlightMode);
            disconnect(this, &Joystick::gimbalPitchStep,    _activeVehicle, &Vehicle::gimbalPitchStep);
            disconnect(this, &Joystick::gimbalYawStep,      _activeVehicle, &Vehicle::gimbalYawStep);
            disconnect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
737
            disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
738
        }
Don Gagne's avatar
Don Gagne committed
739 740
        // FIXME: ****
        //disconnect(this, &Joystick::buttonActionTriggered,  uas, &UAS::triggerAction);
741
        _exitThread = true;
Jacob Walser's avatar
Jacob Walser committed
742
    }
743 744 745 746
}

void Joystick::setCalibration(int axis, Calibration_t& calibration)
{
747
    if (!_validAxis(axis)) {
748 749 750 751 752 753 754 755 756 757
        return;
    }
    _calibrated = true;
    _rgCalibration[axis] = calibration;
    _saveSettings();
    emit calibratedChanged(_calibrated);
}

Joystick::Calibration_t Joystick::getCalibration(int axis)
{
758
    if (!_validAxis(axis)) {
Gus Grubba's avatar
Gus Grubba committed
759
        return Calibration_t();
760 761 762 763 764 765
    }
    return _rgCalibration[axis];
}

void Joystick::setFunctionAxis(AxisFunction_t function, int axis)
{
766
    if (!_validAxis(axis)) {
767 768 769 770 771 772 773 774 775 776
        return;
    }
    _calibrated = true;
    _rgFunctionAxis[function] = axis;
    _saveSettings();
    emit calibratedChanged(_calibrated);
}

int Joystick::getFunctionAxis(AxisFunction_t function)
{
Gus Grubba's avatar
Gus Grubba committed
777
    if (static_cast<int>(function) < 0 || function >= maxFunction) {
778 779 780 781 782
        qCWarning(JoystickLog) << "Invalid function" << function;
    }
    return _rgFunctionAxis[function];
}

Gus Grubba's avatar
Gus Grubba committed
783
void Joystick::setButtonRepeat(int button, bool repeat)
784
{
Gus Grubba's avatar
Gus Grubba committed
785 786 787 788
    if (!_validButton(button) || !_buttonActionArray[button]) {
        return;
    }
    _buttonActionArray[button]->repeat = repeat;
Gus Grubba's avatar
Gus Grubba committed
789 790 791 792 793 794
    _buttonActionArray[button]->buttonTime.start();
    //-- Save to settings
    QSettings settings;
    settings.beginGroup(_settingsGroup);
    settings.beginGroup(_name);
    settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat);
Gus Grubba's avatar
Gus Grubba committed
795 796
}

Gus Grubba's avatar
Gus Grubba committed
797
bool Joystick::getButtonRepeat(int button)
Gus Grubba's avatar
Gus Grubba committed
798 799
{
    if (!_validButton(button) || !_buttonActionArray[button]) {
Gus Grubba's avatar
Gus Grubba committed
800
        return false;
Gus Grubba's avatar
Gus Grubba committed
801
    }
Gus Grubba's avatar
Gus Grubba committed
802
    return _buttonActionArray[button]->repeat;
803 804
}

Don Gagne's avatar
Don Gagne committed
805
void Joystick::setButtonAction(int button, const QString& action)
806
{
807
    if (!_validButton(button)) {
808 809
        return;
    }
Gus Grubba's avatar
Gus Grubba committed
810
    qCWarning(JoystickLog) << "setButtonAction:" << button << action;
Gus Grubba's avatar
Gus Grubba committed
811 812 813
    QSettings settings;
    settings.beginGroup(_settingsGroup);
    settings.beginGroup(_name);
Gus Grubba's avatar
Gus Grubba committed
814 815 816 817
    if(action.isEmpty() || action == _buttonActionNone) {
        if(_buttonActionArray[button]) {
            _buttonActionArray[button]->deleteLater();
            _buttonActionArray[button] = nullptr;
Gus Grubba's avatar
Gus Grubba committed
818 819 820
            //-- Clear from settings
            settings.remove(QString(_buttonActionNameKey).arg(button));
            settings.remove(QString(_buttonActionRepeatKey).arg(button));
Gus Grubba's avatar
Gus Grubba committed
821
        }
Gus Grubba's avatar
Gus Grubba committed
822 823
    } else {
        if(!_buttonActionArray[button]) {
Gus Grubba's avatar
Gus Grubba committed
824 825 826 827 828 829 830 831 832 833 834
            _buttonActionArray[button] = new AssignedButtonAction(this, action);
        } else {
            _buttonActionArray[button]->action = action;
            //-- Make sure repeat is off if this action doesn't support repeats
            int idx = _findAssignableButtonAction(action);
            if(idx >= 0) {
                AssignableButtonAction* p = qobject_cast<AssignableButtonAction*>(_assignableButtonActions[idx]);
                if(!p->canRepeat()) {
                    _buttonActionArray[button]->repeat = false;
                }
            }
Gus Grubba's avatar
Gus Grubba committed
835
        }
Gus Grubba's avatar
Gus Grubba committed
836 837 838
        //-- Save to settings
        settings.setValue(QString(_buttonActionNameKey).arg(button),   _buttonActionArray[button]->action);
        settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat);
Gus Grubba's avatar
Gus Grubba committed
839
    }
Gus Grubba's avatar
Gus Grubba committed
840
    emit buttonActionsChanged();
841 842
}

Don Gagne's avatar
Don Gagne committed
843
QString Joystick::getButtonAction(int button)
844
{
Gus Grubba's avatar
Gus Grubba committed
845 846 847 848
    if (_validButton(button)) {
        if(_buttonActionArray[button]) {
            return _buttonActionArray[button]->action;
        }
Gus Grubba's avatar
Gus Grubba committed
849
    }
Gus Grubba's avatar
Gus Grubba committed
850
    return QString(_buttonActionNone);
851 852
}

Gus Grubba's avatar
Gus Grubba committed
853
QStringList Joystick::buttonActions()
854
{
Gus Grubba's avatar
Gus Grubba committed
855 856 857
    QStringList list;
    for (int button = 0; button < _totalButtonCount; button++) {
        list << getButtonAction(button);
858 859 860 861
    }
    return list;
}

Gus Grubba's avatar
Gus Grubba committed
862
int Joystick::throttleMode()
863 864 865 866 867 868 869 870 871 872
{
    return _throttleMode;
}

void Joystick::setThrottleMode(int mode)
{
    if (mode < 0 || mode >= ThrottleModeMax) {
        qCWarning(JoystickLog) << "Invalid throttle mode" << mode;
        return;
    }
Gus Grubba's avatar
Gus Grubba committed
873
    _throttleMode = static_cast<ThrottleMode_t>(mode);
874 875 876
    if (_throttleMode == ThrottleModeDownZero) {
        setAccumulator(false);
    }
877 878 879 880
    _saveSettings();
    emit throttleModeChanged(_throttleMode);
}

Gus Grubba's avatar
Gus Grubba committed
881
bool Joystick::negativeThrust()
882 883 884 885 886 887 888 889 890 891 892 893 894 895
{
    return _negativeThrust;
}

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

Gus Grubba's avatar
Gus Grubba committed
896
float Joystick::exponential()
897 898 899 900
{
    return _exponential;
}

901
void Joystick::setExponential(float expo)
902 903 904 905 906 907
{
    _exponential = expo;
    _saveSettings();
    emit exponentialChanged(_exponential);
}

Gus Grubba's avatar
Gus Grubba committed
908
bool Joystick::accumulator()
909 910 911 912 913 914 915 916 917 918 919
{
    return _accumulator;
}

void Joystick::setAccumulator(bool accu)
{
    _accumulator = accu;
    _saveSettings();
    emit accumulatorChanged(_accumulator);
}

Gus Grubba's avatar
Gus Grubba committed
920
bool Joystick::deadband()
Gregory Dymarek's avatar
Gregory Dymarek committed
921 922 923 924 925 926 927 928 929 930
{
    return _deadband;
}

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

Gus Grubba's avatar
Gus Grubba committed
931
bool Joystick::circleCorrection()
932 933 934 935 936 937 938 939 940 941 942
{
    return _circleCorrection;
}

void Joystick::setCircleCorrection(bool circleCorrection)
{
    _circleCorrection = circleCorrection;
    _saveSettings();
    emit circleCorrectionChanged(_circleCorrection);
}

Gus Grubba's avatar
Gus Grubba committed
943 944 945 946 947 948 949
void Joystick::setGimbalEnabled(bool set)
{
    _gimbalEnabled = set;
    _saveSettings();
    emit gimbalEnabledChanged();
}

Gus Grubba's avatar
Gus Grubba committed
950 951 952 953 954 955 956 957 958 959 960
void Joystick::setAxisFrequency(float val)
{
    //-- Arbitrary limits
    if(val < 0.25f) val = 0.25f;
    if(val > 50.0f) val = 50.0f;
    _axisFrequency = val;
    _saveSettings();
    emit axisFrequencyChanged();
}

void Joystick::setButtonFrequency(float val)
961 962
{
    //-- Arbitrary limits
Gus Grubba's avatar
Gus Grubba committed
963 964
    if(val < 0.25f) val = 0.25f;
    if(val > 50.0f) val = 50.0f;
Gus Grubba's avatar
Gus Grubba committed
965
    _buttonFrequency = val;
966
    _saveSettings();
Gus Grubba's avatar
Gus Grubba committed
967
    emit buttonFrequencyChanged();
968 969
}

970
void Joystick::setCalibrationMode(bool calibrating)
971
{
972 973
    _calibrationMode = calibrating;
    if (calibrating && !isRunning()) {
974
        _pollingStartedForCalibration = true;
975
        startPolling(_multiVehicleManager->activeVehicle());
976
    }
977 978
    else if (_pollingStartedForCalibration) {
        stopPolling();
Don Gagne's avatar
Don Gagne committed
979
    }
980 981
}

982

983
void Joystick::_executeButtonAction(const QString& action, bool buttonDown)
Don Gagne's avatar
Don Gagne committed
984
{
Gus Grubba's avatar
Gus Grubba committed
985
    if (!_activeVehicle || !_activeVehicle->joystickEnabled() || action == _buttonActionNone) {
986 987
        return;
    }
988
    if (action == _buttonActionArm) {
989
        if (buttonDown) emit setArmed(true);
990
    } else if (action == _buttonActionDisarm) {
991
        if (buttonDown) emit setArmed(false);
Gus Grubba's avatar
Gus Grubba committed
992
    } else if (action == _buttonActionToggleArm) {
993
        if (buttonDown) emit setArmed(!_activeVehicle->armed());
994
    } else if (action == _buttonActionVTOLFixedWing) {
995
        if (buttonDown) emit setVtolInFwdFlight(true);
996
    } else if (action == _buttonActionVTOLMultiRotor) {
997
        if (buttonDown) emit setVtolInFwdFlight(false);
998
    } else if (_activeVehicle->flightModes().contains(action) || _activeVehicle->extraJoystickFlightModes().contains(action)) {
999
        if (buttonDown) emit setFlightMode(action);
Gus Grubba's avatar
Gus Grubba committed
1000
    } else if(action == _buttonActionContinuousZoomIn || action == _buttonActionContinuousZoomOut) {
1001 1002 1003 1004 1005
        if (buttonDown) {
            emit startContinuousZoom(action == _buttonActionContinuousZoomIn ? 1 : -1);
        } else {
            emit stopContinuousZoom();
        }
Gus Grubba's avatar
Gus Grubba committed
1006
    } else if(action == _buttonActionStepZoomIn || action == _buttonActionStepZoomOut) {
1007
        if (buttonDown) emit stepZoom(action == _buttonActionStepZoomIn ? 1 : -1);
1008
    } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) {
1009
        if (buttonDown) emit stepStream(action == _buttonActionNextStream ? 1 : -1);
1010
    } else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) {
1011
        if (buttonDown) emit stepCamera(action == _buttonActionNextCamera ? 1 : -1);
Gus Grubba's avatar
Gus Grubba committed
1012
    } else if(action == _buttonActionTriggerCamera) {
1013
        if (buttonDown) emit triggerCamera();
Gus Grubba's avatar
Gus Grubba committed
1014
    } else if(action == _buttonActionStartVideoRecord) {
1015
        if (buttonDown) emit startVideoRecord();
Gus Grubba's avatar
Gus Grubba committed
1016
    } else if(action == _buttonActionStopVideoRecord) {
1017
        if (buttonDown) emit stopVideoRecord();
Gus Grubba's avatar
Gus Grubba committed
1018
    } else if(action == _buttonActionToggleVideoRecord) {
1019
        if (buttonDown) emit toggleVideoRecord();
Gus Grubba's avatar
Gus Grubba committed
1020
    } else if(action == _buttonActionGimbalUp) {
1021
        if (buttonDown) _pitchStep(1);
Gus Grubba's avatar
Gus Grubba committed
1022
    } else if(action == _buttonActionGimbalDown) {
1023
        if (buttonDown) _pitchStep(-1);
Gus Grubba's avatar
Gus Grubba committed
1024
    } else if(action == _buttonActionGimbalLeft) {
1025
        if (buttonDown) _yawStep(-1);
Gus Grubba's avatar
Gus Grubba committed
1026
    } else if(action == _buttonActionGimbalRight) {
1027
        if (buttonDown) _yawStep(1);
Gus Grubba's avatar
Gus Grubba committed
1028
    } else if(action == _buttonActionGimbalCenter) {
1029 1030 1031 1032 1033
        if (buttonDown) {
            _localPitch = 0.0;
            _localYaw   = 0.0;
            emit gimbalControlValue(0.0, 0.0);
        }
1034 1035
    } else if(action == _buttonActionEmergencyStop) {
      if(buttonDown) emit emergencyStop();
Don Gagne's avatar
Don Gagne committed
1036 1037 1038 1039 1040
    } else {
        qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
    }
}

1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057
void Joystick::_pitchStep(int direction)
{
    _localPitch += static_cast<double>(direction);
    //-- Arbitrary range
    if(_localPitch < -90.0) _localPitch = -90.0;
    if(_localPitch >  35.0) _localPitch =  35.0;
    emit gimbalControlValue(_localPitch, _localYaw);
}

void Joystick::_yawStep(int direction)
{
    _localYaw += static_cast<double>(direction);
    if(_localYaw < -180.0) _localYaw = -180.0;
    if(_localYaw >  180.0) _localYaw =  180.0;
    emit gimbalControlValue(_localPitch, _localYaw);
}

1058 1059
bool Joystick::_validAxis(int axis)
{
Gus Grubba's avatar
Gus Grubba committed
1060 1061 1062 1063 1064
    if(axis >= 0 && axis < _axisCount) {
        return true;
    }
    qCWarning(JoystickLog) << "Invalid axis index" << axis;
    return false;
1065 1066 1067 1068
}

bool Joystick::_validButton(int button)
{
Gus Grubba's avatar
Gus Grubba committed
1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082
    if(button >= 0 && button < _totalButtonCount)
        return true;
    qCWarning(JoystickLog) << "Invalid button index" << button;
    return false;
}

int Joystick::_findAssignableButtonAction(const QString& action)
{
    for(int i = 0; i < _assignableButtonActions.count(); i++) {
        AssignableButtonAction* p = qobject_cast<AssignableButtonAction*>(_assignableButtonActions[i]);
        if(p->action() == action)
            return i;
    }
    return -1;
1083 1084
}

1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099
void Joystick::_buildActionList(Vehicle* activeVehicle)
{
    if(_assignableButtonActions.count())
        _assignableButtonActions.clearAndDeleteContents();
    _availableActionTitles.clear();
    //-- Available Actions
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNone));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionArm));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionDisarm));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleArm));
    if (activeVehicle) {
        QStringList list = activeVehicle->flightModes();
        foreach(auto mode, list) {
            _assignableButtonActions.append(new AssignableButtonAction(this, mode));
        }
1100 1101 1102 1103
        list = activeVehicle->extraJoystickFlightModes();
        foreach(auto mode, list) {
            _assignableButtonActions.append(new AssignableButtonAction(this, mode));
        }
1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123
    }
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLFixedWing));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLMultiRotor));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomIn, true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomOut, true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomIn,  true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomOut, true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextStream));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousStream));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextCamera));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousCamera));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionTriggerCamera));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStartVideoRecord));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStopVideoRecord));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleVideoRecord));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalDown,    true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalUp,      true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalLeft,    true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalRight,   true));
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalCenter));
1124
    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop));
1125 1126 1127 1128 1129 1130
    for(int i = 0; i < _assignableButtonActions.count(); i++) {
        AssignableButtonAction* p = qobject_cast<AssignableButtonAction*>(_assignableButtonActions[i]);
        _availableActionTitles << p->action();
    }
    emit assignableActionsChanged();
}