Joystick.cc 18.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

#include "Joystick.h"
#include "QGC.h"
#include "MultiVehicleManager.h"
#include "AutoPilotPlugin.h"
#include "UAS.h"

#include <QSettings>

32 33 34 35 36 37
#ifndef __mobile__
    #ifdef Q_OS_MAC
        #include <SDL.h>
    #else
        #include <SDL/SDL.h>
    #endif
38 39 40
#endif

QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog")
41
QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog")
42 43 44

const char* Joystick::_settingsGroup =              "Joysticks";
const char* Joystick::_calibratedSettingsKey =      "Calibrated";
Don Gagne's avatar
Don Gagne committed
45
const char* Joystick::_buttonActionSettingsKey =    "ButtonActionName%1";
46 47 48 49 50 51 52 53 54 55
const char* Joystick::_throttleModeSettingsKey =    "ThrottleMode";

const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
    "RollAxis",
    "PitchAxis",
    "YawAxis",
    "ThrottleAxis"
};

Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlIndex)
56
#ifndef __mobile__
57 58 59 60 61
    : _sdlIndex(sdlIndex)
    , _exitThread(false)
    , _name(name)
    , _axisCount(axisCount)
    , _buttonCount(buttonCount)
Don Gagne's avatar
Don Gagne committed
62
    , _calibrationMode(CalibrationModeOff)
63 64 65 66
    , _rgAxisValues(NULL)
    , _rgCalibration(NULL)
    , _rgButtonValues(NULL)
    , _rgButtonActions(NULL)
67 68
    , _lastButtonBits(0)
    , _throttleMode(ThrottleModeCenterZero)
69 70
    , _activeVehicle(NULL)
    , _pollingStartedForCalibration(false)
71
#endif // __mobile__
72
{
73 74 75 76 77 78
#ifdef __mobile__
    Q_UNUSED(name)
    Q_UNUSED(axisCount)
    Q_UNUSED(buttonCount)
    Q_UNUSED(sdlIndex)
#else
79 80 81 82 83 84
    _rgAxisValues = new int[_axisCount];
    _rgCalibration = new Calibration_t[_axisCount];
    _rgButtonValues = new bool[_buttonCount];
    _rgButtonActions = new QString[_buttonCount];

    for (int i=0; i<_axisCount; i++) {
85 86
        _rgAxisValues[i] = 0;
    }
87
    for (int i=0; i<_buttonCount; i++) {
88 89 90 91
        _rgButtonValues[i] = false;
    }
    
    _loadSettings();
92
#endif // __mobile __
93 94 95 96
}

Joystick::~Joystick()
{
97
#ifndef __mobile__
98 99 100 101
    delete _rgAxisValues;
    delete _rgCalibration;
    delete _rgButtonValues;
    delete _rgButtonActions;
102
#endif
103 104
}

105 106
#ifndef __mobile__

107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123
void Joystick::_loadSettings(void)
{
    QSettings   settings;
    
    settings.beginGroup(_settingsGroup);
    settings.beginGroup(_name);
    
    bool badSettings = false;
    bool convertOk;
    
    qCDebug(JoystickLog) << "_loadSettings " << _name;
    
    _calibrated = settings.value(_calibratedSettingsKey, false).toBool();
    
    _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk);
    badSettings |= !convertOk;
    
124
    qCDebug(JoystickLog) << "_loadSettings calibrated:throttlemode:badsettings" << _calibrated << _throttleMode << badSettings;
125 126 127 128 129 130
    
    QString minTpl  ("Axis%1Min");
    QString maxTpl  ("Axis%1Max");
    QString trimTpl ("Axis%1Trim");
    QString revTpl  ("Axis%1Rev");
    
131
    for (int axis=0; axis<_axisCount; axis++) {
132 133 134 135 136 137 138 139
        Calibration_t* calibration = &_rgCalibration[axis];
        
        calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk);
        badSettings |= !convertOk;
        
        calibration->min = settings.value(minTpl.arg(axis), -32768).toInt(&convertOk);
        badSettings |= !convertOk;
        
140
        calibration->max = settings.value(maxTpl.arg(axis), 32767).toInt(&convertOk);
141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158
        badSettings |= !convertOk;
        
        calibration->reversed = settings.value(revTpl.arg(axis), false).toBool();
        
        qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << badSettings;
    }
    
    for (int function=0; function<maxFunction; function++) {
        int functionAxis;
        
        functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk);
        badSettings |= !convertOk || (functionAxis == -1);
        
        _rgFunctionAxis[function] = functionAxis;
        
        qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings;
    }
    
159
    for (int button=0; button<_buttonCount; button++) {
Don Gagne's avatar
Don Gagne committed
160 161
        _rgButtonActions[button] = settings.value(QString(_buttonActionSettingsKey).arg(button), QString()).toString();        
        qCDebug(JoystickLog) << "_loadSettings button:action" << button << _rgButtonActions[button];
162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
    }
    
    if (badSettings) {
        _calibrated = false;
        settings.setValue(_calibratedSettingsKey, false);
    }
}

void Joystick::_saveSettings(void)
{
    QSettings settings;
    
    settings.beginGroup(_settingsGroup);
    settings.beginGroup(_name);
    
    settings.setValue(_calibratedSettingsKey, _calibrated);
    settings.setValue(_throttleModeSettingsKey, _throttleMode);
    
180
    qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode" << _calibrated << _throttleMode;
181 182 183 184 185 186

    QString minTpl  ("Axis%1Min");
    QString maxTpl  ("Axis%1Max");
    QString trimTpl ("Axis%1Trim");
    QString revTpl  ("Axis%1Rev");
    
187
    for (int axis=0; axis<_axisCount; axis++) {
188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208
        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);
        
        qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed"
                                << _name
                                << axis
                                << calibration->min
                                << calibration->max
                                << calibration->center
                                << calibration->reversed;
    }
    
    for (int function=0; function<maxFunction; function++) {
        settings.setValue(_rgFunctionSettingsKey[function], _rgFunctionAxis[function]);
        qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function];
    }
    
209
    for (int button=0; button<_buttonCount; button++) {
210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252
        settings.setValue(QString(_buttonActionSettingsKey).arg(button), _rgButtonActions[button]);
        qCDebug(JoystickLog) << "_saveSettings button:action" << button << _rgButtonActions[button];
    }
}

/// Adjust the raw axis value to the -1:1 range given calibration information
float Joystick::_adjustRange(int value, Calibration_t calibration)
{
    float valueNormalized;
    float axisLength;
    float axisBasis;
    
    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;
    }
    
    float axisPercent = valueNormalized / axisLength;
    
    float correctedValue = axisBasis * axisPercent;
    
    if (calibration.reversed) {
        correctedValue *= -1.0f;
    }
    
#if 0
    qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:basis:normalized:length"
                            << correctedValue
                            << value
                            << calibration.min
                            << calibration.max
                            << calibration.center
                            << calibration.center
                            << axisBasis
                            << valueNormalized
                            << axisLength;
#endif

253
    return std::max(-1.0f, std::min(correctedValue, 1.0f));
254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285
}


void Joystick::run(void)
{
    SDL_Joystick* sdlJoystick = SDL_JoystickOpen(_sdlIndex);
    
    if (!sdlJoystick) {
        qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError();
        return;
    }
    
    while (!_exitThread) {
        SDL_JoystickUpdate();

        // Update axes
        for (int axisIndex=0; axisIndex<_axisCount; axisIndex++) {
            int newAxisValue = SDL_JoystickGetAxis(sdlJoystick, axisIndex);
            // Calibration code requires signal to be emitted even if value hasn't changed
            _rgAxisValues[axisIndex] = newAxisValue;
            emit rawAxisValueChanged(axisIndex, newAxisValue);
        }
        
        // Update buttons
        for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) {
            bool newButtonValue = !!SDL_JoystickGetButton(sdlJoystick, buttonIndex);
            if (newButtonValue != _rgButtonValues[buttonIndex]) {
                _rgButtonValues[buttonIndex] = newButtonValue;
                emit rawButtonPressedChanged(buttonIndex, newButtonValue);
            }
        }
        
Don Gagne's avatar
Don Gagne committed
286
        if (_calibrationMode != CalibrationModeCalibrating) {
287 288 289 290 291 292 293 294
            int     axis = _rgFunctionAxis[rollFunction];
            float   roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
            
                    axis = _rgFunctionAxis[pitchFunction];
            float   pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
            
                    axis = _rgFunctionAxis[yawFunction];
            float   yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
295

296 297 298
                    axis = _rgFunctionAxis[throttleFunction];
            float   throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);

299 300 301 302 303
            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)));

304
            // Map from unit circle to linear range and limit
305 306 307 308
            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));
309 310 311
            
            // Adjust throttle to 0:1 range
            if (_throttleMode == ThrottleModeCenterZero) {
312
                throttle = std::max(0.0f, throttle);
313 314 315 316 317 318 319
            } else {                
                throttle = (throttle + 1.0f) / 2.0f;
            }
            
            // Set up button pressed information
            
            // We only send the buttons the firmwware has reserved
320
            int reservedButtonCount = _activeVehicle->manualControlReservedButtonCount();
321 322 323 324 325 326 327 328 329 330
            if (reservedButtonCount == -1) {
                reservedButtonCount = _buttonCount;
            }
            
            quint16 newButtonBits = 0;      // New set of button which are down
            quint16 buttonPressedBits = 0;  // Buttons pressed for manualControl signal
            
            for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) {
                quint16 buttonBit = 1 << buttonIndex;
                
331 332
                if (!_rgButtonValues[buttonIndex]) {
                    // Button up, just record it
333 334 335
                    newButtonBits |= buttonBit;
                } else {
                    if (_lastButtonBits & buttonBit) {
336
                        // Button was up last time through, but is now down which indicates a button press
337 338 339 340
                        qCDebug(JoystickLog) << "button triggered" << buttonIndex;
                        
                        if (buttonIndex >= reservedButtonCount) {
                            // Button is above firmware reserved set
Don Gagne's avatar
Don Gagne committed
341 342 343
                            QString buttonAction =_rgButtonActions[buttonIndex];
                            if (!buttonAction.isEmpty()) {
                                _buttonAction(buttonAction);
344 345 346
                            }
                        }
                    }
347 348 349

                    // Mark the button as pressed as long as its pressed
                    buttonPressedBits |= buttonBit;
350 351 352 353 354
                }
            }
            
            _lastButtonBits = newButtonBits;
            
355
            qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle" << name() << roll << -pitch << yaw << throttle;
356
            
357
            emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode());
358 359 360 361 362 363 364 365 366
        }
        
        // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms)
        QGC::SLEEP::msleep(40);
    }
    
    SDL_JoystickClose(sdlJoystick);
}

367
void Joystick::startPolling(Vehicle* vehicle)
368
{
369
    if (isRunning()) {
Don Gagne's avatar
Don Gagne committed
370
        if (vehicle != _activeVehicle) {
371
            // Joystick was previously disabled, but now enabled from config screen
Don Gagne's avatar
Don Gagne committed
372 373 374 375 376 377
            
            if (_calibrationMode == CalibrationModeOff) {
                qWarning() << "Incorrect usage pattern";
                return;
            }
            
378 379 380 381 382 383 384
            _activeVehicle = vehicle;
            _pollingStartedForCalibration = false;
        }
    } else {
        _activeVehicle = vehicle;
        
        UAS* uas = _activeVehicle->uas();
385 386
        
        connect(this, &Joystick::manualControl,         uas, &UAS::setExternalControlSetpoint);
Don Gagne's avatar
Don Gagne committed
387 388
        // FIXME: ****
        //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
389 390 391
        
        _exitThread = false;
        start();
392 393 394 395 396
    }
}

void Joystick::stopPolling(void)
{
397 398 399 400
    if (isRunning()) {
        UAS* uas = _activeVehicle->uas();
        
        disconnect(this, &Joystick::manualControl,          uas, &UAS::setExternalControlSetpoint);
Don Gagne's avatar
Don Gagne committed
401 402
        // FIXME: ****
        //disconnect(this, &Joystick::buttonActionTriggered,  uas, &UAS::triggerAction);
403 404 405
        
        _exitThread = true;
        }
406 407 408 409
}

void Joystick::setCalibration(int axis, Calibration_t& calibration)
{
410
    if (!_validAxis(axis)) {
411 412 413 414 415 416 417 418 419 420 421 422
        qCWarning(JoystickLog) << "Invalid axis index" << axis;
        return;
    }
    
    _calibrated = true;
    _rgCalibration[axis] = calibration;
    _saveSettings();
    emit calibratedChanged(_calibrated);
}

Joystick::Calibration_t Joystick::getCalibration(int axis)
{
423
    if (!_validAxis(axis)) {
424 425 426 427 428 429 430 431
        qCWarning(JoystickLog) << "Invalid axis index" << axis;
    }
    
    return _rgCalibration[axis];
}

void Joystick::setFunctionAxis(AxisFunction_t function, int axis)
{
432
    if (!_validAxis(axis)) {
433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454
        qCWarning(JoystickLog) << "Invalid axis index" << axis;
        return;
    }

    _calibrated = true;
    _rgFunctionAxis[function] = axis;
    _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;
Don Gagne's avatar
Don Gagne committed
455 456

    list << "Arm" << "Disarm";
457 458 459 460
    
    return list;
}

Don Gagne's avatar
Don Gagne committed
461
void Joystick::setButtonAction(int button, const QString& action)
462
{
463
    if (!_validButton(button)) {
464 465 466 467
        qCWarning(JoystickLog) << "Invalid button index" << button;
        return;
    }
    
Don Gagne's avatar
Don Gagne committed
468 469
    qDebug() << "setButtonAction" << action;
    
470 471 472 473 474
    _rgButtonActions[button] = action;
    _saveSettings();
    emit buttonActionsChanged(buttonActions());
}

Don Gagne's avatar
Don Gagne committed
475
QString Joystick::getButtonAction(int button)
476
{
477
    if (!_validButton(button)) {
478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511
        qCWarning(JoystickLog) << "Invalid button index" << button;
    }
    
    return _rgButtonActions[button];
}

QVariantList Joystick::buttonActions(void)
{
    QVariantList list;
    
    for (int button=0; button<_buttonCount; button++) {
        list += QVariant::fromValue(_rgButtonActions[button]);
    }
    
    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;
    }
    
    _throttleMode = (ThrottleMode_t)mode;
    _saveSettings();
    emit throttleModeChanged(_throttleMode);
}

Don Gagne's avatar
Don Gagne committed
512
void Joystick::startCalibrationMode(CalibrationMode_t mode)
513
{
Don Gagne's avatar
Don Gagne committed
514 515 516 517 518 519
    if (mode == CalibrationModeOff) {
        qWarning() << "Incorrect mode CalibrationModeOff";
        return;
    }
    
    _calibrationMode = mode;
520
    
521 522 523 524
    if (!isRunning()) {
        _pollingStartedForCalibration = true;
        startPolling(MultiVehicleManager::instance()->activeVehicle());
    }
525 526
}

Don Gagne's avatar
Don Gagne committed
527
void Joystick::stopCalibrationMode(CalibrationMode_t mode)
528
{
Don Gagne's avatar
Don Gagne committed
529 530 531 532 533 534 535 536 537
    if (mode == CalibrationModeOff) {
        qWarning() << "Incorrect mode: CalibrationModeOff";
        return;
    }
    
    if (mode == CalibrationModeCalibrating) {
        _calibrationMode = CalibrationModeMonitor;
    } else {
        _calibrationMode = CalibrationModeOff;
538 539 540
        if (_pollingStartedForCalibration) {
            stopPolling();
        }
541 542
    }
}
543

Don Gagne's avatar
Don Gagne committed
544 545 546 547 548 549 550 551 552 553 554
void Joystick::_buttonAction(const QString& action)
{
    if (action == "Arm") {
        _activeVehicle->setArmed(true);
    } else if (action == "Disarm") {
        _activeVehicle->setArmed(false);
    } else {
        qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
    }
}

555 556 557 558 559 560 561 562 563 564
bool Joystick::_validAxis(int axis)
{
    return axis >= 0 && axis < _axisCount;
}

bool Joystick::_validButton(int button)
{
    return button >= 0 && button < _buttonCount;
}

565
#endif // __mobile__