JoystickConfigController.cc 30 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

#include "JoystickConfigController.h"
#include "JoystickManager.h"
13
#include "QGCApplication.h"
14 15 16 17 18 19 20 21

#include <QSettings>

QGC_LOGGING_CATEGORY(JoystickConfigControllerLog, "JoystickConfigControllerLog")

const int JoystickConfigController::_updateInterval =       150;        ///< Interval for timer which updates radio channel widgets
const int JoystickConfigController::_calCenterPoint =       0;
const int JoystickConfigController::_calValidMinValue =     -32768;     ///< Largest valid minimum axis value
22
const int JoystickConfigController::_calValidMaxValue =     32767;      ///< Smallest valid maximum axis value
23
const int JoystickConfigController::_calDefaultMinValue =   -32768;     ///< Default value for Min if not set
24
const int JoystickConfigController::_calDefaultMaxValue =   32767;      ///< Default value for Max if not set
25 26
const int JoystickConfigController::_calRoughCenterDelta =  500;        ///< Delta around center point which is considered to be roughly centered
const int JoystickConfigController::_calMoveDelta =         32768/2;    ///< Amount of delta past center which is considered stick movement
27
const int JoystickConfigController::_calSettleDelta =       600;        ///< Amount of delta which is considered no stick movement
28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
const int JoystickConfigController::_calMinDelta =          1000;       ///< Amount of delta allowed around min value to consider channel at min

const int JoystickConfigController::_stickDetectSettleMSecs = 500;

const char*  JoystickConfigController::_imageFilePrefix =   "calibration/";
const char*  JoystickConfigController::_imageFileMode2Dir = "joystick/";
const char*  JoystickConfigController::_imageCenter =       "joystickCenter.png";
const char*  JoystickConfigController::_imageThrottleUp =   "joystickThrottleUp.png";
const char*  JoystickConfigController::_imageThrottleDown = "joystickThrottleDown.png";
const char*  JoystickConfigController::_imageYawLeft =      "joystickYawLeft.png";
const char*  JoystickConfigController::_imageYawRight =     "joystickYawRight.png";
const char*  JoystickConfigController::_imageRollLeft =     "joystickRollLeft.png";
const char*  JoystickConfigController::_imageRollRight =    "joystickRollRight.png";
const char*  JoystickConfigController::_imagePitchUp =      "joystickPitchUp.png";
const char*  JoystickConfigController::_imagePitchDown =    "joystickPitchDown.png";

44 45 46 47
JoystickConfigController::JoystickConfigController(void)
    : _activeJoystick(NULL)
    , _currentStep(-1)
    , _axisCount(0)
48 49 50
    , _rgAxisInfo(NULL)
    , _axisValueSave(NULL)
    , _axisRawValue(NULL)
51 52 53 54 55
    , _calState(calStateAxisWait)
    , _statusText(NULL)
    , _cancelButton(NULL)
    , _nextButton(NULL)
    , _skipButton(NULL)
56
    , _joystickManager(qgcApp()->toolbox()->joystickManager())
57 58
{
    
59
    connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &JoystickConfigController::_activeJoystickChanged);
60
    
61
    _activeJoystickChanged(_joystickManager->activeJoystick());
62 63 64 65 66 67 68 69 70 71
    _resetInternalCalibrationValues();
}

void JoystickConfigController::start(void)
{
    _stopCalibration();
}

JoystickConfigController::~JoystickConfigController()
{
Don Gagne's avatar
Don Gagne committed
72
    _activeJoystick->stopCalibrationMode(Joystick::CalibrationModeMonitor);
73 74 75 76 77 78 79 80 81 82 83 84 85 86 87
}

/// @brief Returns the state machine entry for the specified state.
const JoystickConfigController::stateMachineEntry* JoystickConfigController::_getStateMachineEntry(int step)
{
    static const char* msgBegin =           "Allow all sticks to center as shown in diagram.\n\nClick Next to continue";
    static const char* msgThrottleUp =      "Move the Throttle stick all the way up and hold it there...";
    static const char* msgThrottleDown =    "Move the Throttle stick all the way down and hold it there...";
    static const char* msgYawLeft =         "Move the Yaw stick all the way to the left and hold it there...";
    static const char* msgYawRight =        "Move the Yaw stick all the way to the right and hold it there...";
    static const char* msgRollLeft =        "Move the Roll stick all the way to the left and hold it there...";
    static const char* msgRollRight =       "Move the Roll stick all the way to the right and hold it there...";
    static const char* msgPitchDown =       "Move the Pitch stick all the way down and hold it there...";
    static const char* msgPitchUp =         "Move the Pitch stick all the way up and hold it there...";
    static const char* msgPitchCenter =     "Allow the Pitch stick to move back to center...";
88
    static const char* msgComplete =        "All settings have been captured. Click Next to enable the joystick.";
89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125
    
    static const stateMachineEntry rgStateMachine[] = {
        //Function
        { Joystick::maxFunction,       msgBegin,           _imageCenter,       &JoystickConfigController::_inputCenterWaitBegin,   &JoystickConfigController::_saveAllTrims,       NULL },
        { Joystick::throttleFunction,  msgThrottleUp,      _imageThrottleUp,   &JoystickConfigController::_inputStickDetect,       NULL,                                           NULL },
        { Joystick::throttleFunction,  msgThrottleDown,    _imageThrottleDown, &JoystickConfigController::_inputStickMin,          NULL,                                           NULL },
        { Joystick::yawFunction,       msgYawRight,        _imageYawRight,     &JoystickConfigController::_inputStickDetect,       NULL,                                           NULL },
        { Joystick::yawFunction,       msgYawLeft,         _imageYawLeft,      &JoystickConfigController::_inputStickMin,          NULL,                                           NULL },
        { Joystick::rollFunction,      msgRollRight,       _imageRollRight,    &JoystickConfigController::_inputStickDetect,       NULL,                                           NULL },
        { Joystick::rollFunction,      msgRollLeft,        _imageRollLeft,     &JoystickConfigController::_inputStickMin,          NULL,                                           NULL },
        { Joystick::pitchFunction,     msgPitchUp,         _imagePitchUp,      &JoystickConfigController::_inputStickDetect,       NULL,                                           NULL },
        { Joystick::pitchFunction,     msgPitchDown,       _imagePitchDown,    &JoystickConfigController::_inputStickMin,          NULL,                                           NULL },
        { Joystick::pitchFunction,     msgPitchCenter,     _imageCenter,       &JoystickConfigController::_inputCenterWait,        NULL,                                           NULL },
        { Joystick::maxFunction,       msgComplete,        _imageCenter,       NULL,                                               &JoystickConfigController::_writeCalibration,   NULL },
    };
    
    Q_ASSERT(step >=0 && step < (int)(sizeof(rgStateMachine) / sizeof(rgStateMachine[0])));
    
    return &rgStateMachine[step];
}

void JoystickConfigController::_advanceState(void)
{
    _currentStep++;
    _setupCurrentState();
}


/// @brief Sets up the state machine according to the current step from _currentStep.
void JoystickConfigController::_setupCurrentState(void)
{
   const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
    
    _statusText->setProperty("text", state->instructions);
    
    _setHelpImage(state->image);
    
126
    _stickDetectAxis = _axisNoAxis;
127 128 129 130 131 132 133 134 135 136
    _stickDetectSettleStarted = false;
    
    _calSaveCurrentValues();
    
    _nextButton->setEnabled(state->nextFn != NULL);
    _skipButton->setEnabled(state->skipFn != NULL);
}

void JoystickConfigController::_axisValueChanged(int axis, int value)
{
137
    if (_validAxis(axis)) {
138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185
        // We always update raw values
        _axisRawValue[axis] = value;
        emit axisValueChanged(axis, _axisRawValue[axis]);
        
        // Signal attitude axis values to Qml if mapped
        if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
            switch (_rgAxisInfo[axis].function) {
                case Joystick::rollFunction:
                    emit rollAxisValueChanged(_axisRawValue[axis]);
                    break;
                case Joystick::pitchFunction:
                    emit pitchAxisValueChanged(_axisRawValue[axis]);
                    break;
                case Joystick::yawFunction:
                    emit yawAxisValueChanged(_axisRawValue[axis]);
                    break;
                case Joystick::throttleFunction:
                    emit throttleAxisValueChanged(_axisRawValue[axis]);
                    break;
                default:
                    break;
            }
        }
            
        //qCDebug(JoystickConfigControllerLog) << "Raw value" << axis << value;
        
        if (_currentStep == -1) {
            // Track the axis count by keeping track of how many axes we see
            if (axis + 1 > (int)_axisCount) {
                _axisCount = axis + 1;
            }
        }
        
        if (_currentStep != -1) {
            const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
            Q_ASSERT(state);
            if (state->rcInputFn) {
                (this->*state->rcInputFn)(state->function, axis, value);
            }
        }
    }
}

void JoystickConfigController::nextButtonClicked(void)
{
    if (_currentStep == -1) {
        // Need to have enough channels
        if (_axisCount < _axisMinimum) {
186
            qgcApp()->showMessage(QString("Detected %1 joystick axes. To operate PX4, you need at least %2 axes.").arg(_axisCount).arg(_axisMinimum));
187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216
            return;
        }
        _startCalibration();
    } else {
        const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
        Q_ASSERT(state);
        Q_ASSERT(state->nextFn);
        (this->*state->nextFn)();
    }
}

void JoystickConfigController::skipButtonClicked(void)
{
    Q_ASSERT(_currentStep != -1);
    
    const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
    Q_ASSERT(state);
    Q_ASSERT(state->skipFn);
    (this->*state->skipFn)();
}

void JoystickConfigController::cancelButtonClicked(void)
{
    _stopCalibration();
}

void JoystickConfigController::_saveAllTrims(void)
{
    // We save all trims as the first step. At this point no axes are mapped but it should still
    // allow us to get good trims for the roll/pitch/yaw/throttle even though we don't know which
217
    // axis they are yet. As we continue through the process the other axes will get their
218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
    // trims reset to correct values.
    
    for (int i=0; i<_axisCount; i++) {
        qCDebug(JoystickConfigControllerLog) << "_saveAllTrims trim" << _axisRawValue[i];
        _rgAxisInfo[i].axisTrim = _axisRawValue[i];
    }
    _advanceState();
}

/// @brief Waits for the sticks to be centered, enabling Next when done.
void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t function, int axis, int value)
{
    Q_UNUSED(function);
    Q_UNUSED(axis);
    Q_UNUSED(value);
    
    // FIXME: Doesn't wait for center
    
    _nextButton->setEnabled(true);
}

Don Gagne's avatar
Don Gagne committed
239
bool JoystickConfigController::_stickSettleComplete(int axis, int value)
240
{
241 242 243 244 245
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return false;
    }

246 247 248 249 250
    // We are waiting for the stick to settle out to a max position
    
    if (abs(_stickDetectValue - value) > _calSettleDelta) {
        // Stick is moving too much to consider stopped
        
Don Gagne's avatar
Don Gagne committed
251
        qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
252 253 254 255 256 257 258 259 260 261 262

        _stickDetectValue = value;
        _stickDetectSettleStarted = false;
    } else {
        // Stick is still positioned within the specified small range
        
        if (_stickDetectSettleStarted) {
            // We have already started waiting
            
            if (_stickDetectSettleElapsed.elapsed() > _stickDetectSettleMSecs) {
                // Stick has stayed positioned in one place long enough, detection is complete.
Don Gagne's avatar
Don Gagne committed
263
                qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete detection complete, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
264 265 266 267 268
                return true;
            }
        } else {
            // Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs
            
Don Gagne's avatar
Don Gagne committed
269
            qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
270 271 272 273 274 275 276 277 278 279 280 281 282
            
            _stickDetectSettleStarted = true;
            _stickDetectSettleElapsed.start();
        }
    }
    
    return false;
}

void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t function, int axis, int value)
{
    qCDebug(JoystickConfigControllerLog) << "_inputStickDetect function:axis:value" << function << axis << value;
    
283 284 285 286 287
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

288 289 290 291 292
    // If this axis is already used in a mapping we can't use it again
    if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
        return;
    }
    
293
    if (_stickDetectAxis == _axisNoAxis) {
294 295 296 297 298 299 300 301 302 303 304 305 306
        // We have not detected enough movement on a axis yet
        
        if (abs(_axisValueSave[axis] - value) > _calMoveDelta) {
            // Stick has moved far enough to consider it as being selected for the function
            
            qCDebug(JoystickConfigControllerLog) << "_inputStickDetect starting settle wait, function:axis:value" << function << axis << value;
            
            // Setup up to detect stick being pegged to min or max value
            _stickDetectAxis = axis;
            _stickDetectInitialValue = value;
            _stickDetectValue = value;
        }
    } else if (axis == _stickDetectAxis) {
Don Gagne's avatar
Don Gagne committed
307
        if (_stickSettleComplete(axis, value)) {
308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323
            AxisInfo* info = &_rgAxisInfo[axis];
            
            // Stick detection is complete. Stick should be at max position.
            // Map the axis to the function
            _rgFunctionAxisMapping[function] = axis;
            info->function = function;
            
            // Axis should be at max value, if it is below initial set point the the axis is reversed.
            info->reversed = value < _axisValueSave[axis];
            
            if (info->reversed) {
                _rgAxisInfo[axis].axisMin = value;
            } else {
                _rgAxisInfo[axis].axisMax = value;
            }
            
Don Gagne's avatar
Don Gagne committed
324 325
            qCDebug(JoystickConfigControllerLog) << "_inputStickDetect saving values, function:axis:value:reversed:_axisValueSave" << function << axis << value << info->reversed << _axisValueSave[axis];
            
326 327 328 329 330 331 332 333 334
            _signalAllAttiudeValueChanges();
            
            _advanceState();
        }
    }
}

void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, int axis, int value)
{
Don Gagne's avatar
Don Gagne committed
335 336
    qCDebug(JoystickConfigControllerLog) << "_inputStickMin function:axis:value" << function << axis << value;
    
337 338 339 340 341
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

342 343 344 345 346
    // We only care about the axis mapped to the function we are working on
    if (_rgFunctionAxisMapping[function] != axis) {
        return;
    }

347
    if (_stickDetectAxis == _axisNoAxis) {
348 349 350 351 352 353
        // Setup up to detect stick being pegged to extreme position
        if (_rgAxisInfo[axis].reversed) {
            if (value > _calCenterPoint + _calMoveDelta) {
                _stickDetectAxis = axis;
                _stickDetectInitialValue = value;
                _stickDetectValue = value;
Don Gagne's avatar
Don Gagne committed
354
                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
355 356 357 358 359 360
            }
        } else {
            if (value < _calCenterPoint - _calMoveDelta) {
                _stickDetectAxis = axis;
                _stickDetectInitialValue = value;
                _stickDetectValue = value;
Don Gagne's avatar
Don Gagne committed
361
                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
362 363 364 365 366
            }
        }
    } else {
        // We are waiting for the selected axis to settle out
        
Don Gagne's avatar
Don Gagne committed
367
        if (_stickSettleComplete(axis, value)) {
368 369 370 371 372 373 374 375
            AxisInfo* info = &_rgAxisInfo[axis];
            
            // Stick detection is complete. Stick should be at min position.
            if (info->reversed) {
                _rgAxisInfo[axis].axisMax = value;
            } else {
                _rgAxisInfo[axis].axisMin = value;
            }
Don Gagne's avatar
Don Gagne committed
376 377
            
            qCDebug(JoystickConfigControllerLog) << "_inputStickMin saving values, function:axis:value:reversed" << function << axis << value << info->reversed;
378 379 380 381 382 383 384 385
            
            _advanceState();
        }
    }
}

void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t function, int axis, int value)
{
Don Gagne's avatar
Don Gagne committed
386 387
    qCDebug(JoystickConfigControllerLog) << "_inputCenterWait function:axis:value" << function << axis << value;
    
388 389 390 391 392
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

393 394 395 396 397
    // We only care about the axis mapped to the function we are working on
    if (_rgFunctionAxisMapping[function] != axis) {
        return;
    }
    
398
    if (_stickDetectAxis == _axisNoAxis) {
399 400 401 402 403 404 405
        // Sticks have not yet moved close enough to center
        
        if (abs(_calCenterPoint - value) < _calRoughCenterDelta) {
            // Stick has moved close enough to center that we can start waiting for it to settle
            _stickDetectAxis = axis;
            _stickDetectInitialValue = value;
            _stickDetectValue = value;
Don Gagne's avatar
Don Gagne committed
406
            qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected possible center _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
407 408
        }
    } else {
Don Gagne's avatar
Don Gagne committed
409
        if (_stickSettleComplete(axis, value)) {
410 411 412 413 414 415 416 417 418
            _advanceState();
        }
    }
}

/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
void JoystickConfigController::_resetInternalCalibrationValues(void)
{
    // Set all raw axiss to not reversed and center point values
419
    for (int i=0; i<_axisCount; i++) {
420 421 422 423 424 425 426 427 428 429
        struct AxisInfo* info = &_rgAxisInfo[i];
        info->function = Joystick::maxFunction;
        info->reversed = false;
        info->axisMin = JoystickConfigController::_calCenterPoint;
        info->axisMax = JoystickConfigController::_calCenterPoint;
        info->axisTrim = JoystickConfigController::_calCenterPoint;
    }
    
    // Initialize attitude function mapping to function axis not set
    for (size_t i=0; i<Joystick::maxFunction; i++) {
430
        _rgFunctionAxisMapping[i] = _axisNoAxis;
431 432 433 434 435 436 437 438
    }
    
    _signalAllAttiudeValueChanges();
}

/// @brief Sets internal calibration values from the stored settings
void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
{
439
    Joystick* joystick = _joystickManager->activeJoystick();
440 441 442
    
    // Initialize all function mappings to not set
    
443
    for (int i=0; i<_axisCount; i++) {
444 445 446 447 448
        struct AxisInfo* info = &_rgAxisInfo[i];
        info->function = Joystick::maxFunction;
    }
    
    for (size_t i=0; i<Joystick::maxFunction; i++) {
449
        _rgFunctionAxisMapping[i] = _axisNoAxis;
450 451
    }
    
452
    for (int axis=0; axis<_axisCount; axis++) {
453 454 455 456 457 458 459 460
        struct AxisInfo* info = &_rgAxisInfo[axis];
        
        Joystick::Calibration_t calibration = joystick->getCalibration(axis);
        info->axisTrim = calibration.center;
        info->axisMin = calibration.min;
        info->axisMax = calibration.max;
        info->reversed = calibration.reversed;
        
461
        qCDebug(JoystickConfigControllerLog) << "Read settings name:axis:min:max:trim:reversed" << joystick->name() << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed;
462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478
    }
    
    for (int function=0; function<Joystick::maxFunction; function++) {
        int paramAxis;
        
        paramAxis = joystick->getFunctionAxis((Joystick::AxisFunction_t)function);
        
        _rgFunctionAxisMapping[function] = paramAxis;
        _rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
    }
    
    _signalAllAttiudeValueChanges();
}

/// @brief Validates the current settings against the calibration rules resetting values as necessary.
void JoystickConfigController::_validateCalibration(void)
{
479
    for (int chan = 0; chan<_axisCount; chan++) {
480 481 482 483 484
        struct AxisInfo* info = &_rgAxisInfo[chan];
        
        if (chan < _axisCount) {
            // Validate Min/Max values. Although the axis appears as available we still may
            // not have good min/max/trim values for it. Set to defaults if needed.
485
            if (info->axisMin < _calValidMinValue || info->axisMax > _calValidMaxValue) {
486 487 488 489
                qCDebug(JoystickConfigControllerLog) << "_validateCalibration resetting axis" << chan;
                info->axisMin = _calDefaultMinValue;
                info->axisMax = _calDefaultMaxValue;
                info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507
            }
            
            switch (_rgAxisInfo[chan].function) {
                case Joystick::throttleFunction:
                case Joystick::yawFunction:
                case Joystick::rollFunction:
                case Joystick::pitchFunction:
                    // Make sure trim is within min/max
                    if (info->axisTrim < info->axisMin) {
                        info->axisTrim = info->axisMin;
                    } else if (info->axisTrim > info->axisMax) {
                        info->axisTrim = info->axisMax;
                    }
                    break;
                default:
                    // Non-attitude control axis have calculated trim
                    info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
                    break;
508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523
            }
        } else {
            // Unavailable axiss are set to defaults
            qCDebug(JoystickConfigControllerLog) << "_validateCalibration resetting unavailable axis" << chan;
            info->axisMin = _calDefaultMinValue;
            info->axisMax = _calDefaultMaxValue;
            info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
            info->reversed = false;
        }
    }
}


/// @brief Saves the rc calibration values to the board parameters.
void JoystickConfigController::_writeCalibration(void)
{
524
    Joystick* joystick = _joystickManager->activeJoystick();
525 526 527

    _validateCalibration();
    
528
    for (int axis=0; axis<_axisCount; axis++) {
529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547
        Joystick::Calibration_t calibration;
        
        struct AxisInfo* info = &_rgAxisInfo[axis];
        
        calibration.center = info->axisTrim;
        calibration.min = info->axisMin;
        calibration.max = info->axisMax;
        calibration.reversed = info->reversed;
        
        joystick->setCalibration(axis, calibration);
    }
    
    // Write function mapping parameters
    for (int function=0; function<Joystick::maxFunction; function++) {
        joystick->setFunctionAxis((Joystick::AxisFunction_t)function, _rgFunctionAxisMapping[function]);
    }
    
    _stopCalibration();
    _setInternalCalibrationValuesFromSettings();
548 549 550 551 552

    Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
    if (vehicle) {
        vehicle->setJoystickEnabled(true);
    }
553 554 555 556 557
}

/// @brief Starts the calibration process
void JoystickConfigController::_startCalibration(void)
{
Don Gagne's avatar
Don Gagne committed
558
    _activeJoystick->startCalibrationMode(Joystick::CalibrationModeCalibrating);
559 560 561 562 563 564 565 566 567 568 569 570 571 572
    _resetInternalCalibrationValues();
    
    _nextButton->setProperty("text", "Next");
    _cancelButton->setEnabled(true);
    
    _currentStep = 0;
    _setupCurrentState();
}

/// @brief Cancels the calibration process, setting things back to initial state.
void JoystickConfigController::_stopCalibration(void)
{
    _currentStep = -1;
    
Don Gagne's avatar
Don Gagne committed
573
    _activeJoystick->stopCalibrationMode(Joystick::CalibrationModeCalibrating);
574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589
    _setInternalCalibrationValuesFromSettings();
    
    _statusText->setProperty("text", "");

    _nextButton->setProperty("text", "Calibrate");
    _nextButton->setEnabled(true);
    _cancelButton->setEnabled(false);
    _skipButton->setEnabled(false);
    
    _setHelpImage(_imageCenter);
}

/// @brief Saves the current axis values, so that we can detect when the use moves an input.
void JoystickConfigController::_calSaveCurrentValues(void)
{
	qCDebug(JoystickConfigControllerLog) << "_calSaveCurrentValues";
590
    for (int i = 0; i < _axisCount; i++) {
591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632
        _axisValueSave[i] = _axisRawValue[i];
    }
}

/// @brief Set up the Save state of calibration.
void JoystickConfigController::_calSave(void)
{
    _calState = calStateSave;
    
    _statusText->setProperty("text",
                             "The current calibration settings are now displayed for each axis on screen.\n\n"
                                "Click the Next button to upload calibration to board. Click Cancel if you don't want to save these values.");

    _nextButton->setEnabled(true);
    _skipButton->setEnabled(false);
    _cancelButton->setEnabled(true);
    
    // This updates the internal values according to the validation rules. Then _updateView will tick and update ui
    // such that the settings that will be written our are displayed.
    _validateCalibration();
}

void JoystickConfigController::_setHelpImage(const char* imageFile)
{
    QString file = _imageFilePrefix;
    
    file += _imageFileMode2Dir;
    file += imageFile;
    
    qCDebug(JoystickConfigControllerLog) << "_setHelpImage" << file;
    
    _imageHelp = file;
    emit imageHelpChanged(file);
}

int JoystickConfigController::axisCount(void)
{
    return _axisCount;
}

int JoystickConfigController::rollAxisValue(void)
{    
633
    if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) {
634 635 636 637 638 639 640 641
        return _axisRawValue[Joystick::rollFunction];
    } else {
        return 1500;
    }
}

int JoystickConfigController::pitchAxisValue(void)
{
642
    if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) {
643 644 645 646 647 648 649 650
        return _axisRawValue[Joystick::pitchFunction];
    } else {
        return 1500;
    }
}

int JoystickConfigController::yawAxisValue(void)
{
651
    if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) {
652 653 654 655 656 657 658 659
        return _axisRawValue[Joystick::yawFunction];
    } else {
        return 1500;
    }
}

int JoystickConfigController::throttleAxisValue(void)
{
660
    if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) {
661 662 663 664 665 666 667 668
        return _axisRawValue[Joystick::throttleFunction];
    } else {
        return 1500;
    }
}

bool JoystickConfigController::rollAxisMapped(void)
{
669
    return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis;
670 671 672 673
}

bool JoystickConfigController::pitchAxisMapped(void)
{
674
    return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis;
675 676 677 678
}

bool JoystickConfigController::yawAxisMapped(void)
{
679
    return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis;
680 681 682 683
}

bool JoystickConfigController::throttleAxisMapped(void)
{
684
    return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis;
685 686 687 688
}

bool JoystickConfigController::rollAxisReversed(void)
{
689
    if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) {
690 691 692 693 694 695 696 697
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].reversed;
    } else {
        return false;
    }
}

bool JoystickConfigController::pitchAxisReversed(void)
{
698
    if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) {
699 700 701 702 703 704 705 706
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].reversed;
    } else {
        return false;
    }
}

bool JoystickConfigController::yawAxisReversed(void)
{
707
    if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) {
708 709 710 711 712 713 714 715
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].reversed;
    } else {
        return false;
    }
}

bool JoystickConfigController::throttleAxisReversed(void)
{
716
    if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) {
717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].reversed;
    } else {
        return false;
    }
}

void JoystickConfigController::_signalAllAttiudeValueChanges(void)
{
    emit rollAxisMappedChanged(rollAxisMapped());
    emit pitchAxisMappedChanged(pitchAxisMapped());
    emit yawAxisMappedChanged(yawAxisMapped());
    emit throttleAxisMappedChanged(throttleAxisMapped());
    
    emit rollAxisReversedChanged(rollAxisReversed());
    emit pitchAxisReversedChanged(pitchAxisReversed());
    emit yawAxisReversedChanged(yawAxisReversed());
    emit throttleAxisReversedChanged(throttleAxisReversed());
}
735 736 737

void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
{
738 739
    bool joystickTransition = false;
    
740
    if (_activeJoystick) {
741
        joystickTransition = true;
742
        disconnect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
743 744 745 746
        delete _rgAxisInfo;
        delete _axisValueSave;
        delete _axisRawValue;
        _axisCount = 0;
747 748 749 750 751
        _activeJoystick = NULL;
    }
    
    if (joystick) {
        _activeJoystick = joystick;
752 753 754 755
        if (joystickTransition) {
            _stopCalibration();
        }
        _activeJoystick->startCalibrationMode(Joystick::CalibrationModeMonitor);
756 757 758 759
        _axisCount = _activeJoystick->axisCount();
        _rgAxisInfo = new struct AxisInfo[_axisCount];
        _axisValueSave = new int[_axisCount];
        _axisRawValue = new int[_axisCount];
760 761 762
        connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
    }
}
763 764 765 766 767

bool JoystickConfigController::_validAxis(int axis)
{
    return axis >= 0 && axis < _axisCount;
}