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

/// @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"
Don Gagne's avatar
Don Gagne committed
16
#include "QGCApplication.h"
17

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

QGC_LOGGING_CATEGORY(RadioComponentControllerLog, "RadioComponentControllerLog")
21
QGC_LOGGING_CATEGORY(RadioComponentControllerVerboseLog, "RadioComponentControllerVerboseLog")
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;
30 31 32 33 34 35 36 37
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
Don Gagne's avatar
Don Gagne committed
38 39 40

const int RadioComponentController::_stickDetectSettleMSecs = 500;

41
const char*  RadioComponentController::_imageFilePrefix =   "calibration/";
Don Gagne's avatar
Don Gagne committed
42 43
const char*  RadioComponentController::_imageFileMode1Dir = "mode1/";
const char*  RadioComponentController::_imageFileMode2Dir = "mode2/";
44 45 46
const char*  RadioComponentController::_imageCenter =       "radioCenter.png";
const char*  RadioComponentController::_imageHome =         "radioHome.png";
const char*  RadioComponentController::_imageThrottleUp =   "radioThrottleUp.png";
Don Gagne's avatar
Don Gagne committed
47
const char*  RadioComponentController::_imageThrottleDown = "radioThrottleDown.png";
48 49 50 51 52 53
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";
Don Gagne's avatar
Don Gagne committed
54 55
const char*  RadioComponentController::_imageSwitchMinMax = "radioSwitchMinMax.png";

56
const char* RadioComponentController::_settingsGroup =              "RadioCalibration";
Don Gagne's avatar
Don Gagne committed
57 58
const char* RadioComponentController::_settingsKeyTransmitterMode = "TransmitterMode";

59 60 61
const char* RadioComponentController::_px4RevParamFormat =      "RC%1_REV";
const char* RadioComponentController::_apmNewRevParamFormat =   "RC%1_REVERSED";

Don Gagne's avatar
Don Gagne committed
62
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoPX4[RadioComponentController::rcCalFunctionMax] = {
63 64 65 66
{ "RC_MAP_ROLL" },
{ "RC_MAP_PITCH" },
{ "RC_MAP_YAW" },
{ "RC_MAP_THROTTLE" }
67 68
};

Don Gagne's avatar
Don Gagne committed
69
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoAPM[RadioComponentController::rcCalFunctionMax] = {
70 71 72 73
{ "RCMAP_ROLL" },
{ "RCMAP_PITCH" },
{ "RCMAP_YAW" },
{ "RCMAP_THROTTLE" }
Don Gagne's avatar
Don Gagne committed
74 75
};

76 77 78 79 80 81 82 83 84 85
RadioComponentController::RadioComponentController(void)
    : _currentStep(-1)
    , _transmitterMode(2)
    , _chanCount(0)
    , _rcCalState(rcCalStateChannelWait)
    , _unitTestMode(false)
    , _statusText(NULL)
    , _cancelButton(NULL)
    , _nextButton(NULL)
    , _skipButton(NULL)
86
{
87 88 89 90 91
#ifdef UNITTEST_BUILD
    // Nasty hack to expose controller to unit test code
    _unitTestController = this;
#endif

92 93 94 95 96 97 98 99 100 101
    if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("RC1_REVERSED"))) {
        // Newer ardupilot firmwares have a different reverse param naming scheme and value scheme
        _revParamFormat = _apmNewRevParamFormat;
        _revParamIsBool = true; // param value is boolean 0/1 for reversed or not
    } else {
        // Older ardupilot firmwares share the same naming convention as PX4
        _revParamFormat = _px4RevParamFormat;
        _revParamIsBool = false;    // paeram value if -1 indicates reversed
    }

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
{
126
    static const char* msgBeginPX4 =        QT_TR_NOOP("Lower the Throttle stick all the way down as shown in diagram.\n\n"
Don Gagne's avatar
Don Gagne committed
127
                                            "It is recommended to disconnect all motors for additional safety, however, the system is designed to not arm during the calibration.\n\n"
128 129
                                            "Click Next to continue");
    static const char* msgBeginAPM =        QT_TR_NOOP("Lower the Throttle stick all the way down as shown in diagram.\nReset all transmitter trims to center.\n\n"
Don Gagne's avatar
Don Gagne committed
130
                                            "Please ensure all motor power is disconnected AND all props are removed from the vehicle.\n\n"
131 132 133 134 135 136 137 138 139 140 141 142
                                            "Click Next to continue");
    static const char* msgThrottleUp =      QT_TR_NOOP("Move the Throttle stick all the way up and hold it there...");
    static const char* msgThrottleDown =    QT_TR_NOOP("Move the Throttle stick all the way down and leave it there...");
    static const char* msgYawLeft =         QT_TR_NOOP("Move the Yaw stick all the way to the left and hold it there...");
    static const char* msgYawRight =        QT_TR_NOOP("Move the Yaw stick all the way to the right and hold it there...");
    static const char* msgRollLeft =        QT_TR_NOOP("Move the Roll stick all the way to the left and hold it there...");
    static const char* msgRollRight =       QT_TR_NOOP("Move the Roll stick all the way to the right and hold it there...");
    static const char* msgPitchDown =       QT_TR_NOOP("Move the Pitch stick all the way down and hold it there...");
    static const char* msgPitchUp =         QT_TR_NOOP("Move the Pitch stick all the way up and hold it there...");
    static const char* msgPitchCenter =     QT_TR_NOOP("Allow the Pitch stick to move back to center...");
    static const char* msgSwitchMinMax =    QT_TR_NOOP("Move all the transmitter switches and/or dials back and forth to their extreme positions.");
    static const char* msgComplete =        QT_TR_NOOP("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
    static const char* msgBeginAPMRover = QT_TR_NOOP("Center the Throttle stick as shown in diagram.\nReset all transmitter trims to center.\n\n"
Don Gagne's avatar
Don Gagne committed
210
                                          "Please ensure all motor power is disconnected from the vehicle.\n\n"
211
                                          "Click Next to continue");
Don Gagne's avatar
Don Gagne committed
212 213 214 215 216 217 218 219 220 221 222
    const stateMachineEntry* state = _getStateMachineEntry(_currentStep);

    const char* instructions = state->instructions;
    const char* helpImage = state->image;
    if (_vehicle->rover() && _currentStep == 0) {
        // Hack in center throttle start for Rover. This is to set the correct centered trim for throttle.
        instructions = msgBeginAPMRover;
        helpImage = _imageCenter;
    }
    _statusText->setProperty("text", instructions);
    _setHelpImage(helpImage);
Don Gagne's avatar
Don Gagne committed
223
    
Don Gagne's avatar
Don Gagne committed
224
    _stickDetectChannel = _chanMax();
Don Gagne's avatar
Don Gagne committed
225 226 227 228
    _stickDetectSettleStarted = false;
    
    _rcCalSaveCurrentValues();
    
Don Gagne's avatar
Don Gagne committed
229 230
    _nextButton->setEnabled(state->nextFn != NULL);
    _skipButton->setEnabled(state->skipFn != NULL);
Don Gagne's avatar
Don Gagne committed
231 232
}

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

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

        if (channelValue != -1) {
242
            qCDebug(RadioComponentControllerVerboseLog) << "Raw value" << channel << channelValue;
Don Gagne's avatar
Don Gagne committed
243 244 245 246 247 248 249

            _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
250
                case rcCalFunctionRoll:
Don Gagne's avatar
Don Gagne committed
251
                    emit rollChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
252 253
                    break;
                case rcCalFunctionPitch:
Don Gagne's avatar
Don Gagne committed
254
                    emit pitchChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
255 256
                    break;
                case rcCalFunctionYaw:
Don Gagne's avatar
Don Gagne committed
257
                    emit yawChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
258 259
                    break;
                case rcCalFunctionThrottle:
Don Gagne's avatar
Don Gagne committed
260
                    emit throttleChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
261 262 263 264
                    break;
                default:
                    break;

Don Gagne's avatar
Don Gagne committed
265
                }
Don Gagne's avatar
Don Gagne committed
266
            }
Don Gagne's avatar
Don Gagne committed
267 268 269 270 271 272 273 274

            if (_currentStep == -1) {
                if (_chanCount != channelCount) {
                    _chanCount = channelCount;
                    emit channelCountChanged(_chanCount);
                }
            } else {
                const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
275 276 277 278 279 280
                if (state) {
                    if (state->rcInputFn) {
                        (this->*state->rcInputFn)(state->function, channel, channelValue);
                    }
                } else {
                    qWarning() << "Internal error: NULL _getStateMachineEntry return";
Don Gagne's avatar
Don Gagne committed
281
                }
Don Gagne's avatar
Don Gagne committed
282
            }
Don Gagne's avatar
Don Gagne committed
283 284 285 286
        }
    }
}

Don Gagne's avatar
Don Gagne committed
287
void RadioComponentController::nextButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
288 289 290 291 292 293 294
{
    if (_currentStep == -1) {
        // Need to have enough channels
        if (_chanCount < _chanMinimum) {
            if (_unitTestMode) {
                emit nextButtonMessageBoxDisplayed();
            } else {
295
                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
296 297 298 299 300 301
            }
            return;
        }
        _startCalibration();
    } else {
        const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
302 303 304 305 306
        if (state && state->nextFn) {
            (this->*state->nextFn)();
        } else {
            qWarning() << "Internal error: NULL _getStateMachineEntry return";
        }
Don Gagne's avatar
Don Gagne committed
307 308 309
    }
}

Don Gagne's avatar
Don Gagne committed
310
void RadioComponentController::skipButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
311
{
312 313 314 315
    if (_currentStep == -1) {
        qWarning() << "Internal error: _currentStep == -1";
        return;
    }
Don Gagne's avatar
Don Gagne committed
316 317
    
    const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
318 319 320 321 322
    if (state && state->skipFn) {
        (this->*state->skipFn)();
    } else {
        qWarning() << "Internal error: NULL _getStateMachineEntry return";
    }
Don Gagne's avatar
Don Gagne committed
323 324
}

Don Gagne's avatar
Don Gagne committed
325
void RadioComponentController::cancelButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
326
{
Don Gagne's avatar
Don Gagne committed
327
    _stopCalibration();
Don Gagne's avatar
Don Gagne committed
328 329
}

Don Gagne's avatar
Don Gagne committed
330
void RadioComponentController::_saveAllTrims(void)
Don Gagne's avatar
Don Gagne committed
331 332 333 334 335 336 337
{
    // 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++) {
338
        qCDebug(RadioComponentControllerLog) << "_saveAllTrims channel trim" << i<< _rcRawValue[i];
Don Gagne's avatar
Don Gagne committed
339 340
        _rgChannelInfo[i].rcTrim = _rcRawValue[i];
    }
Don Gagne's avatar
Don Gagne committed
341
    _advanceState();
Don Gagne's avatar
Don Gagne committed
342 343 344
}

/// @brief Waits for the sticks to be centered, enabling Next when done.
Don Gagne's avatar
Don Gagne committed
345
void RadioComponentController::_inputCenterWaitBegin(enum rcCalFunctions function, int chan, int value)
Don Gagne's avatar
Don Gagne committed
346 347 348 349 350 351 352
{
    Q_UNUSED(function);
    Q_UNUSED(chan);
    Q_UNUSED(value);
    
    // FIXME: Doesn't wait for center
    
Don Gagne's avatar
Don Gagne committed
353
    _nextButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
354 355
}

Don Gagne's avatar
Don Gagne committed
356
bool RadioComponentController::_stickSettleComplete(int value)
Don Gagne's avatar
Don Gagne committed
357 358 359 360 361 362
{
    // 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
363
        qCDebug(RadioComponentControllerLog) << "_stickSettleComplete still moving, _stickDetectValue:value" << _stickDetectValue << value;
Don Gagne's avatar
Don Gagne committed
364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379

        _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
380
            qCDebug(RadioComponentControllerLog) << "_stickSettleComplete starting settle timer, _stickDetectValue:value" << _stickDetectValue << value;
Don Gagne's avatar
Don Gagne committed
381 382 383 384 385 386 387 388 389
            
            _stickDetectSettleStarted = true;
            _stickDetectSettleElapsed.start();
        }
    }
    
    return false;
}

Don Gagne's avatar
Don Gagne committed
390
void RadioComponentController::_inputStickDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
391
{
Don Gagne's avatar
Don Gagne committed
392
    qCDebug(RadioComponentControllerLog) << "_inputStickDetect function:channel:value" << _functionInfo()[function].parameterName << channel << value;
Don Gagne's avatar
Don Gagne committed
393 394 395 396 397
    
    // If this channel is already used in a mapping we can't use it again
    if (_rgChannelInfo[channel].function != rcCalFunctionMax) {
        return;
    }
398
    
Don Gagne's avatar
Don Gagne committed
399
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
400 401 402 403 404
        // 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
405
            qCDebug(RadioComponentControllerLog) << "_inputStickDetect starting settle wait, function:channel:value" << function << channel << value;
Don Gagne's avatar
Don Gagne committed
406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423
            
            // 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];
            
            // 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];
            
424 425
            qCDebug(RadioComponentControllerLog) << "_inputStickDetect settle complete, function:channel:value:reversed" << function << channel << value << info->reversed;

Don Gagne's avatar
Don Gagne committed
426 427 428 429 430 431
            if (info->reversed) {
                _rgChannelInfo[channel].rcMin = value;
            } else {
                _rgChannelInfo[channel].rcMax = value;
            }
            
nanthony21's avatar
nanthony21 committed
432
            _signalAllAttitudeValueChanges();
Don Gagne's avatar
Don Gagne committed
433 434
            
            _advanceState();
Don Gagne's avatar
Don Gagne committed
435 436 437 438
        }
    }
}

Don Gagne's avatar
Don Gagne committed
439
void RadioComponentController::_inputStickMin(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
440 441 442 443 444 445
{
    // 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
446
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472
        // 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;
            }
473 474 475 476 477 478 479

            // 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
480
            
Don Gagne's avatar
Don Gagne committed
481
            _advanceState();
Don Gagne's avatar
Don Gagne committed
482 483 484 485
        }
    }
}

Don Gagne's avatar
Don Gagne committed
486
void RadioComponentController::_inputCenterWait(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
487 488 489 490 491 492
{
    // 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
493
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
494 495 496 497 498 499 500 501 502 503
        // 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
504
            _advanceState();
Don Gagne's avatar
Don Gagne committed
505 506 507 508 509
        }
    }
}

/// @brief Saves min/max for non-mapped channels
Don Gagne's avatar
Don Gagne committed
510
void RadioComponentController::_inputSwitchMinMax(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
511 512 513 514 515 516 517 518 519 520 521 522 523
{
    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
524
            qCDebug(RadioComponentControllerLog) << "_inputSwitchMinMax setting min channel:min" << channel << minValue;
Don Gagne's avatar
Don Gagne committed
525 526 527 528 529
            
            _rgChannelInfo[channel].rcMin = minValue;
        } else {
            int maxValue = qMax(_rgChannelInfo[channel].rcMax, value);
            
Don Gagne's avatar
Don Gagne committed
530
            qCDebug(RadioComponentControllerLog) << "_inputSwitchMinMax setting max channel:max" << channel << maxValue;
Don Gagne's avatar
Don Gagne committed
531 532 533 534 535 536
            
            _rgChannelInfo[channel].rcMax = maxValue;
        }
    }
}

Don Gagne's avatar
Don Gagne committed
537
void RadioComponentController::_switchDetect(enum rcCalFunctions function, int channel, int value, bool moveToNextStep)
Don Gagne's avatar
Don Gagne committed
538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553
{
    // 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
554
        qCDebug(RadioComponentControllerLog) << "Function:" << function << "mapped to:" << channel;
Don Gagne's avatar
Don Gagne committed
555 556
        
        if (moveToNextStep) {
Don Gagne's avatar
Don Gagne committed
557
            _advanceState();
Don Gagne's avatar
Don Gagne committed
558 559 560 561
        }
    }
}

Don Gagne's avatar
Don Gagne committed
562
void RadioComponentController::_inputSwitchDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
563 564 565 566
{
    _switchDetect(function, channel, value, true /* move to next step after detection */);
}

567
/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
Don Gagne's avatar
Don Gagne committed
568
void RadioComponentController::_resetInternalCalibrationValues(void)
569 570
{
    // Set all raw channels to not reversed and center point values
Don Gagne's avatar
Don Gagne committed
571
    for (int i=0; i<_chanMax(); i++) {
572 573 574
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
        info->reversed = false;
Don Gagne's avatar
Don Gagne committed
575 576 577
        info->rcMin = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcMax = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcTrim = RadioComponentController::_rcCalPWMCenterPoint;
578 579
    }
    
580
    // Initialize attitude function mapping to function channel not set
581
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
582
        _rgFunctionChannelMapping[i] = _chanMax();
583 584
    }
    
nanthony21's avatar
nanthony21 committed
585
    _signalAllAttitudeValueChanges();
Don Gagne's avatar
Don Gagne committed
586 587
}

Don Gagne's avatar
Don Gagne committed
588
/// @brief Sets internal calibration values from the stored parameters
Don Gagne's avatar
Don Gagne committed
589
void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
Don Gagne's avatar
Don Gagne committed
590
{
591 592
    // Initialize all function mappings to not set
    
Don Gagne's avatar
Don Gagne committed
593
    for (int i=0; i<_chanMax(); i++) {
594 595 596 597 598
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
    }
    
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
599
        _rgFunctionChannelMapping[i] = _chanMax();
600 601 602 603 604 605 606 607
    }
    
    // Pull parameters and update
    
    QString minTpl("RC%1_MIN");
    QString maxTpl("RC%1_MAX");
    QString trimTpl("RC%1_TRIM");
    
Don Gagne's avatar
Don Gagne committed
608
    for (int i = 0; i < _chanMax(); ++i) {
609
        struct ChannelInfo* info = &_rgChannelInfo[i];
610 611 612 613 614 615 616 617 618 619 620

        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
621
        
622 623
        Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1));
        if (paramFact) {
624
            info->rcTrim = paramFact->rawValue().toInt();
625
        }
Don Gagne's avatar
Don Gagne committed
626
        
627 628
        paramFact = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(i+1));
        if (paramFact) {
629
            info->rcMin = paramFact->rawValue().toInt();
630
        }
Don Gagne's avatar
Don Gagne committed
631

632 633
        paramFact = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1));
        if (paramFact) {
634
            info->rcMax = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1))->rawValue().toInt();
635
        }
Don Gagne's avatar
Don Gagne committed
636

637
        info->reversed = _channelReversedParamValue(i);
638 639 640 641
    }
    
    for (int i=0; i<rcCalFunctionMax; i++) {
        int32_t paramChannel;
Don Gagne's avatar
Don Gagne committed
642
        
Don Gagne's avatar
Don Gagne committed
643 644
        const char* paramName = _functionInfo()[i].parameterName;
        if (paramName) {
645 646
            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, paramName);
            if (paramFact) {
647
                paramChannel = paramFact->rawValue().toInt();
648

649
                if (paramChannel > 0 && paramChannel <= _chanMax()) {
650 651 652
                    _rgFunctionChannelMapping[i] = paramChannel - 1;
                    _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i;
                }
Don Gagne's avatar
Don Gagne committed
653
            }
Don Gagne's avatar
Don Gagne committed
654 655
        }
    }
656
    
nanthony21's avatar
nanthony21 committed
657
    _signalAllAttitudeValueChanges();
658 659
}

Don Gagne's avatar
Don Gagne committed
660
void RadioComponentController::spektrumBindMode(int mode)
Don Gagne's avatar
Don Gagne committed
661
{
Don Gagne's avatar
Don Gagne committed
662
    _uas->pairRX(0, mode);
663 664
}

Don Gagne's avatar
Don Gagne committed
665
/// @brief Validates the current settings against the calibration rules resetting values as necessary.
Don Gagne's avatar
Don Gagne committed
666
void RadioComponentController::_validateCalibration(void)
Don Gagne's avatar
Don Gagne committed
667
{
Don Gagne's avatar
Don Gagne committed
668
    for (int chan = 0; chan<_chanMax(); chan++) {
Don Gagne's avatar
Don Gagne committed
669 670 671 672 673 674
        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
675
                qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting channel" << chan;
Don Gagne's avatar
Don Gagne committed
676 677
                info->rcMin = _rcCalPWMDefaultMinValue;
                info->rcMax = _rcCalPWMDefaultMaxValue;
Don Gagne's avatar
Don Gagne committed
678 679 680
                info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
            } else {
                switch (_rgChannelInfo[chan].function) {
681 682 683 684 685 686 687 688 689 690 691 692 693 694 695
                case rcCalFunctionThrottle:
                case rcCalFunctionYaw:
                case rcCalFunctionRoll:
                case rcCalFunctionPitch:
                    // 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;
                    }
                    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
696 697
                }
                
Don Gagne's avatar
Don Gagne committed
698 699 700
            }
        } else {
            // Unavailable channels are set to defaults
Don Gagne's avatar
Don Gagne committed
701
            qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting unavailable 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
            info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
Don Gagne's avatar
Don Gagne committed
705 706 707 708 709 710
            info->reversed = false;
        }
    }
}


711
/// @brief Saves the rc calibration values to the board parameters.
Don Gagne's avatar
Don Gagne committed
712
void RadioComponentController::_writeCalibration(void)
713
{
714
    if (!_uas) return;
715
    
716 717 718
    if (_px4Vehicle()) {
        _uas->stopCalibration();
    }
719

Don Gagne's avatar
Don Gagne committed
720
    if (!_px4Vehicle() && (_vehicle->vehicleType() == MAV_TYPE_HELICOPTER || _vehicle->multiRotor()) &&  _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionThrottle]].reversed) {
721 722 723 724 725
        // A reversed throttle could lead to dangerous power up issues if the firmware doesn't handle it absolutely correctly in all places.
        // So in this case fail the calibration for anything other than PX4 which is known to be able to handle this correctly.
        emit throttleReversedCalFailure();
    } else {
        _validateCalibration();
726

727 728 729 730 731 732 733 734 735 736 737 738
        QString minTpl("RC%1_MIN");
        QString maxTpl("RC%1_MAX");
        QString trimTpl("RC%1_TRIM");

        // Note that the rc parameters are all float, so you must cast to float in order to get the right QVariant
        for (int chan = 0; chan<_chanMax(); chan++) {
            struct ChannelInfo* info = &_rgChannelInfo[chan];
            int                 oneBasedChannel = chan + 1;

            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;
739
            }
740 741

            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(oneBasedChannel));
742
            if (paramFact) {
743 744 745 746 747 748 749 750 751
                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);
752
            }
Don Gagne's avatar
Don Gagne committed
753

754
            // For multi-rotor we can determine reverse setting during radio cal. For anything other than multi-rotor, servo installation
Don Gagne's avatar
Don Gagne committed
755 756
            // may affect channel reversing so we can't automatically determine it. This is ok for PX4 given how it uses mixers, but not for ArduPilot.
            if (_vehicle->px4Firmware() || _vehicle->multiRotor()) {
757
                // APM multi-rotor has a backwards interpretation of "reversed" on the Pitch control. So be careful.
758
                bool reversed;
759
                if (_px4Vehicle() || info->function != rcCalFunctionPitch) {
760
                    reversed = info->reversed;
761
                } else {
762
                    reversed = !info->reversed;
763
                }
764
                _setChannelReversedParamValue(chan, reversed);
765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786
            }
        }

        // Write function mapping parameters
        for (size_t i=0; i<rcCalFunctionMax; i++) {
            int32_t paramChannel;
            if (_rgFunctionChannelMapping[i] == _chanMax()) {
                // 0 signals no mapping
                paramChannel = 0;
            } else {
                // Note that the channel value is 1-based
                paramChannel = _rgFunctionChannelMapping[i] + 1;
            }
            const char* paramName = _functionInfo()[i].parameterName;
            if (paramName) {
                Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName);

                if (paramFact && paramFact->rawValue().toInt() != paramChannel) {
                    paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName);
                    if (paramFact) {
                        paramFact->setRawValue(paramChannel);
                    }
787
                }
Don Gagne's avatar
Don Gagne committed
788 789
            }
        }
790
    }
791

Don Gagne's avatar
Don Gagne committed
792 793
    if (_px4Vehicle()) {
        // If the RC_CHAN_COUNT parameter is available write the channel count
Don Gagne's avatar
Don Gagne committed
794 795
        if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("RC_CHAN_CNT"))) {
            getParameterFact(FactSystem::defaultComponentId, QStringLiteral("RC_CHAN_CNT"))->setRawValue(_chanCount);
Don Gagne's avatar
Don Gagne committed
796
        }
797
    }
798
    
Don Gagne's avatar
Don Gagne committed
799
    _stopCalibration();
800
    _setInternalCalibrationValuesFromParameters();
801 802
}

Don Gagne's avatar
Don Gagne committed
803
/// @brief Starts the calibration process
Don Gagne's avatar
Don Gagne committed
804
void RadioComponentController::_startCalibration(void)
805
{
806 807 808 809
    if (_chanCount < _chanMinimum) {
        qWarning() << "Call to RadioComponentController::_startCalibration with _chanCount < _chanMinimum";
        return;
    }
810
    
Don Gagne's avatar
Don Gagne committed
811 812
    _resetInternalCalibrationValues();
    
813
    // Let the mav known we are starting calibration. This should turn off motors and so forth.
814 815 816
    if (_px4Vehicle()) {
        _uas->startCalibration(UASInterface::StartCalibrationRadio);
    }
817
    
Don Gagne's avatar
Don Gagne committed
818
    _nextButton->setProperty("text", tr("Next"));
Don Gagne's avatar
Don Gagne committed
819
    _cancelButton->setEnabled(true);
820
    
Don Gagne's avatar
Don Gagne committed
821 822
    _currentStep = 0;
    _setupCurrentState();
823 824
}

Don Gagne's avatar
Don Gagne committed
825
/// @brief Cancels the calibration process, setting things back to initial state.
Don Gagne's avatar
Don Gagne committed
826
void RadioComponentController::_stopCalibration(void)
827
{
Don Gagne's avatar
Don Gagne committed
828
    _currentStep = -1;
829
    
830
    if (_uas) {
831 832 833
        if (_px4Vehicle()) {
            _uas->stopCalibration();
        }
Don Gagne's avatar
Don Gagne committed
834 835 836
        _setInternalCalibrationValuesFromParameters();
    } else {
        _resetInternalCalibrationValues();
837 838
    }
    
Don Gagne's avatar
Don Gagne committed
839
    _statusText->setProperty("text", "");
840

Don Gagne's avatar
Don Gagne committed
841
    _nextButton->setProperty("text", tr("Calibrate"));
Don Gagne's avatar
Don Gagne committed
842 843 844
    _nextButton->setEnabled(true);
    _cancelButton->setEnabled(false);
    _skipButton->setEnabled(false);
845 846
    
    _setHelpImage(_imageCenter);
847 848
}

Don Gagne's avatar
Don Gagne committed
849
/// @brief Saves the current channel values, so that we can detect when the use moves an input.
Don Gagne's avatar
Don Gagne committed
850
void RadioComponentController::_rcCalSaveCurrentValues(void)
851
{
Don Gagne's avatar
Don Gagne committed
852
    for (int i = 0; i < _chanMax(); i++) {
Don Gagne's avatar
Don Gagne committed
853
        _rcValueSave[i] = _rcRawValue[i];
854
        qCDebug(RadioComponentControllerLog) << "_rcCalSaveCurrentValues channel:value" << i << _rcValueSave[i];
Don Gagne's avatar
Don Gagne committed
855
    }
856 857 858
}

/// @brief Set up the Save state of calibration.
Don Gagne's avatar
Don Gagne committed
859
void RadioComponentController::_rcCalSave(void)
860 861 862
{
    _rcCalState = rcCalStateSave;
    
Don Gagne's avatar
Don Gagne committed
863
    _statusText->setProperty("text",
Don Gagne's avatar
Don Gagne committed
864 865
                             tr("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."));
866

Don Gagne's avatar
Don Gagne committed
867 868 869
    _nextButton->setEnabled(true);
    _skipButton->setEnabled(false);
    _cancelButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
870 871 872 873
    
    // 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
874 875
}

Don Gagne's avatar
Don Gagne committed
876
void RadioComponentController::_loadSettings(void)
877 878 879 880 881 882 883 884 885 886 887 888
{
    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
889
void RadioComponentController::_storeSettings(void)
890 891 892 893 894 895 896 897
{
    QSettings settings;
    
    settings.beginGroup(_settingsGroup);
    settings.setValue(_settingsKeyTransmitterMode, _transmitterMode);
    settings.endGroup();
}

Don Gagne's avatar
Don Gagne committed
898
void RadioComponentController::_setHelpImage(const char* imageFile)
899 900 901 902 903 904 905 906
{
    QString file = _imageFilePrefix;
    
    if (_transmitterMode == 1) {
        file += _imageFileMode1Dir;
    } else if (_transmitterMode == 2) {
        file += _imageFileMode2Dir;
    } else {
907 908
        qWarning() << "Internal error: Bad _transmitterMode value";
        return;
909 910 911
    }
    file += imageFile;
    
Don Gagne's avatar
Don Gagne committed
912
    qCDebug(RadioComponentControllerLog) << "_setHelpImage" << file;
913
    
Don Gagne's avatar
Don Gagne committed
914 915 916 917 918 919 920 921 922 923 924
    _imageHelp = file;
    emit imageHelpChanged(file);
}

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

int RadioComponentController::rollChannelRCValue(void)
{    
Don Gagne's avatar
Don Gagne committed
925
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
926 927 928 929 930 931 932 933
        return _rcRawValue[rcCalFunctionRoll];
    } else {
        return 1500;
    }
}

int RadioComponentController::pitchChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
934
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
935 936 937 938 939 940 941 942
        return _rcRawValue[rcCalFunctionPitch];
    } else {
        return 1500;
    }
}

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

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

bool RadioComponentController::rollChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
961
    return _rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax();
Don Gagne's avatar
Don Gagne committed
962 963 964 965
}

bool RadioComponentController::pitchChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
966
    return _rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax();
Don Gagne's avatar
Don Gagne committed
967 968 969 970
}

bool RadioComponentController::yawChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
971
    return _rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax();
Don Gagne's avatar
Don Gagne committed
972 973 974 975
}

bool RadioComponentController::throttleChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
976
    return _rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax();
Don Gagne's avatar
Don Gagne committed
977 978 979 980
}

bool RadioComponentController::rollChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
981
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
982 983 984 985 986 987 988 989
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionRoll]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::pitchChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
990
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
991 992 993 994 995 996 997 998
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionPitch]].reversed;
    } else {
        return false;
    }
}

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

bool RadioComponentController::throttleChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1008
    if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025
        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);
        }
    }
}

nanthony21's avatar
nanthony21 committed
1026
void RadioComponentController::_signalAllAttitudeValueChanges(void)
Don Gagne's avatar
Don Gagne committed
1027 1028 1029 1030 1031 1032 1033 1034 1035 1036
{
    emit rollChannelMappedChanged(rollChannelMapped());
    emit pitchChannelMappedChanged(pitchChannelMapped());
    emit yawChannelMappedChanged(yawChannelMapped());
    emit throttleChannelMappedChanged(throttleChannelMapped());
    
    emit rollChannelReversedChanged(rollChannelReversed());
    emit pitchChannelReversedChanged(pitchChannelReversed());
    emit yawChannelReversedChanged(yawChannelReversed());
    emit throttleChannelReversedChanged(throttleChannelReversed());
1037
}
Don Gagne's avatar
Don Gagne committed
1038 1039 1040

void RadioComponentController::copyTrims(void)
{
1041
    _uas->startCalibration(UASInterface::StartCalibrationCopyTrims);
Don Gagne's avatar
Don Gagne committed
1042
}
Don Gagne's avatar
Don Gagne committed
1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057

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;
}
1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088

bool RadioComponentController::_channelReversedParamValue(int channel)
{
    Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _revParamFormat.arg(channel+1));
    if (paramFact) {
        if (_revParamIsBool) {
            return paramFact->rawValue().toBool();
        } else {
            bool convertOk;
            float floatReversed = paramFact->rawValue().toFloat(&convertOk);
            if (!convertOk) {
                floatReversed = 1.0f;
            }
            return floatReversed == -1.0f;
        }
    }

    return false;
}

void RadioComponentController::_setChannelReversedParamValue(int channel, bool reversed)
{
    Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _revParamFormat.arg(channel+1));
    if (paramFact) {
        if (_revParamIsBool) {
            paramFact->setRawValue(reversed);
        } else {
            paramFact->setRawValue(reversed ? -1.0f : 1.0f);
        }
    }
}