RadioComponentController.cc 46.7 KB
Newer Older
1 2 3 4
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
Don Gagne's avatar
Don Gagne committed
5
 (c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

/// @file
Don Gagne's avatar
Don Gagne committed
25
///     @brief Radio Config Qml Controller
26 27
///     @author Don Gagne <don@thegagnes.com

Don Gagne's avatar
Don Gagne committed
28
#include "RadioComponentController.h"
29
#include "AutoPilotPluginManager.h"
Don Gagne's avatar
Don Gagne committed
30
#include "QGCApplication.h"
31

Don Gagne's avatar
Don Gagne committed
32 33 34
#include <QSettings>

QGC_LOGGING_CATEGORY(RadioComponentControllerLog, "RadioComponentControllerLog")
35

36 37 38 39 40
#ifdef UNITTEST_BUILD
// Nasty hack to expose controller to unit test code
RadioComponentController* RadioComponentController::_unitTestController = NULL;
#endif

Don Gagne's avatar
Don Gagne committed
41 42
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
43
// FIXME: Double check these mins againt 150% throws
Don Gagne's avatar
Don Gagne committed
44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
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
73
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoPX4[RadioComponentController::rcCalFunctionMax] = {
Don Gagne's avatar
Don Gagne committed
74 75 76 77 78 79 80 81 82
    //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" },
83
    { "RC_MAP_ACRO_SW" },
Don Gagne's avatar
Don Gagne committed
84 85 86
    { "RC_MAP_FLAPS" },
    { "RC_MAP_AUX1" },
    { "RC_MAP_AUX2" },
87 88
};

Don Gagne's avatar
Don Gagne committed
89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoAPM[RadioComponentController::rcCalFunctionMax] = {
    //Parameter          required
    { "RCMAP_ROLL" },
    { "RCMAP_PITCH" },
    { "RCMAP_YAW" },
    { "RCMAP_THROTTLE" },
    { NULL },
    { NULL },
    { NULL },
    { NULL },
    { NULL },
    { NULL },
    { NULL },
    { NULL },
};

Don Gagne's avatar
Don Gagne committed
105
RadioComponentController::RadioComponentController(void) :
Don Gagne's avatar
Don Gagne committed
106
    _currentStep(-1),
107
    _transmitterMode(2),
108 109
    _chanCount(0),
    _rcCalState(rcCalStateChannelWait),
Don Gagne's avatar
Don Gagne committed
110 111 112 113 114
    _unitTestMode(false),
    _statusText(NULL),
    _cancelButton(NULL),
    _nextButton(NULL),
    _skipButton(NULL)
115
{
116 117 118 119 120
#ifdef UNITTEST_BUILD
    // Nasty hack to expose controller to unit test code
    _unitTestController = this;
#endif

Don Gagne's avatar
Don Gagne committed
121
    connect(_vehicle, &Vehicle::rcChannelsChanged, this, &RadioComponentController::_rcChannelsChanged);
122 123
    _loadSettings();
    
Don Gagne's avatar
Don Gagne committed
124 125 126 127 128
    _resetInternalCalibrationValues();
}

void RadioComponentController::start(void)
{
Don Gagne's avatar
Don Gagne committed
129
    _stopCalibration();
130
    _setInternalCalibrationValuesFromParameters();
Don Gagne's avatar
Don Gagne committed
131
}
132

Don Gagne's avatar
Don Gagne committed
133
RadioComponentController::~RadioComponentController()
Don Gagne's avatar
Don Gagne committed
134
{
135
    _storeSettings();
Don Gagne's avatar
Don Gagne committed
136
}
137

Don Gagne's avatar
Don Gagne committed
138
/// @brief Returns the state machine entry for the specified state.
Don Gagne's avatar
Don Gagne committed
139
const RadioComponentController::stateMachineEntry* RadioComponentController::_getStateMachineEntry(int step) const
Don Gagne's avatar
Don Gagne committed
140
{
Don Gagne's avatar
Don Gagne committed
141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
    static const char* msgBeginPX4 =        "Lower the Throttle stick all the way down as shown in diagram.\nReset all transmitter trims to center.\n\n"
                                            "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* msgAux1Switch =      "Move the switch or dial you want to use for Aux1.\n\n"
Don Gagne's avatar
Don Gagne committed
157
                                            "You can click Skip if you don't want to assign.";
Don Gagne's avatar
Don Gagne committed
158
    static const char* msgAux2Switch =      "Move the switch or dial you want to use for Aux2.\n\n"
Don Gagne's avatar
Don Gagne committed
159
                                            "You can click Skip if you don't want to assign.";
Don Gagne's avatar
Don Gagne committed
160 161
    static const char* msgSwitchMinMax =    "Move all the transmitter switches and/or dials back and forth to their extreme positions.";
    static const char* msgFlapsDetect =     "Move the switch or dial you want to use for Flaps back and forth a few times. "
Don Gagne's avatar
Don Gagne committed
162 163 164
                                            "Then leave the switch/dial at the position you want to use for Flaps fully extended.\n\n"
                                            "Click Next to continue.\n"
                                            "If you won't be using Flaps, click Skip.";
Don Gagne's avatar
Don Gagne committed
165 166
    static const char* msgFlapsUp =         "Move the switch or dial you want to use for Flaps to the position you want to use for Flaps fully retracted.";
    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
167
    
Don Gagne's avatar
Don Gagne committed
168
    static const stateMachineEntry rgStateMachinePX4[] = {
Don Gagne's avatar
Don Gagne committed
169
        //Function
Don Gagne's avatar
Don Gagne committed
170
        { rcCalFunctionMax,                 msgBeginPX4,        _imageHome,         &RadioComponentController::_inputCenterWaitBegin,   &RadioComponentController::_saveAllTrims,       NULL },
Don Gagne's avatar
Don Gagne committed
171 172 173 174 175 176 177 178 179 180 181 182 183 184 185
        { 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 },
        { rcCalFunctionFlaps,               msgFlapsDetect,     _imageThrottleDown, &RadioComponentController::_inputFlapsDetect,       &RadioComponentController::_saveFlapsDown,      &RadioComponentController::_skipFlaps },
        { rcCalFunctionFlaps,               msgFlapsUp,         _imageThrottleDown, &RadioComponentController::_inputFlapsUp,           NULL,                                           NULL },
        { rcCalFunctionAux1,                msgAux1Switch,      _imageThrottleDown, &RadioComponentController::_inputSwitchDetect,      NULL,                                           &RadioComponentController::_advanceState },
        { rcCalFunctionAux2,                msgAux2Switch,      _imageThrottleDown, &RadioComponentController::_inputSwitchDetect,      NULL,                                           &RadioComponentController::_advanceState },
        { rcCalFunctionMax,                 msgComplete,        _imageThrottleDown, NULL,                                               &RadioComponentController::_writeCalibration,   NULL },
Don Gagne's avatar
Don Gagne committed
186 187
    };
    
Don Gagne's avatar
Don Gagne committed
188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222
    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
223
    
Don Gagne's avatar
Don Gagne committed
224
    return &stateMachine[step];
Don Gagne's avatar
Don Gagne committed
225
}
226

Don Gagne's avatar
Don Gagne committed
227
void RadioComponentController::_advanceState(void)
Don Gagne's avatar
Don Gagne committed
228 229 230 231 232 233 234
{
    _currentStep++;
    _setupCurrentState();
}


/// @brief Sets up the state machine according to the current step from _currentStep.
Don Gagne's avatar
Don Gagne committed
235
void RadioComponentController::_setupCurrentState(void)
Don Gagne's avatar
Don Gagne committed
236 237 238
{
   const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
    
Don Gagne's avatar
Don Gagne committed
239
    _statusText->setProperty("text", state->instructions);
240 241
    
    _setHelpImage(state->image);
Don Gagne's avatar
Don Gagne committed
242
    
Don Gagne's avatar
Don Gagne committed
243
    _stickDetectChannel = _chanMax();
Don Gagne's avatar
Don Gagne committed
244 245 246 247
    _stickDetectSettleStarted = false;
    
    _rcCalSaveCurrentValues();
    
Don Gagne's avatar
Don Gagne committed
248 249
    _nextButton->setEnabled(state->nextFn != NULL);
    _skipButton->setEnabled(state->skipFn != NULL);
Don Gagne's avatar
Don Gagne committed
250 251
}

Don Gagne's avatar
Don Gagne committed
252 253
/// Connected to Vehicle::rcChannelsChanged signal
void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
254
{
Don Gagne's avatar
Don Gagne committed
255 256 257 258 259 260 261 262 263 264 265 266 267 268
    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
269
                case rcCalFunctionRoll:
Don Gagne's avatar
Don Gagne committed
270
                    emit rollChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
271 272
                    break;
                case rcCalFunctionPitch:
Don Gagne's avatar
Don Gagne committed
273
                    emit pitchChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
274 275
                    break;
                case rcCalFunctionYaw:
Don Gagne's avatar
Don Gagne committed
276
                    emit yawChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
277 278
                    break;
                case rcCalFunctionThrottle:
Don Gagne's avatar
Don Gagne committed
279
                    emit throttleChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
280 281 282 283
                    break;
                default:
                    break;

Don Gagne's avatar
Don Gagne committed
284
                }
Don Gagne's avatar
Don Gagne committed
285
            }
Don Gagne's avatar
Don Gagne committed
286 287 288 289 290 291 292 293 294 295 296 297

            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
298
            }
Don Gagne's avatar
Don Gagne committed
299 300 301 302
        }
    }
}

Don Gagne's avatar
Don Gagne committed
303
void RadioComponentController::nextButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
304 305 306 307 308 309 310
{
    if (_currentStep == -1) {
        // Need to have enough channels
        if (_chanCount < _chanMinimum) {
            if (_unitTestMode) {
                emit nextButtonMessageBoxDisplayed();
            } else {
311
                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
312 313 314 315 316 317 318 319 320 321 322 323
            }
            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
324
void RadioComponentController::skipButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
325 326 327 328 329 330 331 332 333
{
    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
334
void RadioComponentController::cancelButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
335
{
Don Gagne's avatar
Don Gagne committed
336
    _stopCalibration();
Don Gagne's avatar
Don Gagne committed
337 338
}

Don Gagne's avatar
Don Gagne committed
339
void RadioComponentController::_saveAllTrims(void)
Don Gagne's avatar
Don Gagne committed
340 341 342 343 344 345 346
{
    // 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
347
        qCDebug(RadioComponentControllerLog) << "_saveAllTrims trim" << _rcRawValue[i];
Don Gagne's avatar
Don Gagne committed
348 349
        _rgChannelInfo[i].rcTrim = _rcRawValue[i];
    }
Don Gagne's avatar
Don Gagne committed
350
    _advanceState();
Don Gagne's avatar
Don Gagne committed
351 352 353
}

/// @brief Waits for the sticks to be centered, enabling Next when done.
Don Gagne's avatar
Don Gagne committed
354
void RadioComponentController::_inputCenterWaitBegin(enum rcCalFunctions function, int chan, int value)
Don Gagne's avatar
Don Gagne committed
355 356 357 358 359 360 361
{
    Q_UNUSED(function);
    Q_UNUSED(chan);
    Q_UNUSED(value);
    
    // FIXME: Doesn't wait for center
    
Don Gagne's avatar
Don Gagne committed
362
    _nextButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
363 364
}

Don Gagne's avatar
Don Gagne committed
365
bool RadioComponentController::_stickSettleComplete(int value)
Don Gagne's avatar
Don Gagne committed
366 367 368 369 370 371
{
    // 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
372
        qCDebug(RadioComponentControllerLog) << "_stickSettleComplete still moving, _stickDetectValue:value" << _stickDetectValue << value;
Don Gagne's avatar
Don Gagne committed
373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388

        _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
389
            qCDebug(RadioComponentControllerLog) << "_stickSettleComplete starting settle timer, _stickDetectValue:value" << _stickDetectValue << value;
Don Gagne's avatar
Don Gagne committed
390 391 392 393 394 395 396 397 398
            
            _stickDetectSettleStarted = true;
            _stickDetectSettleElapsed.start();
        }
    }
    
    return false;
}

Don Gagne's avatar
Don Gagne committed
399
void RadioComponentController::_inputStickDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
400
{
Don Gagne's avatar
Don Gagne committed
401
    qCDebug(RadioComponentControllerLog) << "_inputStickDetect function:channel:value" << _functionInfo()[function].parameterName << channel << value;
Don Gagne's avatar
Don Gagne committed
402 403 404 405 406
    
    // If this channel is already used in a mapping we can't use it again
    if (_rgChannelInfo[channel].function != rcCalFunctionMax) {
        return;
    }
407
    
Don Gagne's avatar
Don Gagne committed
408
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
409 410 411 412 413
        // 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
414
            qCDebug(RadioComponentControllerLog) << "_inputStickDetect starting settle wait, function:channel:value" << function << channel << value;
Don Gagne's avatar
Don Gagne committed
415 416 417 418 419 420 421 422 423 424
            
            // 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
425
            qCDebug(RadioComponentControllerLog) << "_inputStickDetect settle complete, function:channel:value" << function << channel << value;
Don Gagne's avatar
Don Gagne committed
426 427 428 429 430 431 432 433 434 435 436 437 438 439 440
            
            // 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
441 442 443
            _signalAllAttiudeValueChanges();
            
            _advanceState();
Don Gagne's avatar
Don Gagne committed
444 445 446 447
        }
    }
}

Don Gagne's avatar
Don Gagne committed
448
void RadioComponentController::_inputStickMin(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
449 450 451 452 453 454
{
    // 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
455
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481
        // 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;
            }
482 483 484 485 486 487 488

            // 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
489
            
Don Gagne's avatar
Don Gagne committed
490
            _advanceState();
Don Gagne's avatar
Don Gagne committed
491 492 493 494
        }
    }
}

Don Gagne's avatar
Don Gagne committed
495
void RadioComponentController::_inputCenterWait(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
496 497 498 499 500 501
{
    // 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
502
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
503 504 505 506 507 508 509 510 511 512
        // 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
513
            _advanceState();
Don Gagne's avatar
Don Gagne committed
514 515 516 517 518
        }
    }
}

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

Don Gagne's avatar
Don Gagne committed
546
void RadioComponentController::_skipFlaps(void)
Don Gagne's avatar
Don Gagne committed
547 548 549
{
    // Flaps channel may have been identified. Clear it out.
    for (int i=0; i<_chanCount; i++) {
Don Gagne's avatar
Don Gagne committed
550
        if (_rgChannelInfo[i].function == RadioComponentController::rcCalFunctionFlaps) {
Don Gagne's avatar
Don Gagne committed
551 552 553
            _rgChannelInfo[i].function = rcCalFunctionMax;
        }
    }
Don Gagne's avatar
Don Gagne committed
554
    _rgFunctionChannelMapping[RadioComponentController::rcCalFunctionFlaps] = _chanMax();
Don Gagne's avatar
Don Gagne committed
555 556 557 558 559 560

    // Skip over flap steps
    _currentStep += 2;
    _setupCurrentState();
}

Don Gagne's avatar
Don Gagne committed
561
void RadioComponentController::_saveFlapsDown(void)
Don Gagne's avatar
Don Gagne committed
562 563 564
{
    int channel = _rgFunctionChannelMapping[rcCalFunctionFlaps];
    
Don Gagne's avatar
Don Gagne committed
565
    if (channel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
566 567 568 569
        // Channel not yet mapped, still waiting for switch to move
        if (_unitTestMode) {
            emit nextButtonMessageBoxDisplayed();
        } else {
570
            qgcApp()->showMessage("Flaps switch has not yet been detected.");
Don Gagne's avatar
Don Gagne committed
571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590
        }
        return;
    }
    
    Q_ASSERT(channel != -1);
    ChannelInfo* info = &_rgChannelInfo[channel];
    
    int rcValue = _rcRawValue[channel];
    
    // Switch detection is complete. Switch should be at flaps fully extended position.
    
    // Channel should be at max value, if it is below initial set point the channel is reversed.
    info->reversed = rcValue < _rcValueSave[channel];
    
    if (info->reversed) {
        _rgChannelInfo[channel].rcMin = rcValue;
    } else {
        _rgChannelInfo[channel].rcMax = rcValue;
    }
    
Don Gagne's avatar
Don Gagne committed
591
    _advanceState();
Don Gagne's avatar
Don Gagne committed
592 593
}

Don Gagne's avatar
Don Gagne committed
594
void RadioComponentController::_inputFlapsUp(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
595
{
596 597
    Q_UNUSED(function);
    
Don Gagne's avatar
Don Gagne committed
598 599 600 601 602 603 604 605 606
    // FIXME: Duplication
    
    Q_ASSERT(function == rcCalFunctionFlaps);
    
    // We only care about the channel mapped to flaps
    if (_rgFunctionChannelMapping[rcCalFunctionFlaps] != channel) {
        return;
    }
    
Don Gagne's avatar
Don Gagne committed
607
    if (_stickDetectChannel == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634
        // 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;
            }
            
Don Gagne's avatar
Don Gagne committed
635
            _advanceState();
Don Gagne's avatar
Don Gagne committed
636 637 638 639
        }
    }
}

Don Gagne's avatar
Don Gagne committed
640
void RadioComponentController::_switchDetect(enum rcCalFunctions function, int channel, int value, bool moveToNextStep)
Don Gagne's avatar
Don Gagne committed
641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656
{
    // 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
657
        qCDebug(RadioComponentControllerLog) << "Function:" << function << "mapped to:" << channel;
Don Gagne's avatar
Don Gagne committed
658 659
        
        if (moveToNextStep) {
Don Gagne's avatar
Don Gagne committed
660
            _advanceState();
Don Gagne's avatar
Don Gagne committed
661 662 663 664
        }
    }
}

Don Gagne's avatar
Don Gagne committed
665
void RadioComponentController::_inputSwitchDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
666 667 668 669
{
    _switchDetect(function, channel, value, true /* move to next step after detection */);
}

Don Gagne's avatar
Don Gagne committed
670
void RadioComponentController::_inputFlapsDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
671 672
{
    _switchDetect(function, channel, value, false /* do not move to next step after detection */);
673 674 675
}

/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
Don Gagne's avatar
Don Gagne committed
676
void RadioComponentController::_resetInternalCalibrationValues(void)
677 678
{
    // Set all raw channels to not reversed and center point values
Don Gagne's avatar
Don Gagne committed
679
    for (int i=0; i<_chanMax(); i++) {
680 681 682
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
        info->reversed = false;
Don Gagne's avatar
Don Gagne committed
683 684 685
        info->rcMin = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcMax = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcTrim = RadioComponentController::_rcCalPWMCenterPoint;
686 687
    }
    
688
    // Initialize attitude function mapping to function channel not set
689
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
690
        _rgFunctionChannelMapping[i] = _chanMax();
691 692
    }
    
Don Gagne's avatar
Don Gagne committed
693 694
    if (_px4Vehicle()) {
        // Reserve the existing Flight Mode switch settings channels so we don't re-use them
695

Don Gagne's avatar
Don Gagne committed
696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718
        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];

            bool ok;
            int switchChannel = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[curFunction].parameterName)->rawValue().toInt(&ok);
            Q_ASSERT(ok);

            // Parameter: 1-based channel, 0=not mapped
            // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped

            if (switchChannel != 0) {
                qCDebug(RadioComponentControllerLog) << "Reserving 0-based switch channel" << switchChannel - 1;
                _rgFunctionChannelMapping[curFunction] = switchChannel - 1;
                _rgChannelInfo[switchChannel - 1].function = curFunction;
            }
719 720
        }
    }
Don Gagne's avatar
Don Gagne committed
721 722
    
    _signalAllAttiudeValueChanges();
Don Gagne's avatar
Don Gagne committed
723 724
}

Don Gagne's avatar
Don Gagne committed
725
/// @brief Sets internal calibration values from the stored parameters
Don Gagne's avatar
Don Gagne committed
726
void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
Don Gagne's avatar
Don Gagne committed
727
{
728 729
    // Initialize all function mappings to not set
    
Don Gagne's avatar
Don Gagne committed
730
    for (int i=0; i<_chanMax(); i++) {
731 732 733 734 735
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
    }
    
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
736
        _rgFunctionChannelMapping[i] = _chanMax();
737 738 739 740 741 742 743 744
    }
    
    // Pull parameters and update
    
    QString minTpl("RC%1_MIN");
    QString maxTpl("RC%1_MAX");
    QString trimTpl("RC%1_TRIM");
    QString revTpl("RC%1_REV");
745
    
746 747
    bool convertOk;
    
Don Gagne's avatar
Don Gagne committed
748
    for (int i = 0; i < _chanMax(); ++i) {
749
        struct ChannelInfo* info = &_rgChannelInfo[i];
Don Gagne's avatar
Don Gagne committed
750
        
Don Gagne's avatar
Don Gagne committed
751
        info->rcTrim = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1))->rawValue().toInt(&convertOk);
752
        Q_ASSERT(convertOk);
Don Gagne's avatar
Don Gagne committed
753
        
Don Gagne's avatar
Don Gagne committed
754
        info->rcMin = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(i+1))->rawValue().toInt(&convertOk);
755
        Q_ASSERT(convertOk);
Don Gagne's avatar
Don Gagne committed
756

Don Gagne's avatar
Don Gagne committed
757
        info->rcMax = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1))->rawValue().toInt(&convertOk);
758
        Q_ASSERT(convertOk);
Don Gagne's avatar
Don Gagne committed
759

Don Gagne's avatar
Don Gagne committed
760
        float floatReversed = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(i+1))->rawValue().toFloat(&convertOk);
761 762 763
        Q_ASSERT(convertOk);
        Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f);
        info->reversed = floatReversed == -1.0f;
764 765 766 767
    }
    
    for (int i=0; i<rcCalFunctionMax; i++) {
        int32_t paramChannel;
Don Gagne's avatar
Don Gagne committed
768
        
Don Gagne's avatar
Don Gagne committed
769 770 771 772 773 774 775 776 777
        const char* paramName = _functionInfo()[i].parameterName;
        if (paramName) {
            paramChannel = getParameterFact(FactSystem::defaultComponentId, paramName)->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
778 779
        }
    }
780
    
Don Gagne's avatar
Don Gagne committed
781
    _signalAllAttiudeValueChanges();
782 783
}

Don Gagne's avatar
Don Gagne committed
784
void RadioComponentController::spektrumBindMode(int mode)
Don Gagne's avatar
Don Gagne committed
785
{
Don Gagne's avatar
Don Gagne committed
786
    _uas->pairRX(0, mode);
787 788
}

Don Gagne's avatar
Don Gagne committed
789
/// @brief Validates the current settings against the calibration rules resetting values as necessary.
Don Gagne's avatar
Don Gagne committed
790
void RadioComponentController::_validateCalibration(void)
Don Gagne's avatar
Don Gagne committed
791
{
Don Gagne's avatar
Don Gagne committed
792
    for (int chan = 0; chan<_chanMax(); chan++) {
Don Gagne's avatar
Don Gagne committed
793 794 795 796 797 798
        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
799
                qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting channel" << chan;
Don Gagne's avatar
Don Gagne committed
800 801
                info->rcMin = _rcCalPWMDefaultMinValue;
                info->rcMax = _rcCalPWMDefaultMaxValue;
Don Gagne's avatar
Don Gagne committed
802 803 804 805 806 807 808
                info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
            } else {
                switch (_rgChannelInfo[chan].function) {
                    case rcCalFunctionThrottle:
                    case rcCalFunctionYaw:
                    case rcCalFunctionRoll:
                    case rcCalFunctionPitch:
809 810 811 812 813 814
                        // 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
815 816 817 818 819 820 821
                        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
822 823 824
            }
        } else {
            // Unavailable channels are set to defaults
Don Gagne's avatar
Don Gagne committed
825
            qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting unavailable channel" << chan;
Don Gagne's avatar
Don Gagne committed
826 827
            info->rcMin = _rcCalPWMDefaultMinValue;
            info->rcMax = _rcCalPWMDefaultMaxValue;
Don Gagne's avatar
Don Gagne committed
828
            info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
Don Gagne's avatar
Don Gagne committed
829 830 831 832 833 834
            info->reversed = false;
        }
    }
}


835
/// @brief Saves the rc calibration values to the board parameters.
Don Gagne's avatar
Don Gagne committed
836
void RadioComponentController::_writeCalibration(void)
837
{
838
    if (!_uas) return;
839
    
840
    _uas->stopCalibration();
841
    
Don Gagne's avatar
Don Gagne committed
842 843
    _validateCalibration();
    
844 845 846 847 848
    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
849
    // 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
850
    for (int chan = 0; chan<_chanMax(); chan++) {
Don Gagne's avatar
Don Gagne committed
851
        struct ChannelInfo* info = &_rgChannelInfo[chan];
852 853
        int                 oneBasedChannel = chan + 1;
        
Don Gagne's avatar
Don Gagne committed
854 855 856
        getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(oneBasedChannel))->setRawValue((float)info->rcTrim);
        getParameterFact(FactSystem::defaultComponentId, minTpl.arg(oneBasedChannel))->setRawValue((float)info->rcMin);
        getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(oneBasedChannel))->setRawValue((float)info->rcMax);
857 858 859 860 861 862 863 864 865

        // APM 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;
        }
        getParameterFact(FactSystem::defaultComponentId, revTpl.arg(oneBasedChannel))->setRawValue(reversedParamValue);
866 867
    }
    
Don Gagne's avatar
Don Gagne committed
868
    // Write function mapping parameters
Don Gagne's avatar
Don Gagne committed
869
    bool functionMappingChanged = false;
Don Gagne's avatar
Don Gagne committed
870 871
    for (size_t i=0; i<rcCalFunctionMax; i++) {
        int32_t paramChannel;
Don Gagne's avatar
Don Gagne committed
872
        if (_rgFunctionChannelMapping[i] == _chanMax()) {
Don Gagne's avatar
Don Gagne committed
873 874 875 876 877
            // 0 signals no mapping
            paramChannel = 0;
        } else {
            // Note that the channel value is 1-based
            paramChannel = _rgFunctionChannelMapping[i] + 1;
878
        }
Don Gagne's avatar
Don Gagne committed
879 880 881 882 883 884 885 886 887
        const char* paramName = _functionInfo()[i].parameterName;
        if (paramName) {
            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName);

            if (paramFact->rawValue().toInt() != paramChannel) {
                functionMappingChanged = true;
                getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName)->setRawValue(paramChannel);
            }
        }
888 889
    }
    
Don Gagne's avatar
Don Gagne committed
890 891 892 893 894
    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);
        }
895
    }
896
    
Don Gagne's avatar
Don Gagne committed
897
    _stopCalibration();
898
    _setInternalCalibrationValuesFromParameters();
Don Gagne's avatar
Don Gagne committed
899

Don Gagne's avatar
Don Gagne committed
900 901
    if (_vehicle->apmFirmware() && functionMappingChanged && !_unitTestMode) {
        // We can't emit this in unit test mode since it confused to Qml which is running in an invisible widget
Don Gagne's avatar
Don Gagne committed
902 903
        emit functionMappingChangedAPMReboot();
    }
904 905
}

Don Gagne's avatar
Don Gagne committed
906
/// @brief Starts the calibration process
Don Gagne's avatar
Don Gagne committed
907
void RadioComponentController::_startCalibration(void)
908 909 910
{
    Q_ASSERT(_chanCount >= _chanMinimum);
    
Don Gagne's avatar
Don Gagne committed
911 912
    _resetInternalCalibrationValues();
    
913
    // Let the mav known we are starting calibration. This should turn off motors and so forth.
914
    _uas->startCalibration(UASInterface::StartCalibrationRadio);
915
    
Don Gagne's avatar
Don Gagne committed
916 917
    _nextButton->setProperty("text", "Next");
    _cancelButton->setEnabled(true);
918
    
Don Gagne's avatar
Don Gagne committed
919 920
    _currentStep = 0;
    _setupCurrentState();
921 922
}

Don Gagne's avatar
Don Gagne committed
923
/// @brief Cancels the calibration process, setting things back to initial state.
Don Gagne's avatar
Don Gagne committed
924
void RadioComponentController::_stopCalibration(void)
925
{
Don Gagne's avatar
Don Gagne committed
926
    _currentStep = -1;
927
    
928 929
    if (_uas) {
        _uas->stopCalibration();
Don Gagne's avatar
Don Gagne committed
930 931 932
        _setInternalCalibrationValuesFromParameters();
    } else {
        _resetInternalCalibrationValues();
933 934
    }
    
Don Gagne's avatar
Don Gagne committed
935
    _statusText->setProperty("text", "");
936

Don Gagne's avatar
Don Gagne committed
937 938 939 940
    _nextButton->setProperty("text", "Calibrate");
    _nextButton->setEnabled(true);
    _cancelButton->setEnabled(false);
    _skipButton->setEnabled(false);
941 942
    
    _setHelpImage(_imageCenter);
943 944
}

Don Gagne's avatar
Don Gagne committed
945
/// @brief Saves the current channel values, so that we can detect when the use moves an input.
Don Gagne's avatar
Don Gagne committed
946
void RadioComponentController::_rcCalSaveCurrentValues(void)
947
{
Don Gagne's avatar
Don Gagne committed
948
	qCDebug(RadioComponentControllerLog) << "_rcCalSaveCurrentValues";
Don Gagne's avatar
Don Gagne committed
949
    for (int i = 0; i < _chanMax(); i++) {
Don Gagne's avatar
Don Gagne committed
950 951
        _rcValueSave[i] = _rcRawValue[i];
    }
952 953 954
}

/// @brief Set up the Save state of calibration.
Don Gagne's avatar
Don Gagne committed
955
void RadioComponentController::_rcCalSave(void)
956 957 958
{
    _rcCalState = rcCalStateSave;
    
Don Gagne's avatar
Don Gagne committed
959 960 961
    _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.");
962

Don Gagne's avatar
Don Gagne committed
963 964 965
    _nextButton->setEnabled(true);
    _skipButton->setEnabled(false);
    _cancelButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
966 967 968 969
    
    // 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
970 971
}

Don Gagne's avatar
Don Gagne committed
972
void RadioComponentController::_loadSettings(void)
973 974 975 976 977 978 979 980 981 982 983 984
{
    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
985
void RadioComponentController::_storeSettings(void)
986 987 988 989 990 991 992 993
{
    QSettings settings;
    
    settings.beginGroup(_settingsGroup);
    settings.setValue(_settingsKeyTransmitterMode, _transmitterMode);
    settings.endGroup();
}

Don Gagne's avatar
Don Gagne committed
994
void RadioComponentController::_setHelpImage(const char* imageFile)
995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006
{
    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
1007
    qCDebug(RadioComponentControllerLog) << "_setHelpImage" << file;
1008
    
Don Gagne's avatar
Don Gagne committed
1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019
    _imageHelp = file;
    emit imageHelpChanged(file);
}

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

int RadioComponentController::rollChannelRCValue(void)
{    
Don Gagne's avatar
Don Gagne committed
1020
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1021 1022 1023 1024 1025 1026 1027 1028
        return _rcRawValue[rcCalFunctionRoll];
    } else {
        return 1500;
    }
}

int RadioComponentController::pitchChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
1029
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1030 1031 1032 1033 1034 1035 1036 1037
        return _rcRawValue[rcCalFunctionPitch];
    } else {
        return 1500;
    }
}

int RadioComponentController::yawChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
1038
    if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1039 1040 1041 1042 1043 1044 1045 1046
        return _rcRawValue[rcCalFunctionYaw];
    } else {
        return 1500;
    }
}

int RadioComponentController::throttleChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
1047
    if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1048 1049 1050 1051 1052 1053 1054 1055
        return _rcRawValue[rcCalFunctionThrottle];
    } else {
        return 1500;
    }
}

bool RadioComponentController::rollChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
1056
    return _rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax();
Don Gagne's avatar
Don Gagne committed
1057 1058 1059 1060
}

bool RadioComponentController::pitchChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
1061
    return _rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax();
Don Gagne's avatar
Don Gagne committed
1062 1063 1064 1065
}

bool RadioComponentController::yawChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
1066
    return _rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax();
Don Gagne's avatar
Don Gagne committed
1067 1068 1069 1070
}

bool RadioComponentController::throttleChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
1071
    return _rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax();
Don Gagne's avatar
Don Gagne committed
1072 1073 1074 1075
}

bool RadioComponentController::rollChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1076
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1077 1078 1079 1080 1081 1082 1083 1084
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionRoll]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::pitchChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1085
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1086 1087 1088 1089 1090 1091 1092 1093
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionPitch]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::yawChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1094
    if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1095 1096 1097 1098 1099 1100 1101 1102
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionYaw]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::throttleChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1103
    if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131
        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());
1132
}
Don Gagne's avatar
Don Gagne committed
1133 1134 1135

void RadioComponentController::copyTrims(void)
{
1136
    _uas->startCalibration(UASInterface::StartCalibrationCopyTrims);
Don Gagne's avatar
Don Gagne committed
1137
}
Don Gagne's avatar
Don Gagne committed
1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152

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;
}