JoystickConfigController.cc 30.8 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
const int JoystickConfigController::_calMinDelta =          1000;       ///< Amount of delta allowed around min value to consider channel at min

const int JoystickConfigController::_stickDetectSettleMSecs = 500;

32 33 34 35 36
const char*  JoystickConfigController::_imageFilePrefix =   "calibration/joystick/";
const char*  JoystickConfigController::_imageFileMode1Dir = "mode1/";
const char*  JoystickConfigController::_imageFileMode2Dir = "mode2/";
const char*  JoystickConfigController::_imageFileMode3Dir = "mode3/";
const char*  JoystickConfigController::_imageFileMode4Dir = "mode4/";
37 38 39 40 41 42 43 44 45 46
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";

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

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

JoystickConfigController::~JoystickConfigController()
{
Jacob Walser's avatar
Jacob Walser committed
76 77 78
    if(_activeJoystick) {
        _activeJoystick->stopCalibrationMode(Joystick::CalibrationModeMonitor);
    }
79 80 81 82 83
}

/// @brief Returns the state machine entry for the specified state.
const JoystickConfigController::stateMachineEntry* JoystickConfigController::_getStateMachineEntry(int step)
{
84
    static const char* msgBegin =           "Allow all sticks to center as shown in diagram.\n\nClick Next to continue";
85 86 87 88 89 90 91 92 93
    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...";
94
    static const char* msgComplete =        "All settings have been captured. Click Next to enable the joystick.";
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 126 127 128 129 130 131
    
    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);
    
132
    _stickDetectAxis = _axisNoAxis;
133 134 135 136 137 138 139 140 141 142
    _stickDetectSettleStarted = false;
    
    _calSaveCurrentValues();
    
    _nextButton->setEnabled(state->nextFn != NULL);
    _skipButton->setEnabled(state->skipFn != NULL);
}

void JoystickConfigController::_axisValueChanged(int axis, int value)
{
143
    if (_validAxis(axis)) {
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
        // We always update raw values
        _axisRawValue[axis] = value;
        emit axisValueChanged(axis, _axisRawValue[axis]);
            
        //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) {
172
            qgcApp()->showMessage(tr("Detected %1 joystick axes. To operate PX4, you need at least %2 axes.").arg(_axisCount).arg(_axisMinimum));
173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198
            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();
}

199 200 201 202 203 204 205
bool JoystickConfigController::getDeadbandToggle() {
    return _activeJoystick->deadband();
}

void JoystickConfigController::setDeadbandToggle(bool deadband) {
    _activeJoystick->setDeadband(deadband);

nanthony21's avatar
nanthony21 committed
206
    _signalAllAttitudeValueChanges();
207 208 209 210

    emit deadbandToggled(deadband);
}

211 212 213 214
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
215
    // axis they are yet. As we continue through the process the other axes will get their
216 217 218 219 220 221 222 223 224
    // trims reset to correct values.
    
    for (int i=0; i<_axisCount; i++) {
        qCDebug(JoystickConfigControllerLog) << "_saveAllTrims trim" << _axisRawValue[i];
        _rgAxisInfo[i].axisTrim = _axisRawValue[i];
    }
    _advanceState();
}

Gregory Dymarek's avatar
Gregory Dymarek committed
225 226 227 228
void JoystickConfigController::_axisDeadbandChanged(int axis, int value)
{
    value = abs(value)<_calValidMaxValue?abs(value):_calValidMaxValue;

229
    _rgAxisInfo[axis].deadband = value;
230
    emit axisDeadbandChanged(axis,value);
231
    qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
Gregory Dymarek's avatar
Gregory Dymarek committed
232 233
}

234 235 236 237
/// @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);
Gregory Dymarek's avatar
Gregory Dymarek committed
238 239

    //sensing deadband
240
    if ((abs(value)*1.1f>_rgAxisInfo[axis].deadband)&&(_activeJoystick->deadband())) {   //add 10% on top of existing deadband
241
        _axisDeadbandChanged(axis,abs(value)*1.1f);
Gregory Dymarek's avatar
Gregory Dymarek committed
242 243
    }

244
    _nextButton->setEnabled(true);
245 246

    // FIXME: Doesn't wait for center
247 248
}

Don Gagne's avatar
Don Gagne committed
249
bool JoystickConfigController::_stickSettleComplete(int axis, int value)
250
{
251 252 253 254 255
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return false;
    }

256 257 258 259 260
    // 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
261
        qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
262 263 264 265 266 267 268 269 270 271 272

        _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
273
                qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete detection complete, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
274 275 276 277 278
                return true;
            }
        } else {
            // Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs
            
Don Gagne's avatar
Don Gagne committed
279
            qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
280 281 282 283 284 285 286 287 288 289 290 291 292
            
            _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;
    
293 294 295 296 297
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

298 299 300 301 302
    // If this axis is already used in a mapping we can't use it again
    if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
        return;
    }
    
303
    if (_stickDetectAxis == _axisNoAxis) {
304 305 306 307 308 309 310 311 312 313 314 315 316
        // 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
317
        if (_stickSettleComplete(axis, value)) {
318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333
            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
334 335
            qCDebug(JoystickConfigControllerLog) << "_inputStickDetect saving values, function:axis:value:reversed:_axisValueSave" << function << axis << value << info->reversed << _axisValueSave[axis];
            
nanthony21's avatar
nanthony21 committed
336
            _signalAllAttitudeValueChanges();
337 338 339 340 341 342 343 344
            
            _advanceState();
        }
    }
}

void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, int axis, int value)
{
Don Gagne's avatar
Don Gagne committed
345 346
    qCDebug(JoystickConfigControllerLog) << "_inputStickMin function:axis:value" << function << axis << value;
    
347 348 349 350 351
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

352 353 354 355 356
    // We only care about the axis mapped to the function we are working on
    if (_rgFunctionAxisMapping[function] != axis) {
        return;
    }

357
    if (_stickDetectAxis == _axisNoAxis) {
358 359 360 361 362 363
        // 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
364
                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
365 366 367 368 369 370
            }
        } else {
            if (value < _calCenterPoint - _calMoveDelta) {
                _stickDetectAxis = axis;
                _stickDetectInitialValue = value;
                _stickDetectValue = value;
Don Gagne's avatar
Don Gagne committed
371
                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
372 373 374 375 376
            }
        }
    } else {
        // We are waiting for the selected axis to settle out
        
Don Gagne's avatar
Don Gagne committed
377
        if (_stickSettleComplete(axis, value)) {
378 379 380 381 382 383 384 385
            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
386 387
            
            qCDebug(JoystickConfigControllerLog) << "_inputStickMin saving values, function:axis:value:reversed" << function << axis << value << info->reversed;
388 389 390 391 392 393 394 395
            
            _advanceState();
        }
    }
}

void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t function, int axis, int value)
{
Don Gagne's avatar
Don Gagne committed
396 397
    qCDebug(JoystickConfigControllerLog) << "_inputCenterWait function:axis:value" << function << axis << value;
    
398 399 400 401 402
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

403 404 405 406 407
    // We only care about the axis mapped to the function we are working on
    if (_rgFunctionAxisMapping[function] != axis) {
        return;
    }
    
408
    if (_stickDetectAxis == _axisNoAxis) {
409 410 411 412 413 414 415
        // 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
416
            qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected possible center _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
417 418
        }
    } else {
Don Gagne's avatar
Don Gagne committed
419
        if (_stickSettleComplete(axis, value)) {
420 421 422 423 424 425 426 427 428
            _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
429
    for (int i=0; i<_axisCount; i++) {
430 431 432
        struct AxisInfo* info = &_rgAxisInfo[i];
        info->function = Joystick::maxFunction;
        info->reversed = false;
433
        info->deadband = 0;
434
        emit axisDeadbandChanged(i,info->deadband);
435 436 437 438 439 440 441
        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++) {
442
        _rgFunctionAxisMapping[i] = _axisNoAxis;
443 444
    }
    
nanthony21's avatar
nanthony21 committed
445
    _signalAllAttitudeValueChanges();
446 447 448 449 450
}

/// @brief Sets internal calibration values from the stored settings
void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
{
451
    Joystick* joystick = _joystickManager->activeJoystick();
452 453 454
    
    // Initialize all function mappings to not set
    
455
    for (int i=0; i<_axisCount; i++) {
456 457 458 459 460
        struct AxisInfo* info = &_rgAxisInfo[i];
        info->function = Joystick::maxFunction;
    }
    
    for (size_t i=0; i<Joystick::maxFunction; i++) {
461
        _rgFunctionAxisMapping[i] = _axisNoAxis;
462 463
    }
    
464
    for (int axis=0; axis<_axisCount; axis++) {
465 466 467 468 469 470 471
        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;
472
        info->deadband = calibration.deadband;
473
        emit axisDeadbandChanged(axis,info->deadband);
Gregory Dymarek's avatar
Gregory Dymarek committed
474

475
        qCDebug(JoystickConfigControllerLog) << "Read settings name:axis:min:max:trim:reversed" << joystick->name() << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed;
476 477 478 479 480 481
    }
    
    for (int function=0; function<Joystick::maxFunction; function++) {
        int paramAxis;
        
        paramAxis = joystick->getFunctionAxis((Joystick::AxisFunction_t)function);
Jacob Walser's avatar
Jacob Walser committed
482 483 484 485
        if(paramAxis >= 0) {
            _rgFunctionAxisMapping[function] = paramAxis;
            _rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
        }
486
    }
487 488

    _transmitterMode = joystick->getTXMode();
489
    
nanthony21's avatar
nanthony21 committed
490
    _signalAllAttitudeValueChanges();
491 492 493 494 495
}

/// @brief Validates the current settings against the calibration rules resetting values as necessary.
void JoystickConfigController::_validateCalibration(void)
{
496
    for (int chan = 0; chan<_axisCount; chan++) {
497 498 499 500 501
        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.
502
            if (info->axisMin < _calValidMinValue || info->axisMax > _calValidMaxValue) {
503 504 505 506
                qCDebug(JoystickConfigControllerLog) << "_validateCalibration resetting axis" << chan;
                info->axisMin = _calDefaultMinValue;
                info->axisMax = _calDefaultMaxValue;
                info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524
            }
            
            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;
525 526 527 528 529 530 531
            }
        } 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);
532
            info->deadband = 0;
533 534 535 536 537 538 539 540 541
            info->reversed = false;
        }
    }
}


/// @brief Saves the rc calibration values to the board parameters.
void JoystickConfigController::_writeCalibration(void)
{
542
    Joystick* joystick = _joystickManager->activeJoystick();
543 544 545

    _validateCalibration();
    
546
    for (int axis=0; axis<_axisCount; axis++) {
547 548 549 550 551 552 553 554
        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;
555
        calibration.deadband = info->deadband;
556 557 558 559 560 561 562 563 564 565 566
        
        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();
567 568 569 570 571

    Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
    if (vehicle) {
        vehicle->setJoystickEnabled(true);
    }
572 573 574 575 576
}

/// @brief Starts the calibration process
void JoystickConfigController::_startCalibration(void)
{
Don Gagne's avatar
Don Gagne committed
577
    _activeJoystick->startCalibrationMode(Joystick::CalibrationModeCalibrating);
578 579 580 581 582 583 584
    _resetInternalCalibrationValues();
    
    _nextButton->setProperty("text", "Next");
    _cancelButton->setEnabled(true);
    
    _currentStep = 0;
    _setupCurrentState();
585
    emit calibratingChanged();
586 587 588 589 590 591 592
}

/// @brief Cancels the calibration process, setting things back to initial state.
void JoystickConfigController::_stopCalibration(void)
{
    _currentStep = -1;
    
Don Gagne's avatar
Don Gagne committed
593
    _activeJoystick->stopCalibrationMode(Joystick::CalibrationModeCalibrating);
594 595 596 597
    _setInternalCalibrationValuesFromSettings();
    
    _statusText->setProperty("text", "");

598
    _nextButton->setProperty("text", tr("Calibrate"));
599 600 601 602 603
    _nextButton->setEnabled(true);
    _cancelButton->setEnabled(false);
    _skipButton->setEnabled(false);
    
    _setHelpImage(_imageCenter);
604
    emit calibratingChanged();
605 606 607 608 609 610
}

/// @brief Saves the current axis values, so that we can detect when the use moves an input.
void JoystickConfigController::_calSaveCurrentValues(void)
{
	qCDebug(JoystickConfigControllerLog) << "_calSaveCurrentValues";
611
    for (int i = 0; i < _axisCount; i++) {
612 613 614 615 616 617 618 619 620 621
        _axisValueSave[i] = _axisRawValue[i];
    }
}

/// @brief Set up the Save state of calibration.
void JoystickConfigController::_calSave(void)
{
    _calState = calStateSave;
    
    _statusText->setProperty("text",
622 623
                             tr("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."));
624 625 626 627 628 629 630 631 632 633 634 635 636 637

    _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;
    
638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654
    switch(_transmitterMode) {
    case 1:
        file += _imageFileMode1Dir;
        break;
    case 2:
        file += _imageFileMode2Dir;
        break;
    case 3:
        file += _imageFileMode3Dir;
        break;
    case 4:
        file += _imageFileMode4Dir;
        break;
    default:
        Q_ASSERT(false);
    }

655 656 657 658 659 660 661 662 663 664 665 666 667 668
    file += imageFile;
    
    qCDebug(JoystickConfigControllerLog) << "_setHelpImage" << file;
    
    _imageHelp = file;
    emit imageHelpChanged(file);
}

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


Gregory Dymarek's avatar
Gregory Dymarek committed
669

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

bool JoystickConfigController::pitchAxisMapped(void)
{
677
    return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis;
678 679 680 681
}

bool JoystickConfigController::yawAxisMapped(void)
{
682
    return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis;
683 684 685 686
}

bool JoystickConfigController::throttleAxisMapped(void)
{
687
    return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis;
688 689 690 691
}

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

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

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

bool JoystickConfigController::throttleAxisReversed(void)
{
719
    if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) {
720 721 722 723 724 725
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].reversed;
    } else {
        return false;
    }
}

726 727
void JoystickConfigController::setTransmitterMode(int mode)
{
728
    if (mode > 0 && mode <= 4) {
729
        _transmitterMode = mode;
730
        if (_currentStep != -1) { // This should never be true, mode selection is disabled during calibration
731 732
            const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
            _setHelpImage(state->image);
733 734 735
        } else {
            _activeJoystick->setTXMode(mode);
            _setInternalCalibrationValuesFromSettings();
736 737 738 739
        }
    }
}

nanthony21's avatar
nanthony21 committed
740
void JoystickConfigController::_signalAllAttitudeValueChanges(void)
741 742 743 744 745 746 747 748 749 750
{
    emit rollAxisMappedChanged(rollAxisMapped());
    emit pitchAxisMappedChanged(pitchAxisMapped());
    emit yawAxisMappedChanged(yawAxisMapped());
    emit throttleAxisMappedChanged(throttleAxisMapped());
    
    emit rollAxisReversedChanged(rollAxisReversed());
    emit pitchAxisReversedChanged(pitchAxisReversed());
    emit yawAxisReversedChanged(yawAxisReversed());
    emit throttleAxisReversedChanged(throttleAxisReversed());
Gregory Dymarek's avatar
Gregory Dymarek committed
751

752
    emit transmitterModeChanged(_transmitterMode);
753
}
754 755 756

void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
{
757 758
    bool joystickTransition = false;
    
759
    if (_activeJoystick) {
760
        joystickTransition = true;
761
        disconnect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
Jacob Walser's avatar
Jacob Walser committed
762 763 764 765 766
        // This will reset _rgFunctionAxis values to -1 to prevent out-of-bounds accesses
        _resetInternalCalibrationValues();
        delete[] _rgAxisInfo;
        delete[] _axisValueSave;
        delete[] _axisRawValue;
767
        _axisCount = 0;
768 769 770 771 772
        _activeJoystick = NULL;
    }
    
    if (joystick) {
        _activeJoystick = joystick;
773 774 775 776
        if (joystickTransition) {
            _stopCalibration();
        }
        _activeJoystick->startCalibrationMode(Joystick::CalibrationModeMonitor);
777 778 779 780
        _axisCount = _activeJoystick->axisCount();
        _rgAxisInfo = new struct AxisInfo[_axisCount];
        _axisValueSave = new int[_axisCount];
        _axisRawValue = new int[_axisCount];
Jacob Walser's avatar
Jacob Walser committed
781
        _setInternalCalibrationValuesFromSettings();
782 783 784
        connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
    }
}
785 786 787 788 789

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