RadioComponentController.cc 43 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

/// @file
Don Gagne's avatar
Don Gagne committed
12
///     @brief Radio Config Qml Controller
13 14
///     @author Don Gagne <don@thegagnes.com

Don Gagne's avatar
Don Gagne committed
15
#include "RadioComponentController.h"
16
#include "AutoPilotPluginManager.h"
Don Gagne's avatar
Don Gagne committed
17
#include "QGCApplication.h"
18

Don Gagne's avatar
Don Gagne committed
19 20 21
#include <QSettings>

QGC_LOGGING_CATEGORY(RadioComponentControllerLog, "RadioComponentControllerLog")
22

23 24 25 26 27
#ifdef UNITTEST_BUILD
// Nasty hack to expose controller to unit test code
RadioComponentController* RadioComponentController::_unitTestController = NULL;
#endif

Don Gagne's avatar
Don Gagne committed
28 29
const int RadioComponentController::_updateInterval = 150;              ///< Interval for timer which updates radio channel widgets
const int RadioComponentController::_rcCalPWMCenterPoint = ((RadioComponentController::_rcCalPWMValidMaxValue - RadioComponentController::_rcCalPWMValidMinValue) / 2.0f) + RadioComponentController::_rcCalPWMValidMinValue;
Don Gagne's avatar
Don Gagne committed
30
// FIXME: Double check these mins againt 150% throws
Don Gagne's avatar
Don Gagne committed
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
const int RadioComponentController::_rcCalPWMValidMinValue = 1300;      ///< Largest valid minimum PWM Min range value
const int RadioComponentController::_rcCalPWMValidMaxValue = 1700;      ///< Smallest valid maximum PWM Max range value
const int RadioComponentController::_rcCalPWMDefaultMinValue = 1000;    ///< Default value for Min if not set
const int RadioComponentController::_rcCalPWMDefaultMaxValue = 2000;    ///< Default value for Max if not set
const int RadioComponentController::_rcCalRoughCenterDelta = 50;        ///< Delta around center point which is considered to be roughly centered
const int RadioComponentController::_rcCalMoveDelta = 300;            ///< Amount of delta past center which is considered stick movement
const int RadioComponentController::_rcCalSettleDelta = 20;           ///< Amount of delta which is considered no stick movement
const int RadioComponentController::_rcCalMinDelta = 100;             ///< Amount of delta allowed around min value to consider channel at min

const int RadioComponentController::_stickDetectSettleMSecs = 500;

const char*  RadioComponentController::_imageFilePrefix = "calibration/";
const char*  RadioComponentController::_imageFileMode1Dir = "mode1/";
const char*  RadioComponentController::_imageFileMode2Dir = "mode2/";
const char*  RadioComponentController::_imageCenter = "radioCenter.png";
const char*  RadioComponentController::_imageHome = "radioHome.png";
const char*  RadioComponentController::_imageThrottleUp = "radioThrottleUp.png";
const char*  RadioComponentController::_imageThrottleDown = "radioThrottleDown.png";
const char*  RadioComponentController::_imageYawLeft = "radioYawLeft.png";
const char*  RadioComponentController::_imageYawRight = "radioYawRight.png";
const char*  RadioComponentController::_imageRollLeft = "radioRollLeft.png";
const char*  RadioComponentController::_imageRollRight = "radioRollRight.png";
const char*  RadioComponentController::_imagePitchUp = "radioPitchUp.png";
const char*  RadioComponentController::_imagePitchDown = "radioPitchDown.png";
const char*  RadioComponentController::_imageSwitchMinMax = "radioSwitchMinMax.png";

const char* RadioComponentController::_settingsGroup = "RadioCalibration";
const char* RadioComponentController::_settingsKeyTransmitterMode = "TransmitterMode";

Don Gagne's avatar
Don Gagne committed
60
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoPX4[RadioComponentController::rcCalFunctionMax] = {
Don Gagne's avatar
Don Gagne committed
61 62 63 64 65 66 67 68 69
    //Parameter          required
    { "RC_MAP_ROLL" },
    { "RC_MAP_PITCH" },
    { "RC_MAP_YAW" },
    { "RC_MAP_THROTTLE" },
    { "RC_MAP_MODE_SW" },
    { "RC_MAP_POSCTL_SW" },
    { "RC_MAP_LOITER_SW" },
    { "RC_MAP_RETURN_SW" },
70
    { "RC_MAP_ACRO_SW" },
71 72
};

Don Gagne's avatar
Don Gagne committed
73 74 75 76 77 78 79 80 81 82 83 84 85
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoAPM[RadioComponentController::rcCalFunctionMax] = {
    //Parameter          required
    { "RCMAP_ROLL" },
    { "RCMAP_PITCH" },
    { "RCMAP_YAW" },
    { "RCMAP_THROTTLE" },
    { NULL },
    { NULL },
    { NULL },
    { NULL },
    { NULL },
};

Don Gagne's avatar
Don Gagne committed
86
RadioComponentController::RadioComponentController(void) :
Don Gagne's avatar
Don Gagne committed
87
    _currentStep(-1),
88
    _transmitterMode(2),
89 90
    _chanCount(0),
    _rcCalState(rcCalStateChannelWait),
Don Gagne's avatar
Don Gagne committed
91 92 93 94 95
    _unitTestMode(false),
    _statusText(NULL),
    _cancelButton(NULL),
    _nextButton(NULL),
    _skipButton(NULL)
96
{
97 98 99 100 101
#ifdef UNITTEST_BUILD
    // Nasty hack to expose controller to unit test code
    _unitTestController = this;
#endif

Don Gagne's avatar
Don Gagne committed
102
    connect(_vehicle, &Vehicle::rcChannelsChanged, this, &RadioComponentController::_rcChannelsChanged);
103
    _loadSettings();
104 105 106 107

    // APM Stack has a bug where some RC params are missing. We need to know what these are so we can skip them if missing
    // instead of popping missing param warnings.
    _apmPossibleMissingRCChannelParams << 9 << 11 << 12 << 13 << 14;
108
    
Don Gagne's avatar
Don Gagne committed
109 110 111 112 113
    _resetInternalCalibrationValues();
}

void RadioComponentController::start(void)
{
Don Gagne's avatar
Don Gagne committed
114
    _stopCalibration();
115
    _setInternalCalibrationValuesFromParameters();
Don Gagne's avatar
Don Gagne committed
116
}
117

Don Gagne's avatar
Don Gagne committed
118
RadioComponentController::~RadioComponentController()
Don Gagne's avatar
Don Gagne committed
119
{
120
    _storeSettings();
Don Gagne's avatar
Don Gagne committed
121
}
122

Don Gagne's avatar
Don Gagne committed
123
/// @brief Returns the state machine entry for the specified state.
Don Gagne's avatar
Don Gagne committed
124
const RadioComponentController::stateMachineEntry* RadioComponentController::_getStateMachineEntry(int step) const
Don Gagne's avatar
Don Gagne committed
125
{
Don Gagne's avatar
Don Gagne committed
126
    static const char* msgBeginPX4 =        "Lower the Throttle stick all the way down as shown in diagram.\n\n"
Don Gagne's avatar
Don Gagne committed
127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142
                                            "It is recommended to disconnect all motors for additional safety, however, the system is designed to not arm during the calibration.\n\n"
                                            "Click Next to continue";
    static const char* msgBeginAPM =        "Lower the Throttle stick all the way down as shown in diagram.\nReset all transmitter trims to center.\n\n"
                                            "Please ensure all motor power is disconnected AND all props are removed from the vehicle.\n\n"
                                            "Click 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 leave 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...";
    static const char* msgSwitchMinMax =    "Move all the transmitter switches and/or dials back and forth to their extreme positions.";
    static const char* msgComplete =        "All settings have been captured. Click Next to write the new parameters to your board.";
Don Gagne's avatar
Don Gagne committed
143
    
Don Gagne's avatar
Don Gagne committed
144
    static const stateMachineEntry rgStateMachinePX4[] = {
Don Gagne's avatar
Don Gagne committed
145
        //Function
Don Gagne's avatar
Don Gagne committed
146
        { rcCalFunctionMax,                 msgBeginPX4,        _imageHome,         &RadioComponentController::_inputCenterWaitBegin,   &RadioComponentController::_saveAllTrims,       NULL },
Don Gagne's avatar
Don Gagne committed
147 148 149 150 151 152 153 154 155 156 157
        { rcCalFunctionThrottle,            msgThrottleUp,      _imageThrottleUp,   &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionThrottle,            msgThrottleDown,    _imageThrottleDown, &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionYaw,                 msgYawRight,        _imageYawRight,     &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionYaw,                 msgYawLeft,         _imageYawLeft,      &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionRoll,                msgRollRight,       _imageRollRight,    &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionRoll,                msgRollLeft,        _imageRollLeft,     &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionPitch,               msgPitchUp,         _imagePitchUp,      &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionPitch,               msgPitchDown,       _imagePitchDown,    &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionPitch,               msgPitchCenter,     _imageHome,         &RadioComponentController::_inputCenterWait,        NULL,                                           NULL },
        { rcCalFunctionMax,                 msgSwitchMinMax,    _imageSwitchMinMax, &RadioComponentController::_inputSwitchMinMax,      &RadioComponentController::_advanceState,       NULL },
        { rcCalFunctionMax,                 msgComplete,        _imageThrottleDown, NULL,                                               &RadioComponentController::_writeCalibration,   NULL },
Don Gagne's avatar
Don Gagne committed
158 159
    };
    
Don Gagne's avatar
Don Gagne committed
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 186 187 188 189 190 191 192 193 194
    static const stateMachineEntry rgStateMachineAPM[] = {
        //Function
        { rcCalFunctionMax,                 msgBeginAPM,        _imageHome,         &RadioComponentController::_inputCenterWaitBegin,   &RadioComponentController::_saveAllTrims,       NULL },
        { rcCalFunctionThrottle,            msgThrottleUp,      _imageThrottleUp,   &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionThrottle,            msgThrottleDown,    _imageThrottleDown, &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionYaw,                 msgYawRight,        _imageYawRight,     &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionYaw,                 msgYawLeft,         _imageYawLeft,      &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionRoll,                msgRollRight,       _imageRollRight,    &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionRoll,                msgRollLeft,        _imageRollLeft,     &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionPitch,               msgPitchUp,         _imagePitchUp,      &RadioComponentController::_inputStickDetect,       NULL,                                           NULL },
        { rcCalFunctionPitch,               msgPitchDown,       _imagePitchDown,    &RadioComponentController::_inputStickMin,          NULL,                                           NULL },
        { rcCalFunctionPitch,               msgPitchCenter,     _imageHome,         &RadioComponentController::_inputCenterWait,        NULL,                                           NULL },
        { rcCalFunctionMax,                 msgSwitchMinMax,    _imageSwitchMinMax, &RadioComponentController::_inputSwitchMinMax,      &RadioComponentController::_advanceState,       NULL },
        { rcCalFunctionMax,                 msgComplete,        _imageThrottleDown, NULL,                                               &RadioComponentController::_writeCalibration,   NULL },
    };

    bool badStep = false;
    if (step < 0) {
        badStep = true;
    }
    if (_px4Vehicle()) {
        if (step >= (int)(sizeof(rgStateMachinePX4) / sizeof(rgStateMachinePX4[0]))) {
            badStep = true;
        }
    } else {
        if (step >= (int)(sizeof(rgStateMachineAPM) / sizeof(rgStateMachineAPM[0]))) {
            badStep = true;
        }
    }
    if (badStep) {
        qWarning() << "Bad step value" << step;
        step = 0;
    }

    const stateMachineEntry* stateMachine = _px4Vehicle() ? rgStateMachinePX4 : rgStateMachineAPM;
Don Gagne's avatar
Don Gagne committed
195
    
Don Gagne's avatar
Don Gagne committed
196
    return &stateMachine[step];
Don Gagne's avatar
Don Gagne committed
197
}
198

Don Gagne's avatar
Don Gagne committed
199
void RadioComponentController::_advanceState(void)
Don Gagne's avatar
Don Gagne committed
200 201 202 203 204 205 206
{
    _currentStep++;
    _setupCurrentState();
}


/// @brief Sets up the state machine according to the current step from _currentStep.
Don Gagne's avatar
Don Gagne committed
207
void RadioComponentController::_setupCurrentState(void)
Don Gagne's avatar
Don Gagne committed
208 209 210
{
   const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
    
Don Gagne's avatar
Don Gagne committed
211
    _statusText->setProperty("text", state->instructions);
212 213
    
    _setHelpImage(state->image);
Don Gagne's avatar
Don Gagne committed
214
    
Don Gagne's avatar
Don Gagne committed
215
    _stickDetectChannel = _chanMax();
Don Gagne's avatar
Don Gagne committed
216 217 218 219
    _stickDetectSettleStarted = false;
    
    _rcCalSaveCurrentValues();
    
Don Gagne's avatar
Don Gagne committed
220 221
    _nextButton->setEnabled(state->nextFn != NULL);
    _skipButton->setEnabled(state->skipFn != NULL);
Don Gagne's avatar
Don Gagne committed
222 223
}

Don Gagne's avatar
Don Gagne committed
224 225
/// Connected to Vehicle::rcChannelsChanged signal
void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
226
{
Don Gagne's avatar
Don Gagne committed
227 228 229 230 231 232 233 234 235 236 237 238 239 240
    int maxChannel = std::min(channelCount, _chanMax());

    for (int channel=0; channel<maxChannel; channel++) {
        int channelValue = pwmValues[channel];

        if (channelValue != -1) {
            qCDebug(RadioComponentControllerLog) << "Raw value" << channel << channelValue;

            _rcRawValue[channel] = channelValue;
            emit channelRCValueChanged(channel, channelValue);

            // Signal attitude rc values to Qml if mapped
            if (_rgChannelInfo[channel].function != rcCalFunctionMax) {
                switch (_rgChannelInfo[channel].function) {
Don Gagne's avatar
Don Gagne committed
241
                case rcCalFunctionRoll:
Don Gagne's avatar
Don Gagne committed
242
                    emit rollChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
243 244
                    break;
                case rcCalFunctionPitch:
Don Gagne's avatar
Don Gagne committed
245
                    emit pitchChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
246 247
                    break;
                case rcCalFunctionYaw:
Don Gagne's avatar
Don Gagne committed
248
                    emit yawChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
249 250
                    break;
                case rcCalFunctionThrottle:
Don Gagne's avatar
Don Gagne committed
251
                    emit throttleChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
252 253 254 255
                    break;
                default:
                    break;

Don Gagne's avatar
Don Gagne committed
256
                }
Don Gagne's avatar
Don Gagne committed
257
            }
Don Gagne's avatar
Don Gagne committed
258 259 260 261 262 263 264 265 266 267 268 269

            if (_currentStep == -1) {
                if (_chanCount != channelCount) {
                    _chanCount = channelCount;
                    emit channelCountChanged(_chanCount);
                }
            } else {
                const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
                Q_ASSERT(state);
                if (state->rcInputFn) {
                    (this->*state->rcInputFn)(state->function, channel, channelValue);
                }
Don Gagne's avatar
Don Gagne committed
270
            }
Don Gagne's avatar
Don Gagne committed
271 272 273 274
        }
    }
}

Don Gagne's avatar
Don Gagne committed
275
void RadioComponentController::nextButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
276 277 278 279 280 281 282
{
    if (_currentStep == -1) {
        // Need to have enough channels
        if (_chanCount < _chanMinimum) {
            if (_unitTestMode) {
                emit nextButtonMessageBoxDisplayed();
            } else {
283
                qgcApp()->showMessage(QString("Detected %1 radio channels. To operate PX4, you need at least %2 channels.").arg(_chanCount).arg(_chanMinimum));
Don Gagne's avatar
Don Gagne committed
284 285 286 287 288 289 290 291 292 293 294 295
            }
            return;
        }
        _startCalibration();
    } else {
        const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
        Q_ASSERT(state);
        Q_ASSERT(state->nextFn);
        (this->*state->nextFn)();
    }
}

Don Gagne's avatar
Don Gagne committed
296
void RadioComponentController::skipButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
297 298 299 300 301 302 303 304 305
{
    Q_ASSERT(_currentStep != -1);
    
    const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
    Q_ASSERT(state);
    Q_ASSERT(state->skipFn);
    (this->*state->skipFn)();
}

Don Gagne's avatar
Don Gagne committed
306
void RadioComponentController::cancelButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
307
{
Don Gagne's avatar
Don Gagne committed
308
    _stopCalibration();
Don Gagne's avatar
Don Gagne committed
309 310
}

Don Gagne's avatar
Don Gagne committed
311
void RadioComponentController::_saveAllTrims(void)
Don Gagne's avatar
Don Gagne committed
312 313 314 315 316 317 318
{
    // We save all trims as the first step. At this point no channels 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
    // channels they are yet. AS we continue through the process the other channels will get their
    // trims reset to correct values.
    
    for (int i=0; i<_chanCount; i++) {
Don Gagne's avatar
Don Gagne committed
319
        qCDebug(RadioComponentControllerLog) << "_saveAllTrims trim" << _rcRawValue[i];
Don Gagne's avatar
Don Gagne committed
320 321
        _rgChannelInfo[i].rcTrim = _rcRawValue[i];
    }
Don Gagne's avatar
Don Gagne committed
322
    _advanceState();
Don Gagne's avatar
Don Gagne committed
323 324 325
}

/// @brief Waits for the sticks to be centered, enabling Next when done.
Don Gagne's avatar
Don Gagne committed
326
void RadioComponentController::_inputCenterWaitBegin(enum rcCalFunctions function, int chan, int value)
Don Gagne's avatar
Don Gagne committed
327 328 329 330 331 332 333
{
    Q_UNUSED(function);
    Q_UNUSED(chan);
    Q_UNUSED(value);
    
    // FIXME: Doesn't wait for center
    
Don Gagne's avatar
Don Gagne committed
334
    _nextButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
335 336
}

Don Gagne's avatar
Don Gagne committed
337
bool RadioComponentController::_stickSettleComplete(int value)
Don Gagne's avatar
Don Gagne committed
338 339 340 341 342 343
{
    // We are waiting for the stick to settle out to a max position
    
    if (abs(_stickDetectValue - value) > _rcCalSettleDelta) {
        // Stick is moving too much to consider stopped
        
Don Gagne's avatar
Don Gagne committed
344
        qCDebug(RadioComponentControllerLog) << "_stickSettleComplete still moving, _stickDetectValue:value" << _stickDetectValue << value;
Don Gagne's avatar
Don Gagne committed
345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360

        _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.
                return true;
            }
        } else {
            // Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs
            
Don Gagne's avatar
Don Gagne committed
361
            qCDebug(RadioComponentControllerLog) << "_stickSettleComplete starting settle timer, _stickDetectValue:value" << _stickDetectValue << value;
Don Gagne's avatar
Don Gagne committed
362 363 364 365 366 367 368 369 370
            
            _stickDetectSettleStarted = true;
            _stickDetectSettleElapsed.start();
        }
    }
    
    return false;
}

Don Gagne's avatar
Don Gagne committed
371
void RadioComponentController::_inputStickDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
372
{
Don Gagne's avatar
Don Gagne committed
373
    qCDebug(RadioComponentControllerLog) << "_inputStickDetect function:channel:value" << _functionInfo()[function].parameterName << channel << value;
Don Gagne's avatar
Don Gagne committed
374 375 376 377 378
    
    // If this channel is already used in a mapping we can't use it again
    if (_rgChannelInfo[channel].function != rcCalFunctionMax) {
        return;
    }
379
    
Don Gagne's avatar
Don Gagne committed
380
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
381 382 383 384 385
        // We have not detected enough movement on a channel yet
        
        if (abs(_rcValueSave[channel] - value) > _rcCalMoveDelta) {
            // Stick has moved far enough to consider it as being selected for the function
            
Don Gagne's avatar
Don Gagne committed
386
            qCDebug(RadioComponentControllerLog) << "_inputStickDetect starting settle wait, function:channel:value" << function << channel << value;
Don Gagne's avatar
Don Gagne committed
387 388 389 390 391 392 393 394 395 396
            
            // Setup up to detect stick being pegged to min or max value
            _stickDetectChannel = channel;
            _stickDetectInitialValue = value;
            _stickDetectValue = value;
        }
    } else if (channel == _stickDetectChannel) {
        if (_stickSettleComplete(value)) {
            ChannelInfo* info = &_rgChannelInfo[channel];
            
Don Gagne's avatar
Don Gagne committed
397
            qCDebug(RadioComponentControllerLog) << "_inputStickDetect settle complete, function:channel:value" << function << channel << value;
Don Gagne's avatar
Don Gagne committed
398 399 400 401 402 403 404 405 406 407 408 409 410 411 412
            
            // Stick detection is complete. Stick should be at max position.
            // Map the channel to the function
            _rgFunctionChannelMapping[function] = channel;
            info->function = function;
            
            // Channel should be at max value, if it is below initial set point the the channel is reversed.
            info->reversed = value < _rcValueSave[channel];
            
            if (info->reversed) {
                _rgChannelInfo[channel].rcMin = value;
            } else {
                _rgChannelInfo[channel].rcMax = value;
            }
            
Don Gagne's avatar
Don Gagne committed
413 414 415
            _signalAllAttiudeValueChanges();
            
            _advanceState();
Don Gagne's avatar
Don Gagne committed
416 417 418 419
        }
    }
}

Don Gagne's avatar
Don Gagne committed
420
void RadioComponentController::_inputStickMin(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
421 422 423 424 425 426
{
    // We only care about the channel mapped to the function we are working on
    if (_rgFunctionChannelMapping[function] != channel) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
427
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453
        // Setup up to detect stick being pegged to extreme position
        if (_rgChannelInfo[channel].reversed) {
            if (value > _rcCalPWMCenterPoint + _rcCalMoveDelta) {
                _stickDetectChannel = channel;
                _stickDetectInitialValue = value;
                _stickDetectValue = value;
            }
        } else {
            if (value < _rcCalPWMCenterPoint - _rcCalMoveDelta) {
                _stickDetectChannel = channel;
                _stickDetectInitialValue = value;
                _stickDetectValue = value;
            }
        }
    } else {
        // We are waiting for the selected channel to settle out
        
        if (_stickSettleComplete(value)) {
            ChannelInfo* info = &_rgChannelInfo[channel];
            
            // Stick detection is complete. Stick should be at min position.
            if (info->reversed) {
                _rgChannelInfo[channel].rcMax = value;
            } else {
                _rgChannelInfo[channel].rcMin = value;
            }
454 455 456 457 458 459 460

            // Check if this is throttle and set trim accordingly
            if (function == rcCalFunctionThrottle) {
                _rgChannelInfo[channel].rcTrim = value;
            }
            // XXX to support configs which can reverse they need to check a reverse
            // flag here and not do this.
Don Gagne's avatar
Don Gagne committed
461
            
Don Gagne's avatar
Don Gagne committed
462
            _advanceState();
Don Gagne's avatar
Don Gagne committed
463 464 465 466
        }
    }
}

Don Gagne's avatar
Don Gagne committed
467
void RadioComponentController::_inputCenterWait(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
468 469 470 471 472 473
{
    // We only care about the channel mapped to the function we are working on
    if (_rgFunctionChannelMapping[function] != channel) {
        return;
    }
    
Don Gagne's avatar
Don Gagne committed
474
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
475 476 477 478 479 480 481 482 483 484
        // Sticks have not yet moved close enough to center
        
        if (abs(_rcCalPWMCenterPoint - value) < _rcCalRoughCenterDelta) {
            // Stick has moved close enough to center that we can start waiting for it to settle
            _stickDetectChannel = channel;
            _stickDetectInitialValue = value;
            _stickDetectValue = value;
        }
    } else {
        if (_stickSettleComplete(value)) {
Don Gagne's avatar
Don Gagne committed
485
            _advanceState();
Don Gagne's avatar
Don Gagne committed
486 487 488 489 490
        }
    }
}

/// @brief Saves min/max for non-mapped channels
Don Gagne's avatar
Don Gagne committed
491
void RadioComponentController::_inputSwitchMinMax(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
492 493 494 495 496 497 498 499 500 501 502 503 504
{
    Q_UNUSED(function);

    // If the channel is mapped we already have min/max
    if (_rgChannelInfo[channel].function != rcCalFunctionMax) {
        return;
    }
    
    if (abs(_rcCalPWMCenterPoint - value) > _rcCalMoveDelta) {
        // Stick has moved far enough from center to consider for min/max
        if (value < _rcCalPWMCenterPoint) {
            int minValue = qMin(_rgChannelInfo[channel].rcMin, value);
            
Don Gagne's avatar
Don Gagne committed
505
            qCDebug(RadioComponentControllerLog) << "_inputSwitchMinMax setting min channel:min" << channel << minValue;
Don Gagne's avatar
Don Gagne committed
506 507 508 509 510
            
            _rgChannelInfo[channel].rcMin = minValue;
        } else {
            int maxValue = qMax(_rgChannelInfo[channel].rcMax, value);
            
Don Gagne's avatar
Don Gagne committed
511
            qCDebug(RadioComponentControllerLog) << "_inputSwitchMinMax setting max channel:max" << channel << maxValue;
Don Gagne's avatar
Don Gagne committed
512 513 514 515 516 517
            
            _rgChannelInfo[channel].rcMax = maxValue;
        }
    }
}

Don Gagne's avatar
Don Gagne committed
518
void RadioComponentController::_switchDetect(enum rcCalFunctions function, int channel, int value, bool moveToNextStep)
Don Gagne's avatar
Don Gagne committed
519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534
{
    // If this channel is already used in a mapping we can't use it again
    if (_rgChannelInfo[channel].function != rcCalFunctionMax) {
        return;
    }
    
    if (abs(_rcValueSave[channel] - value) > _rcCalMoveDelta) {
        ChannelInfo* info = &_rgChannelInfo[channel];
        
        // Switch has moved far enough to consider it as being selected for the function
        
        // Map the channel to the function
        _rgChannelInfo[channel].function = function;
        _rgFunctionChannelMapping[function] = channel;
        info->function = function;
        
Don Gagne's avatar
Don Gagne committed
535
        qCDebug(RadioComponentControllerLog) << "Function:" << function << "mapped to:" << channel;
Don Gagne's avatar
Don Gagne committed
536 537
        
        if (moveToNextStep) {
Don Gagne's avatar
Don Gagne committed
538
            _advanceState();
Don Gagne's avatar
Don Gagne committed
539 540 541 542
        }
    }
}

Don Gagne's avatar
Don Gagne committed
543
void RadioComponentController::_inputSwitchDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
544 545 546 547
{
    _switchDetect(function, channel, value, true /* move to next step after detection */);
}

548
/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
Don Gagne's avatar
Don Gagne committed
549
void RadioComponentController::_resetInternalCalibrationValues(void)
550 551
{
    // Set all raw channels to not reversed and center point values
Don Gagne's avatar
Don Gagne committed
552
    for (int i=0; i<_chanMax(); i++) {
553 554 555
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
        info->reversed = false;
Don Gagne's avatar
Don Gagne committed
556 557 558
        info->rcMin = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcMax = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcTrim = RadioComponentController::_rcCalPWMCenterPoint;
559 560
    }
    
561
    // Initialize attitude function mapping to function channel not set
562
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
563
        _rgFunctionChannelMapping[i] = _chanMax();
564 565
    }
    
Don Gagne's avatar
Don Gagne committed
566 567
    if (_px4Vehicle()) {
        // Reserve the existing Flight Mode switch settings channels so we don't re-use them
568

Don Gagne's avatar
Don Gagne committed
569 570 571 572 573 574 575 576 577 578 579
        static const rcCalFunctions rgFlightModeFunctions[] = {
            rcCalFunctionModeSwitch,
            rcCalFunctionPosCtlSwitch,
            rcCalFunctionLoiterSwitch,
            rcCalFunctionReturnSwitch };
        static const size_t crgFlightModeFunctions = sizeof(rgFlightModeFunctions) / sizeof(rgFlightModeFunctions[0]);

        for (size_t i=0; i < crgFlightModeFunctions; i++) {
            QVariant value;
            enum rcCalFunctions curFunction = rgFlightModeFunctions[i];

580 581 582 583 584
            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[curFunction].parameterName);
            if (paramFact) {
                bool ok;
                int switchChannel = paramFact->rawValue().toInt(&ok);
                Q_ASSERT(ok);
Don Gagne's avatar
Don Gagne committed
585

586 587
                // Parameter: 1-based channel, 0=not mapped
                // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped
Don Gagne's avatar
Don Gagne committed
588

589 590 591 592 593
                if (switchChannel != 0) {
                    qCDebug(RadioComponentControllerLog) << "Reserving 0-based switch channel" << switchChannel - 1;
                    _rgFunctionChannelMapping[curFunction] = switchChannel - 1;
                    _rgChannelInfo[switchChannel - 1].function = curFunction;
                }
Don Gagne's avatar
Don Gagne committed
594
            }
595 596
        }
    }
Don Gagne's avatar
Don Gagne committed
597 598
    
    _signalAllAttiudeValueChanges();
Don Gagne's avatar
Don Gagne committed
599 600
}

Don Gagne's avatar
Don Gagne committed
601
/// @brief Sets internal calibration values from the stored parameters
Don Gagne's avatar
Don Gagne committed
602
void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
Don Gagne's avatar
Don Gagne committed
603
{
604 605
    // Initialize all function mappings to not set
    
Don Gagne's avatar
Don Gagne committed
606
    for (int i=0; i<_chanMax(); i++) {
607 608 609 610 611
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
    }
    
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
612
        _rgFunctionChannelMapping[i] = _chanMax();
613 614 615 616 617 618 619 620
    }
    
    // Pull parameters and update
    
    QString minTpl("RC%1_MIN");
    QString maxTpl("RC%1_MAX");
    QString trimTpl("RC%1_TRIM");
    QString revTpl("RC%1_REV");
621
    
622 623
    bool convertOk;
    
Don Gagne's avatar
Don Gagne committed
624
    for (int i = 0; i < _chanMax(); ++i) {
625
        struct ChannelInfo* info = &_rgChannelInfo[i];
626 627 628 629 630 631 632 633 634 635 636

        if (_px4Vehicle() && _apmPossibleMissingRCChannelParams.contains(i+1)) {
            if (!parameterExists(FactSystem::defaultComponentId, minTpl.arg(i+1))) {
                // Parameter is missing from this version of APM
                info->rcTrim = 1500;
                info->rcMin = 1100;
                info->rcMax = 1900;
                info->reversed = false;
                continue;
            }
        }
Don Gagne's avatar
Don Gagne committed
637
        
638 639 640 641 642
        Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1));
        if (paramFact) {
            info->rcTrim = paramFact->rawValue().toInt(&convertOk);
            Q_ASSERT(convertOk);
        }
Don Gagne's avatar
Don Gagne committed
643
        
644 645 646 647 648
        paramFact = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(i+1));
        if (paramFact) {
            info->rcMin = paramFact->rawValue().toInt(&convertOk);
            Q_ASSERT(convertOk);
        }
Don Gagne's avatar
Don Gagne committed
649

650 651 652 653 654
        paramFact = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1));
        if (paramFact) {
            info->rcMax = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1))->rawValue().toInt(&convertOk);
            Q_ASSERT(convertOk);
        }
Don Gagne's avatar
Don Gagne committed
655

656 657 658 659 660 661 662
        paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(i+1));
        if (paramFact) {
            float floatReversed = paramFact->rawValue().toFloat(&convertOk);
            Q_ASSERT(convertOk);
            Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f);
            info->reversed = floatReversed == -1.0f;
        }
663 664 665 666
    }
    
    for (int i=0; i<rcCalFunctionMax; i++) {
        int32_t paramChannel;
Don Gagne's avatar
Don Gagne committed
667
        
Don Gagne's avatar
Don Gagne committed
668 669
        const char* paramName = _functionInfo()[i].parameterName;
        if (paramName) {
670 671 672 673 674 675 676 677 678
            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, paramName);
            if (paramFact) {
                paramChannel = paramFact->rawValue().toInt(&convertOk);
                Q_ASSERT(convertOk);

                if (paramChannel != 0) {
                    _rgFunctionChannelMapping[i] = paramChannel - 1;
                    _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i;
                }
Don Gagne's avatar
Don Gagne committed
679
            }
Don Gagne's avatar
Don Gagne committed
680 681
        }
    }
682
    
Don Gagne's avatar
Don Gagne committed
683
    _signalAllAttiudeValueChanges();
684 685
}

Don Gagne's avatar
Don Gagne committed
686
void RadioComponentController::spektrumBindMode(int mode)
Don Gagne's avatar
Don Gagne committed
687
{
Don Gagne's avatar
Don Gagne committed
688
    _uas->pairRX(0, mode);
689 690
}

Don Gagne's avatar
Don Gagne committed
691
/// @brief Validates the current settings against the calibration rules resetting values as necessary.
Don Gagne's avatar
Don Gagne committed
692
void RadioComponentController::_validateCalibration(void)
Don Gagne's avatar
Don Gagne committed
693
{
Don Gagne's avatar
Don Gagne committed
694
    for (int chan = 0; chan<_chanMax(); chan++) {
Don Gagne's avatar
Don Gagne committed
695 696 697 698 699 700
        struct ChannelInfo* info = &_rgChannelInfo[chan];
        
        if (chan < _chanCount) {
            // Validate Min/Max values. Although the channel appears as available we still may
            // not have good min/max/trim values for it. Set to defaults if needed.
            if (info->rcMin > _rcCalPWMValidMinValue || info->rcMax < _rcCalPWMValidMaxValue) {
Don Gagne's avatar
Don Gagne committed
701
                qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting channel" << chan;
Don Gagne's avatar
Don Gagne committed
702 703
                info->rcMin = _rcCalPWMDefaultMinValue;
                info->rcMax = _rcCalPWMDefaultMaxValue;
Don Gagne's avatar
Don Gagne committed
704 705 706 707 708 709 710
                info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
            } else {
                switch (_rgChannelInfo[chan].function) {
                    case rcCalFunctionThrottle:
                    case rcCalFunctionYaw:
                    case rcCalFunctionRoll:
                    case rcCalFunctionPitch:
711 712 713 714 715 716
                        // Make sure trim is within min/max
                        if (info->rcTrim < info->rcMin) {
                            info->rcTrim = info->rcMin;
                        } else if (info->rcTrim > info->rcMax) {
                            info->rcTrim = info->rcMax;
                        }
Don Gagne's avatar
Don Gagne committed
717 718 719 720 721 722 723
                        break;
                    default:
                        // Non-attitude control channels have calculated trim
                        info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
                        break;
                }
                
Don Gagne's avatar
Don Gagne committed
724 725 726
            }
        } else {
            // Unavailable channels are set to defaults
Don Gagne's avatar
Don Gagne committed
727
            qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting unavailable channel" << chan;
Don Gagne's avatar
Don Gagne committed
728 729
            info->rcMin = _rcCalPWMDefaultMinValue;
            info->rcMax = _rcCalPWMDefaultMaxValue;
Don Gagne's avatar
Don Gagne committed
730
            info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
Don Gagne's avatar
Don Gagne committed
731 732 733 734 735 736
            info->reversed = false;
        }
    }
}


737
/// @brief Saves the rc calibration values to the board parameters.
Don Gagne's avatar
Don Gagne committed
738
void RadioComponentController::_writeCalibration(void)
739
{
740
    if (!_uas) return;
741
    
742 743 744
    if (_px4Vehicle()) {
        _uas->stopCalibration();
    }
745
    
Don Gagne's avatar
Don Gagne committed
746 747
    _validateCalibration();
    
748 749 750 751 752
    QString minTpl("RC%1_MIN");
    QString maxTpl("RC%1_MAX");
    QString trimTpl("RC%1_TRIM");
    QString revTpl("RC%1_REV");
    
Don Gagne's avatar
Don Gagne committed
753
    // Note that the rc parameters are all float, so you must cast to float in order to get the right QVariant
Don Gagne's avatar
Don Gagne committed
754
    for (int chan = 0; chan<_chanMax(); chan++) {
Don Gagne's avatar
Don Gagne committed
755
        struct ChannelInfo* info = &_rgChannelInfo[chan];
756 757
        int                 oneBasedChannel = chan + 1;
        
758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774
        if (_px4Vehicle() && _apmPossibleMissingRCChannelParams.contains(chan+1) && !parameterExists(FactSystem::defaultComponentId, minTpl.arg(chan+1))) {
            // RC parameters for this channel are missing from this version of APM
            continue;
        }

        Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(oneBasedChannel));
        if (paramFact) {
            paramFact->setRawValue((float)info->rcTrim);
        }
        paramFact = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(oneBasedChannel));
        if (paramFact) {
            paramFact->setRawValue((float)info->rcMin);
        }
        paramFact = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(oneBasedChannel));
        if (paramFact) {
            paramFact->setRawValue((float)info->rcMax);
        }
775

776 777 778 779 780 781 782 783 784 785 786 787 788 789
        // For multi-rotor we can determine reverse setting during radio cal. For anything other than multi-rotor, servo installation
        // may affect channel reversing so we can't automatically determine it.
        if (_vehicle->multiRotor()) {
            // APM multi-rotor has a backwards interpretation of "reversed" on the Pitch control. So be careful.
            float reversedParamValue;
            if (_px4Vehicle() || info->function != rcCalFunctionPitch) {
                reversedParamValue = info->reversed ? -1.0f : 1.0f;
            } else {
                reversedParamValue = info->reversed ? 1.0f : -1.0f;
            }
            paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(oneBasedChannel));
            if (paramFact) {
                paramFact->setRawValue(reversedParamValue);
            }
790
        }
791 792
    }
    
Don Gagne's avatar
Don Gagne committed
793 794 795
    // Write function mapping parameters
    for (size_t i=0; i<rcCalFunctionMax; i++) {
        int32_t paramChannel;
Don Gagne's avatar
Don Gagne committed
796
        if (_rgFunctionChannelMapping[i] == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
797 798 799 800 801
            // 0 signals no mapping
            paramChannel = 0;
        } else {
            // Note that the channel value is 1-based
            paramChannel = _rgFunctionChannelMapping[i] + 1;
802
        }
Don Gagne's avatar
Don Gagne committed
803 804 805 806
        const char* paramName = _functionInfo()[i].parameterName;
        if (paramName) {
            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName);

807 808 809 810 811
            if (paramFact && paramFact->rawValue().toInt() != paramChannel) {
                paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName);
                if (paramFact) {
                    paramFact->setRawValue(paramChannel);
                }
Don Gagne's avatar
Don Gagne committed
812 813
            }
        }
814 815
    }
    
Don Gagne's avatar
Don Gagne committed
816 817 818 819 820
    if (_px4Vehicle()) {
        // If the RC_CHAN_COUNT parameter is available write the channel count
        if (parameterExists(FactSystem::defaultComponentId, "RC_CHAN_CNT")) {
            getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->setRawValue(_chanCount);
        }
821
    }
822
    
Don Gagne's avatar
Don Gagne committed
823
    _stopCalibration();
824
    _setInternalCalibrationValuesFromParameters();
825 826
}

Don Gagne's avatar
Don Gagne committed
827
/// @brief Starts the calibration process
Don Gagne's avatar
Don Gagne committed
828
void RadioComponentController::_startCalibration(void)
829 830 831
{
    Q_ASSERT(_chanCount >= _chanMinimum);
    
Don Gagne's avatar
Don Gagne committed
832 833
    _resetInternalCalibrationValues();
    
834
    // Let the mav known we are starting calibration. This should turn off motors and so forth.
835 836 837
    if (_px4Vehicle()) {
        _uas->startCalibration(UASInterface::StartCalibrationRadio);
    }
838
    
Don Gagne's avatar
Don Gagne committed
839 840
    _nextButton->setProperty("text", "Next");
    _cancelButton->setEnabled(true);
841
    
Don Gagne's avatar
Don Gagne committed
842 843
    _currentStep = 0;
    _setupCurrentState();
844 845
}

Don Gagne's avatar
Don Gagne committed
846
/// @brief Cancels the calibration process, setting things back to initial state.
Don Gagne's avatar
Don Gagne committed
847
void RadioComponentController::_stopCalibration(void)
848
{
Don Gagne's avatar
Don Gagne committed
849
    _currentStep = -1;
850
    
851
    if (_uas) {
852 853 854
        if (_px4Vehicle()) {
            _uas->stopCalibration();
        }
Don Gagne's avatar
Don Gagne committed
855 856 857
        _setInternalCalibrationValuesFromParameters();
    } else {
        _resetInternalCalibrationValues();
858 859
    }
    
Don Gagne's avatar
Don Gagne committed
860
    _statusText->setProperty("text", "");
861

Don Gagne's avatar
Don Gagne committed
862 863 864 865
    _nextButton->setProperty("text", "Calibrate");
    _nextButton->setEnabled(true);
    _cancelButton->setEnabled(false);
    _skipButton->setEnabled(false);
866 867
    
    _setHelpImage(_imageCenter);
868 869
}

Don Gagne's avatar
Don Gagne committed
870
/// @brief Saves the current channel values, so that we can detect when the use moves an input.
Don Gagne's avatar
Don Gagne committed
871
void RadioComponentController::_rcCalSaveCurrentValues(void)
872
{
Don Gagne's avatar
Don Gagne committed
873
	qCDebug(RadioComponentControllerLog) << "_rcCalSaveCurrentValues";
Don Gagne's avatar
Don Gagne committed
874
    for (int i = 0; i < _chanMax(); i++) {
Don Gagne's avatar
Don Gagne committed
875 876
        _rcValueSave[i] = _rcRawValue[i];
    }
877 878 879
}

/// @brief Set up the Save state of calibration.
Don Gagne's avatar
Don Gagne committed
880
void RadioComponentController::_rcCalSave(void)
881 882 883
{
    _rcCalState = rcCalStateSave;
    
Don Gagne's avatar
Don Gagne committed
884 885 886
    _statusText->setProperty("text",
                             "The current calibration settings are now displayed for each channel on screen.\n\n"
                                "Click the Next button to upload calibration to board. Click Cancel if you don't want to save these values.");
887

Don Gagne's avatar
Don Gagne committed
888 889 890
    _nextButton->setEnabled(true);
    _skipButton->setEnabled(false);
    _cancelButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
891 892 893 894
    
    // 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();
Don Gagne's avatar
Don Gagne committed
895 896
}

Don Gagne's avatar
Don Gagne committed
897
void RadioComponentController::_loadSettings(void)
898 899 900 901 902 903 904 905 906 907 908 909
{
    QSettings settings;
    
    settings.beginGroup(_settingsGroup);
    _transmitterMode = settings.value(_settingsKeyTransmitterMode, 2).toInt();
    settings.endGroup();
    
    if (_transmitterMode != 1 || _transmitterMode != 2) {
        _transmitterMode = 2;
    }
}

Don Gagne's avatar
Don Gagne committed
910
void RadioComponentController::_storeSettings(void)
911 912 913 914 915 916 917 918
{
    QSettings settings;
    
    settings.beginGroup(_settingsGroup);
    settings.setValue(_settingsKeyTransmitterMode, _transmitterMode);
    settings.endGroup();
}

Don Gagne's avatar
Don Gagne committed
919
void RadioComponentController::_setHelpImage(const char* imageFile)
920 921 922 923 924 925 926 927 928 929 930 931
{
    QString file = _imageFilePrefix;
    
    if (_transmitterMode == 1) {
        file += _imageFileMode1Dir;
    } else if (_transmitterMode == 2) {
        file += _imageFileMode2Dir;
    } else {
        Q_ASSERT(false);
    }
    file += imageFile;
    
Don Gagne's avatar
Don Gagne committed
932
    qCDebug(RadioComponentControllerLog) << "_setHelpImage" << file;
933
    
Don Gagne's avatar
Don Gagne committed
934 935 936 937 938 939 940 941 942 943 944
    _imageHelp = file;
    emit imageHelpChanged(file);
}

int RadioComponentController::channelCount(void)
{
    return _chanCount;
}

int RadioComponentController::rollChannelRCValue(void)
{    
Don Gagne's avatar
Don Gagne committed
945
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
946 947 948 949 950 951 952 953
        return _rcRawValue[rcCalFunctionRoll];
    } else {
        return 1500;
    }
}

int RadioComponentController::pitchChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
954
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
955 956 957 958 959 960 961 962
        return _rcRawValue[rcCalFunctionPitch];
    } else {
        return 1500;
    }
}

int RadioComponentController::yawChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
963
    if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
964 965 966 967 968 969 970 971
        return _rcRawValue[rcCalFunctionYaw];
    } else {
        return 1500;
    }
}

int RadioComponentController::throttleChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
972
    if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
973 974 975 976 977 978 979 980
        return _rcRawValue[rcCalFunctionThrottle];
    } else {
        return 1500;
    }
}

bool RadioComponentController::rollChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
981
    return _rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax();
Don Gagne's avatar
Don Gagne committed
982 983 984 985
}

bool RadioComponentController::pitchChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
986
    return _rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax();
Don Gagne's avatar
Don Gagne committed
987 988 989 990
}

bool RadioComponentController::yawChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
991
    return _rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax();
Don Gagne's avatar
Don Gagne committed
992 993 994 995
}

bool RadioComponentController::throttleChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
996
    return _rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax();
Don Gagne's avatar
Don Gagne committed
997 998 999 1000
}

bool RadioComponentController::rollChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1001
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1002 1003 1004 1005 1006 1007 1008 1009
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionRoll]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::pitchChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1010
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1011 1012 1013 1014 1015 1016 1017 1018
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionPitch]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::yawChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1019
    if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1020 1021 1022 1023 1024 1025 1026 1027
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionYaw]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::throttleChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1028
    if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionThrottle]].reversed;
    } else {
        return false;
    }
}

void RadioComponentController::setTransmitterMode(int mode)
{
    if (mode == 1 || mode == 2) {
        _transmitterMode = mode;
        if (_currentStep != -1) {
            const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
            _setHelpImage(state->image);
        }
    }
}

void RadioComponentController::_signalAllAttiudeValueChanges(void)
{
    emit rollChannelMappedChanged(rollChannelMapped());
    emit pitchChannelMappedChanged(pitchChannelMapped());
    emit yawChannelMappedChanged(yawChannelMapped());
    emit throttleChannelMappedChanged(throttleChannelMapped());
    
    emit rollChannelReversedChanged(rollChannelReversed());
    emit pitchChannelReversedChanged(pitchChannelReversed());
    emit yawChannelReversedChanged(yawChannelReversed());
    emit throttleChannelReversedChanged(throttleChannelReversed());
1057
}
Don Gagne's avatar
Don Gagne committed
1058 1059 1060

void RadioComponentController::copyTrims(void)
{
1061
    _uas->startCalibration(UASInterface::StartCalibrationCopyTrims);
Don Gagne's avatar
Don Gagne committed
1062
}
Don Gagne's avatar
Don Gagne committed
1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077

bool RadioComponentController::_px4Vehicle(void) const
{
    return _vehicle->firmwareType() == MAV_AUTOPILOT_PX4;
}

const struct RadioComponentController::FunctionInfo* RadioComponentController::_functionInfo(void) const
{
    return _px4Vehicle() ? _rgFunctionInfoPX4 : _rgFunctionInfoAPM;
}

int RadioComponentController::_chanMax(void) const
{
    return _px4Vehicle() ? _chanMaxPX4 : _chanMaxAPM;
}