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

QGC_LOGGING_CATEGORY(JoystickConfigControllerLog, "JoystickConfigControllerLog")

17 18
#define ENABLE_GIMBAL 0

19 20
const int JoystickConfigController::_calCenterPoint =       0;
const int JoystickConfigController::_calValidMinValue =     -32768;     ///< Largest valid minimum axis value
21
const int JoystickConfigController::_calValidMaxValue =     32767;      ///< Smallest valid maximum axis value
22
const int JoystickConfigController::_calDefaultMinValue =   -32768;     ///< Default value for Min if not set
23
const int JoystickConfigController::_calDefaultMaxValue =   32767;      ///< Default value for Max if not set
24 25
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
26
const int JoystickConfigController::_calSettleDelta =       600;        ///< Amount of delta which is considered no stick movement
27 28 29 30
const int JoystickConfigController::_calMinDelta =          1000;       ///< Amount of delta allowed around min value to consider channel at min

const int JoystickConfigController::_stickDetectSettleMSecs = 500;

31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
static const JoystickConfigController::stateStickPositions stSticksCentered {
    0.25, 0.5, 0.75, 0.5
};

static const JoystickConfigController::stateStickPositions stLeftStickUp {
    0.25, 0.3084, 0.75, 0.5
};

static const JoystickConfigController::stateStickPositions stLeftStickDown {
    0.25, 0.6916, 0.75, 0.5
};

static const JoystickConfigController::stateStickPositions stLeftStickLeft {
    0.1542, 0.5, 0.75, 0.5
};

static const JoystickConfigController::stateStickPositions stLeftStickRight {
    0.3458, 0.5, 0.75, 0.5
};

static const JoystickConfigController::stateStickPositions stRightStickUp {
    0.25, 0.5, 0.75, 0.3084
};

static const JoystickConfigController::stateStickPositions stRightStickDown {
    0.25, 0.5, 0.75, 0.6916
};

static const JoystickConfigController::stateStickPositions stRightStickLeft {
    0.25, 0.5, 0.6542, 0.5
};

static const JoystickConfigController::stateStickPositions stRightStickRight {
    0.25, 0.5, 0.8423, 0.5
};

//-- Gimbal

static const JoystickConfigController::stateStickPositions stGimbalCentered {
    0.5, 0.5, 0.5, 0.3
};

73
#if ENABLE_GIMBAL
74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
static const JoystickConfigController::stateStickPositions stGimbalPitchDown {
    0.5, 0.6, 0.5, 0.3
};

static const JoystickConfigController::stateStickPositions stGimbalPitchUp {
    0.5, 0.4, 0.5, 0.3
};

static const JoystickConfigController::stateStickPositions stGimbalYawLeft {
    0.5, 0.5, 0.4, 0.3
};

static const JoystickConfigController::stateStickPositions stGimbalYawRight {
    0.5, 0.5, 0.6, 0.3
};
89
#endif
90

91
JoystickConfigController::JoystickConfigController(void)
Gus Grubba's avatar
Gus Grubba committed
92
    : _joystickManager(qgcApp()->toolbox()->joystickManager())
93 94
{
    
95 96
    connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &JoystickConfigController::_activeJoystickChanged);
    _activeJoystickChanged(_joystickManager->activeJoystick());
97
    _setStickPositions();
98
    _resetInternalCalibrationValues();
99 100
    _currentStickPositions  << _sticksCentered.leftX  << _sticksCentered.leftY  << _sticksCentered.rightX  << _sticksCentered.rightY;
    _currentGimbalPositions << stGimbalCentered.leftX << stGimbalCentered.leftY << stGimbalCentered.rightX << stGimbalCentered.rightY;
101 102 103 104 105 106 107
}

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

108 109 110 111 112 113 114 115 116
void JoystickConfigController::setDeadbandValue(int axis, int value)
{
    _axisDeadbandChanged(axis,value);
    Joystick* joystick = _joystickManager->activeJoystick();
    Joystick::Calibration_t calibration = joystick->getCalibration(axis);
    calibration.deadband = value;
    joystick->setCalibration(axis,calibration);
}

117 118
JoystickConfigController::~JoystickConfigController()
{
Jacob Walser's avatar
Jacob Walser committed
119
    if(_activeJoystick) {
120
        _activeJoystick->setCalibrationMode(false);
Jacob Walser's avatar
Jacob Walser committed
121
    }
122 123 124 125 126
}

/// @brief Returns the state machine entry for the specified state.
const JoystickConfigController::stateMachineEntry* JoystickConfigController::_getStateMachineEntry(int step)
{
Gus Grubba's avatar
Gus Grubba committed
127
    static const char* msgBegin =               "Allow all sticks to center as shown in diagram.\nClick Next to continue";
Gus Grubba's avatar
Gus Grubba committed
128 129 130 131 132 133 134 135 136
    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...";
137
#if ENABLE_GIMBAL
Gus Grubba's avatar
Gus Grubba committed
138 139 140 141
    static const char* msgGimbalPitchDown =     "Move the Gimbal Pitch control all the way down and hold it there...";
    static const char* msgGimbalPitchUp =       "Move the Gimbal Pitch control all the way up and hold it there...";
    static const char* msgGimbalYawLeft =       "Move the Gimbal Yaw control all the way to the left and hold it there...";
    static const char* msgGimbalYawRight =      "Move the Gimbal Yaw control all the way to the right and hold it there...";
142
#endif
Gus Grubba's avatar
Gus Grubba committed
143
    static const char* msgComplete =            "All settings have been captured.\nClick Next to enable the joystick.";
144

145 146
    static const stateMachineEntry rgStateMachine[] = {
        //Function
147 148 149 150 151 152 153 154 155 156
        { Joystick::maxFunction,            msgBegin,           _sticksCentered,        stGimbalCentered,   &JoystickConfigController::_inputCenterWaitBegin,   &JoystickConfigController::_saveAllTrims,        nullptr, 0 },
        { Joystick::throttleFunction,       msgThrottleUp,      _sticksThrottleUp,      stGimbalCentered,   &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 0 },
        { Joystick::throttleFunction,       msgThrottleDown,    _sticksThrottleDown,    stGimbalCentered,   &JoystickConfigController::_inputStickMin,          nullptr,                                         nullptr, 0 },
        { Joystick::yawFunction,            msgYawRight,        _sticksYawRight,        stGimbalCentered,   &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 1 },
        { Joystick::yawFunction,            msgYawLeft,         _sticksYawLeft,         stGimbalCentered,   &JoystickConfigController::_inputStickMin,          nullptr,                                         nullptr, 1 },
        { Joystick::rollFunction,           msgRollRight,       _sticksRollRight,       stGimbalCentered,   &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 2 },
        { Joystick::rollFunction,           msgRollLeft,        _sticksRollLeft,        stGimbalCentered,   &JoystickConfigController::_inputStickMin,          nullptr,                                         nullptr, 2 },
        { Joystick::pitchFunction,          msgPitchUp,         _sticksPitchUp,         stGimbalCentered,   &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 3 },
        { Joystick::pitchFunction,          msgPitchDown,       _sticksPitchDown,       stGimbalCentered,   &JoystickConfigController::_inputStickMin,          nullptr,                                         nullptr, 3 },
        { Joystick::pitchFunction,          msgPitchCenter,     _sticksCentered,        stGimbalCentered,   &JoystickConfigController::_inputCenterWait,        nullptr,                                         nullptr, 3 },
157
#if ENABLE_GIMBAL
158 159 160 161
        { Joystick::gimbalPitchFunction,    msgGimbalPitchUp,   _sticksCentered,        stGimbalPitchUp,    &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 4 },
        { Joystick::gimbalPitchFunction,    msgGimbalPitchDown, _sticksCentered,        stGimbalPitchDown,  &JoystickConfigController::_inputStickMin,          nullptr,                                         nullptr, 4 },
        { Joystick::gimbalYawFunction,      msgGimbalYawRight,  _sticksCentered,        stGimbalYawRight,   &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 5 },
        { Joystick::gimbalYawFunction,      msgGimbalYawLeft,   _sticksCentered,        stGimbalYawLeft,    &JoystickConfigController::_inputStickMin,          nullptr,                                         nullptr, 5 },
162
#endif
163
        { Joystick::maxFunction,            msgComplete,        _sticksCentered,        stGimbalCentered,   nullptr,                                            &JoystickConfigController::_writeCalibration,    nullptr, -1 },
164
    };
165

166
    Q_ASSERT(step >= 0 && step < static_cast<int>((sizeof(rgStateMachine) / sizeof(rgStateMachine[0]))));
167 168 169
    return &rgStateMachine[step];
}

Gus Grubba's avatar
Gus Grubba committed
170
void JoystickConfigController::_advanceState()
171 172
{
    _currentStep++;
173 174
#if ENABLE_GIMBAL
    // TODO There are no MAVLink messages to handle gimbal from this
Gus Grubba's avatar
Gus Grubba committed
175
    const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
Gus Grubba's avatar
Gus Grubba committed
176 177 178 179 180 181 182 183
    //-- Handle Gimbal
    if (state->channelID > _axisCount) {
        //-- No channels for gimbal
        _advanceState();
    }
    if((state->channelID == 4 || state->channelID == 5) && !_activeJoystick->gimbalEnabled()) {
        //-- Gimbal disabled. Skip it.
        _advanceState();
Gus Grubba's avatar
Gus Grubba committed
184
    }
185
#endif
186 187 188
    _setupCurrentState();
}

Gus Grubba's avatar
Gus Grubba committed
189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206
bool JoystickConfigController::nextEnabled()
{
    if(_currentStep >= 0) {
        const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
        return state->nextFn != nullptr;
    }
    return false;
}

bool JoystickConfigController::skipEnabled()
{
    if(_currentStep >= 0) {
        const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
        return state->skipFn != nullptr;
    }
    return false;
}

207
/// @brief Sets up the state machine according to the current step from _currentStep.
Gus Grubba's avatar
Gus Grubba committed
208
void JoystickConfigController::_setupCurrentState()
209
{
Gus Grubba's avatar
Gus Grubba committed
210
    const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
211
    _setStatusText(state->instructions);
212
    _stickDetectAxis = _axisNoAxis;
213 214
    _stickDetectSettleStarted = false;
    _calSaveCurrentValues();
215 216 217 218 219 220
    _currentStickPositions.clear();
    _currentStickPositions << state->stickPositions.leftX << state->stickPositions.leftY << state->stickPositions.rightX << state->stickPositions.rightY;
    _currentGimbalPositions.clear();
    _currentGimbalPositions << state->gimbalPositions.leftX << state->gimbalPositions.leftY << state->gimbalPositions.rightX << state->gimbalPositions.rightY;
    emit stickPositionsChanged();
    emit gimbalPositionsChanged();
Gus Grubba's avatar
Gus Grubba committed
221 222
    emit nextEnabledChanged();
    emit skipEnabledChanged();
223 224 225 226
}

void JoystickConfigController::_axisValueChanged(int axis, int value)
{
227
    if (_validAxis(axis)) {
228 229 230 231 232
        // We always update raw values
        _axisRawValue[axis] = value;
        emit axisValueChanged(axis, _axisRawValue[axis]);
        if (_currentStep == -1) {
            // Track the axis count by keeping track of how many axes we see
Gus Grubba's avatar
Gus Grubba committed
233
            if (axis + 1 > static_cast<int>(_axisCount)) {
234
                _axisCount = axis + 1;
Gus Grubba's avatar
Gus Grubba committed
235 236 237 238
                if(_axisCount > 4)
                    emit hasGimbalPitchChanged();
                if(_axisCount > 5)
                    emit hasGimbalYawChanged();
239 240 241 242 243 244 245 246 247 248 249 250
            }
        }
        if (_currentStep != -1) {
            const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
            Q_ASSERT(state);
            if (state->rcInputFn) {
                (this->*state->rcInputFn)(state->function, axis, value);
            }
        }
    }
}

Gus Grubba's avatar
Gus Grubba committed
251
void JoystickConfigController::nextButtonClicked()
252 253 254 255
{
    if (_currentStep == -1) {
        // Need to have enough channels
        if (_axisCount < _axisMinimum) {
256
            qgcApp()->showMessage(tr("Detected %1 joystick axes. To operate PX4, you need at least %2 axes.").arg(_axisCount).arg(_axisMinimum));
257 258 259 260 261 262 263 264 265 266 267
            return;
        }
        _startCalibration();
    } else {
        const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
        Q_ASSERT(state);
        Q_ASSERT(state->nextFn);
        (this->*state->nextFn)();
    }
}

Gus Grubba's avatar
Gus Grubba committed
268
void JoystickConfigController::skipButtonClicked()
269 270 271 272 273 274 275 276 277 278 279 280 281
{
    Q_ASSERT(_currentStep != -1);
    const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
    Q_ASSERT(state);
    Q_ASSERT(state->skipFn);
    (this->*state->skipFn)();
}

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

282 283 284 285 286 287
bool JoystickConfigController::getDeadbandToggle() {
    return _activeJoystick->deadband();
}

void JoystickConfigController::setDeadbandToggle(bool deadband) {
    _activeJoystick->setDeadband(deadband);
nanthony21's avatar
nanthony21 committed
288
    _signalAllAttitudeValueChanges();
289 290 291
    emit deadbandToggled(deadband);
}

Gus Grubba's avatar
Gus Grubba committed
292
void JoystickConfigController::_saveAllTrims()
293 294 295
{
    // 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
296
    // axis they are yet. As we continue through the process the other axes will get their
297
    // trims reset to correct values.
Gus Grubba's avatar
Gus Grubba committed
298
    for (int i = 0; i < _axisCount; i++) {
299 300 301 302 303 304
        qCDebug(JoystickConfigControllerLog) << "_saveAllTrims trim" << _axisRawValue[i];
        _rgAxisInfo[i].axisTrim = _axisRawValue[i];
    }
    _advanceState();
}

Gregory Dymarek's avatar
Gregory Dymarek committed
305 306 307
void JoystickConfigController::_axisDeadbandChanged(int axis, int value)
{
    value = abs(value)<_calValidMaxValue?abs(value):_calValidMaxValue;
308
    _rgAxisInfo[axis].deadband = value;
309
    emit axisDeadbandChanged(axis,value);
310
    qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
Gregory Dymarek's avatar
Gregory Dymarek committed
311 312
}

313 314 315 316
/// @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
317
    //sensing deadband
Gus Grubba's avatar
Gus Grubba committed
318 319
    if ((abs(value) * 1.1f > _rgAxisInfo[axis].deadband) && (_activeJoystick->deadband())) {   //add 10% on top of existing deadband
        _axisDeadbandChanged(axis, static_cast<int>(abs(value) * 1.1f));
Gregory Dymarek's avatar
Gregory Dymarek committed
320
    }
321
    // FIXME: Doesn't wait for center
322 323
}

Don Gagne's avatar
Don Gagne committed
324
bool JoystickConfigController::_stickSettleComplete(int axis, int value)
325
{
326 327 328 329 330
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return false;
    }

331 332 333 334
    // 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
335
        qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
336 337 338 339 340 341 342 343
        _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
344
                qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete detection complete, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
345 346 347 348
                return true;
            }
        } else {
            // Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs
Don Gagne's avatar
Don Gagne committed
349
            qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
350 351 352 353 354 355 356 357 358 359 360 361
            _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;
    
362 363 364 365 366
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

367 368 369 370 371
    // If this axis is already used in a mapping we can't use it again
    if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
        return;
    }
    
372
    if (_stickDetectAxis == _axisNoAxis) {
373 374 375 376 377 378 379 380 381 382
        // 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
383
        if (_stickSettleComplete(axis, value)) {
384 385 386 387 388 389 390 391 392 393 394 395
            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
396
            qCDebug(JoystickConfigControllerLog) << "_inputStickDetect saving values, function:axis:value:reversed:_axisValueSave" << function << axis << value << info->reversed << _axisValueSave[axis];
nanthony21's avatar
nanthony21 committed
397
            _signalAllAttitudeValueChanges();
398 399 400 401 402 403 404
            _advanceState();
        }
    }
}

void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, int axis, int value)
{
Don Gagne's avatar
Don Gagne committed
405
    qCDebug(JoystickConfigControllerLog) << "_inputStickMin function:axis:value" << function << axis << value;
406 407 408 409
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }
410 411 412 413
    // We only care about the axis mapped to the function we are working on
    if (_rgFunctionAxisMapping[function] != axis) {
        return;
    }
414
    if (_stickDetectAxis == _axisNoAxis) {
415 416 417 418 419 420
        // 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
421
                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
422 423 424 425 426 427
            }
        } else {
            if (value < _calCenterPoint - _calMoveDelta) {
                _stickDetectAxis = axis;
                _stickDetectInitialValue = value;
                _stickDetectValue = value;
Don Gagne's avatar
Don Gagne committed
428
                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
429 430 431 432
            }
        }
    } else {
        // We are waiting for the selected axis to settle out
Don Gagne's avatar
Don Gagne committed
433
        if (_stickSettleComplete(axis, value)) {
434 435 436 437 438 439 440
            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
441
            qCDebug(JoystickConfigControllerLog) << "_inputStickMin saving values, function:axis:value:reversed" << function << axis << value << info->reversed;
442 443 444 445 446 447 448
            _advanceState();
        }
    }
}

void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t function, int axis, int value)
{
Don Gagne's avatar
Don Gagne committed
449
    qCDebug(JoystickConfigControllerLog) << "_inputCenterWait function:axis:value" << function << axis << value;
450 451 452 453 454
    if (!_validAxis(axis)) {
        qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount;
        return;
    }

455 456 457 458 459
    // We only care about the axis mapped to the function we are working on
    if (_rgFunctionAxisMapping[function] != axis) {
        return;
    }
    
460
    if (_stickDetectAxis == _axisNoAxis) {
461
        // Sticks have not yet moved close enough to center
462
        int roughCenter = getDeadbandToggle() ? std::max(_rgAxisInfo[axis].deadband,_calRoughCenterDelta) : _calRoughCenterDelta;
463
        if (abs(_calCenterPoint - value) < roughCenter) {
464 465 466 467
            // 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
468
            qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected possible center _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
469 470
        }
    } else {
Don Gagne's avatar
Don Gagne committed
471
        if (_stickSettleComplete(axis, value)) {
472 473 474 475 476 477
            _advanceState();
        }
    }
}

/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
Gus Grubba's avatar
Gus Grubba committed
478
void JoystickConfigController::_resetInternalCalibrationValues()
479
{
Gus Grubba's avatar
Gus Grubba committed
480 481
    // Set all raw axis to not reversed and center point values
    for (int i = 0; i < _axisCount; i++) {
482 483 484
        struct AxisInfo* info = &_rgAxisInfo[i];
        info->function = Joystick::maxFunction;
        info->reversed = false;
485
        info->deadband = 0;
486
        emit axisDeadbandChanged(i,info->deadband);
Gus Grubba's avatar
Gus Grubba committed
487 488
        info->axisMin  = JoystickConfigController::_calCenterPoint;
        info->axisMax  = JoystickConfigController::_calCenterPoint;
489 490 491
        info->axisTrim = JoystickConfigController::_calCenterPoint;
    }
    // Initialize attitude function mapping to function axis not set
Gus Grubba's avatar
Gus Grubba committed
492
    for (size_t i = 0; i < Joystick::maxFunction; i++) {
493
        _rgFunctionAxisMapping[i] = _axisNoAxis;
494
    }
nanthony21's avatar
nanthony21 committed
495
    _signalAllAttitudeValueChanges();
496 497 498
}

/// @brief Sets internal calibration values from the stored settings
Gus Grubba's avatar
Gus Grubba committed
499
void JoystickConfigController::_setInternalCalibrationValuesFromSettings()
500
{
501
    Joystick* joystick = _joystickManager->activeJoystick();
502
    // Initialize all function mappings to not set
Gus Grubba's avatar
Gus Grubba committed
503
    for (int i = 0; i < _axisCount; i++) {
504 505 506 507
        struct AxisInfo* info = &_rgAxisInfo[i];
        info->function = Joystick::maxFunction;
    }
    
Gus Grubba's avatar
Gus Grubba committed
508
    for (size_t i = 0; i < Joystick::maxFunction; i++) {
509
        _rgFunctionAxisMapping[i] = _axisNoAxis;
510 511
    }
    
Gus Grubba's avatar
Gus Grubba committed
512
    for (int axis = 0; axis < _axisCount; axis++) {
513 514
        struct AxisInfo* info = &_rgAxisInfo[axis];
        Joystick::Calibration_t calibration = joystick->getCalibration(axis);
Gus Grubba's avatar
Gus Grubba committed
515 516 517 518 519
        info->axisTrim  = calibration.center;
        info->axisMin   = calibration.min;
        info->axisMax   = calibration.max;
        info->reversed  = calibration.reversed;
        info->deadband  = calibration.deadband;
520
        emit axisDeadbandChanged(axis,info->deadband);
521
        qCDebug(JoystickConfigControllerLog) << "Read settings name:axis:min:max:trim:reversed" << joystick->name() << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed;
522 523
    }
    
Gus Grubba's avatar
Gus Grubba committed
524
    for (int function = 0; function < Joystick::maxFunction; function++) {
525
        int paramAxis;
Gus Grubba's avatar
Gus Grubba committed
526 527
        paramAxis = joystick->getFunctionAxis(static_cast<Joystick::AxisFunction_t>(function));
        if(paramAxis >= 0 && paramAxis < _axisCount) {
Jacob Walser's avatar
Jacob Walser committed
528
            _rgFunctionAxisMapping[function] = paramAxis;
Gus Grubba's avatar
Gus Grubba committed
529
            _rgAxisInfo[paramAxis].function = static_cast<Joystick::AxisFunction_t>(function);
Jacob Walser's avatar
Jacob Walser committed
530
        }
531
    }
532 533

    _transmitterMode = joystick->getTXMode();
nanthony21's avatar
nanthony21 committed
534
    _signalAllAttitudeValueChanges();
535 536 537
}

/// @brief Validates the current settings against the calibration rules resetting values as necessary.
Gus Grubba's avatar
Gus Grubba committed
538
void JoystickConfigController::_validateCalibration()
539
{
Gus Grubba's avatar
Gus Grubba committed
540
    for (int chan = 0; chan < _axisCount; chan++) {
541
        struct AxisInfo* info = &_rgAxisInfo[chan];
542 543 544 545
        // 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.
        if (info->axisMin < _calValidMinValue || info->axisMax > _calValidMaxValue) {
            qCDebug(JoystickConfigControllerLog) << "_validateCalibration resetting axis" << chan;
546 547 548
            info->axisMin = _calDefaultMinValue;
            info->axisMax = _calDefaultMaxValue;
            info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567
        }
        switch (_rgAxisInfo[chan].function) {
            case Joystick::throttleFunction:
            case Joystick::yawFunction:
            case Joystick::rollFunction:
            case Joystick::pitchFunction:
            case Joystick::gimbalPitchFunction:
            case Joystick::gimbalYawFunction:
                // 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;
568 569 570 571 572
        }
    }
}

/// @brief Saves the rc calibration values to the board parameters.
Gus Grubba's avatar
Gus Grubba committed
573
void JoystickConfigController::_writeCalibration()
574
{
575
    Joystick* joystick = _joystickManager->activeJoystick();
576 577
    _validateCalibration();
    
Gus Grubba's avatar
Gus Grubba committed
578
    for (int axis = 0; axis < _axisCount; axis++) {
579
        Joystick::Calibration_t calibration;
Gus Grubba's avatar
Gus Grubba committed
580 581 582 583 584 585
        struct AxisInfo* info   = &_rgAxisInfo[axis];
        calibration.center      = info->axisTrim;
        calibration.min         = info->axisMin;
        calibration.max         = info->axisMax;
        calibration.reversed    = info->reversed;
        calibration.deadband    = info->deadband;
586 587 588 589
        joystick->setCalibration(axis, calibration);
    }
    
    // Write function mapping parameters
Gus Grubba's avatar
Gus Grubba committed
590 591
    for (int function = 0; function < Joystick::maxFunction; function++) {
        joystick->setFunctionAxis(static_cast<Joystick::AxisFunction_t>(function), _rgFunctionAxisMapping[function]);
592 593 594 595
    }
    
    _stopCalibration();
    _setInternalCalibrationValuesFromSettings();
596 597 598 599 600

    Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
    if (vehicle) {
        vehicle->setJoystickEnabled(true);
    }
601 602 603
}

/// @brief Starts the calibration process
Gus Grubba's avatar
Gus Grubba committed
604
void JoystickConfigController::_startCalibration()
605
{
606
    _activeJoystick->setCalibrationMode(true);
607 608 609
    _resetInternalCalibrationValues();
    _currentStep = 0;
    _setupCurrentState();
610
    emit calibratingChanged();
611 612 613
}

/// @brief Cancels the calibration process, setting things back to initial state.
Gus Grubba's avatar
Gus Grubba committed
614
void JoystickConfigController::_stopCalibration()
615 616
{
    _currentStep = -1;
617
    _activeJoystick->setCalibrationMode(false);
618
    _setInternalCalibrationValuesFromSettings();
619
    _setStatusText("");
620
    emit calibratingChanged();
Gus Grubba's avatar
Gus Grubba committed
621 622 623 624 625 626
    _currentStickPositions.clear();
    _currentGimbalPositions.clear();
    _currentStickPositions  << _sticksCentered.leftX  << _sticksCentered.leftY  << _sticksCentered.rightX  << _sticksCentered.rightY;
    _currentGimbalPositions << stGimbalCentered.leftX << stGimbalCentered.leftY << stGimbalCentered.rightX << stGimbalCentered.rightY;
    emit stickPositionsChanged();
    emit gimbalPositionsChanged();
627 628 629
}

/// @brief Saves the current axis values, so that we can detect when the use moves an input.
Gus Grubba's avatar
Gus Grubba committed
630
void JoystickConfigController::_calSaveCurrentValues()
631 632
{
	qCDebug(JoystickConfigControllerLog) << "_calSaveCurrentValues";
633
    for (int i = 0; i < _axisCount; i++) {
634 635 636 637
        _axisValueSave[i] = _axisRawValue[i];
    }
}

638
void JoystickConfigController::_setStickPositions()
639
{
640
    _sticksCentered = stSticksCentered;
641 642
    switch(_transmitterMode) {
    case 1:
643 644 645 646 647 648
        _sticksThrottleUp   = stRightStickUp;
        _sticksThrottleDown = stRightStickDown;
        _sticksYawLeft      = stLeftStickLeft;
        _sticksYawRight     = stLeftStickRight;
        _sticksRollLeft     = stRightStickLeft;
        _sticksRollRight    = stRightStickRight;
Gus Grubba's avatar
Gus Grubba committed
649 650
        _sticksPitchUp      = stLeftStickUp;
        _sticksPitchDown    = stLeftStickDown;
651 652
        break;
    case 2:
653 654 655 656 657 658
        _sticksThrottleUp   = stLeftStickUp;
        _sticksThrottleDown = stLeftStickDown;
        _sticksYawLeft      = stLeftStickLeft;
        _sticksYawRight     = stLeftStickRight;
        _sticksRollLeft     = stRightStickLeft;
        _sticksRollRight    = stRightStickRight;
Gus Grubba's avatar
Gus Grubba committed
659 660
        _sticksPitchUp      = stRightStickUp;
        _sticksPitchDown    = stRightStickDown;
661 662
        break;
    case 3:
663 664 665 666 667 668
        _sticksThrottleUp   = stRightStickUp;
        _sticksThrottleDown = stRightStickDown;
        _sticksYawLeft      = stRightStickLeft;
        _sticksYawRight     = stRightStickRight;
        _sticksRollLeft     = stLeftStickLeft;
        _sticksRollRight    = stLeftStickRight;
Gus Grubba's avatar
Gus Grubba committed
669 670
        _sticksPitchUp      = stLeftStickUp;
        _sticksPitchDown    = stLeftStickDown;
671 672
        break;
    case 4:
673 674 675 676 677 678
        _sticksThrottleUp   = stLeftStickUp;
        _sticksThrottleDown = stLeftStickDown;
        _sticksYawLeft      = stRightStickLeft;
        _sticksYawRight     = stRightStickRight;
        _sticksRollLeft     = stLeftStickLeft;
        _sticksRollRight    = stLeftStickRight;
Gus Grubba's avatar
Gus Grubba committed
679 680
        _sticksPitchUp      = stRightStickUp;
        _sticksPitchDown    = stRightStickDown;
681 682 683 684
        break;
    default:
        Q_ASSERT(false);
    }
685 686
}

Gus Grubba's avatar
Gus Grubba committed
687
bool JoystickConfigController::rollAxisReversed()
688
{
689
    if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) {
690 691 692 693 694 695
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].reversed;
    } else {
        return false;
    }
}

Gus Grubba's avatar
Gus Grubba committed
696
bool JoystickConfigController::pitchAxisReversed()
697
{
698
    if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) {
699 700 701 702 703 704
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].reversed;
    } else {
        return false;
    }
}

Gus Grubba's avatar
Gus Grubba committed
705
bool JoystickConfigController::yawAxisReversed()
706
{
707
    if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) {
708 709 710 711 712 713
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].reversed;
    } else {
        return false;
    }
}

Gus Grubba's avatar
Gus Grubba committed
714
bool JoystickConfigController::throttleAxisReversed()
715
{
716
    if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) {
717 718 719 720 721 722
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].reversed;
    } else {
        return false;
    }
}

Gus Grubba's avatar
Gus Grubba committed
723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740
bool JoystickConfigController::gimbalPitchAxisReversed()
{
    if (_rgFunctionAxisMapping[Joystick::gimbalPitchFunction] != _axisNoAxis) {
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::gimbalPitchFunction]].reversed;
    } else {
        return false;
    }
}

bool JoystickConfigController::gimbalYawAxisReversed()
{
    if (_rgFunctionAxisMapping[Joystick::gimbalYawFunction] != _axisNoAxis) {
        return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::gimbalYawFunction]].reversed;
    } else {
        return false;
    }
}

741 742
void JoystickConfigController::setTransmitterMode(int mode)
{
743 744
    // Mode selection is disabled during calibration
    if (mode > 0 && mode <= 4 && _currentStep == -1) {
745
        _transmitterMode = mode;
746 747 748
        _setStickPositions();
        _activeJoystick->setTXMode(mode);
        _setInternalCalibrationValuesFromSettings();
749 750 751
    }
}

Gus Grubba's avatar
Gus Grubba committed
752
void JoystickConfigController::_signalAllAttitudeValueChanges()
753 754 755 756 757
{
    emit rollAxisMappedChanged(rollAxisMapped());
    emit pitchAxisMappedChanged(pitchAxisMapped());
    emit yawAxisMappedChanged(yawAxisMapped());
    emit throttleAxisMappedChanged(throttleAxisMapped());
Gus Grubba's avatar
Gus Grubba committed
758 759 760
    emit gimbalPitchAxisMappedChanged(gimbalPitchAxisMapped());
    emit gimbalYawAxisMappedChanged(gimbalYawAxisMapped());

761 762 763 764
    emit rollAxisReversedChanged(rollAxisReversed());
    emit pitchAxisReversedChanged(pitchAxisReversed());
    emit yawAxisReversedChanged(yawAxisReversed());
    emit throttleAxisReversedChanged(throttleAxisReversed());
Gus Grubba's avatar
Gus Grubba committed
765 766
    emit gimbalPitchAxisReversedChanged(pitchAxisReversed());
    emit gimbalYawAxisReversedChanged(yawAxisReversed());
Gregory Dymarek's avatar
Gregory Dymarek committed
767

768
    emit transmitterModeChanged(_transmitterMode);
769
}
770 771 772

void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
{
773
    bool joystickTransition = false;
774
    if (_activeJoystick) {
775
        joystickTransition = true;
776
        disconnect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
Jacob Walser's avatar
Jacob Walser committed
777 778 779 780 781
        // This will reset _rgFunctionAxis values to -1 to prevent out-of-bounds accesses
        _resetInternalCalibrationValues();
        delete[] _rgAxisInfo;
        delete[] _axisValueSave;
        delete[] _axisRawValue;
782
        _axisCount = 0;
Gus Grubba's avatar
Gus Grubba committed
783
        _activeJoystick = nullptr;
Gus Grubba's avatar
Gus Grubba committed
784 785
        emit hasGimbalPitchChanged();
        emit hasGimbalYawChanged();
786 787 788 789
    }
    
    if (joystick) {
        _activeJoystick = joystick;
790 791 792
        if (joystickTransition) {
            _stopCalibration();
        }
793
        _activeJoystick->setCalibrationMode(false);
Gus Grubba's avatar
Gus Grubba committed
794 795 796 797
        _axisCount      = _activeJoystick->axisCount();
        _rgAxisInfo     = new struct AxisInfo[_axisCount];
        _axisValueSave  = new int[_axisCount];
        _axisRawValue   = new int[_axisCount];
Jacob Walser's avatar
Jacob Walser committed
798
        _setInternalCalibrationValuesFromSettings();
799
        connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
Gus Grubba's avatar
Gus Grubba committed
800 801
        emit hasGimbalPitchChanged();
        emit hasGimbalYawChanged();
802 803
    }
}
804 805 806 807 808

bool JoystickConfigController::_validAxis(int axis)
{
    return axis >= 0 && axis < _axisCount;
}
809 810 811 812 813 814

void JoystickConfigController::_setStatusText(const QString& text)
{
    _statusText = text;
    emit statusTextChanged();
}