RadioComponentController.cc 43.6 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" },
84 85
};

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

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

Don Gagne's avatar
Don Gagne committed
115
    connect(_vehicle, &Vehicle::rcChannelsChanged, this, &RadioComponentController::_rcChannelsChanged);
116
    _loadSettings();
117 118 119 120

    // 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;
121
    
Don Gagne's avatar
Don Gagne committed
122 123 124 125 126
    _resetInternalCalibrationValues();
}

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

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

Don Gagne's avatar
Don Gagne committed
136
/// @brief Returns the state machine entry for the specified state.
Don Gagne's avatar
Don Gagne committed
137
const RadioComponentController::stateMachineEntry* RadioComponentController::_getStateMachineEntry(int step) const
Don Gagne's avatar
Don Gagne committed
138
{
Don Gagne's avatar
Don Gagne committed
139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155
    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* msgSwitchMinMax =    "Move all the transmitter switches and/or dials back and forth to their extreme positions.";
    static const char* msgComplete =        "All settings have been captured. Click Next to write the new parameters to your board.";
Don Gagne's avatar
Don Gagne committed
156
    
Don Gagne's avatar
Don Gagne committed
157
    static const stateMachineEntry rgStateMachinePX4[] = {
Don Gagne's avatar
Don Gagne committed
158
        //Function
Don Gagne's avatar
Don Gagne committed
159
        { rcCalFunctionMax,                 msgBeginPX4,        _imageHome,         &RadioComponentController::_inputCenterWaitBegin,   &RadioComponentController::_saveAllTrims,       NULL },
Don Gagne's avatar
Don Gagne committed
160 161 162 163 164 165 166 167 168 169 170
        { 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
171 172
    };
    
Don Gagne's avatar
Don Gagne committed
173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
    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
208
    
Don Gagne's avatar
Don Gagne committed
209
    return &stateMachine[step];
Don Gagne's avatar
Don Gagne committed
210
}
211

Don Gagne's avatar
Don Gagne committed
212
void RadioComponentController::_advanceState(void)
Don Gagne's avatar
Don Gagne committed
213 214 215 216 217 218 219
{
    _currentStep++;
    _setupCurrentState();
}


/// @brief Sets up the state machine according to the current step from _currentStep.
Don Gagne's avatar
Don Gagne committed
220
void RadioComponentController::_setupCurrentState(void)
Don Gagne's avatar
Don Gagne committed
221 222 223
{
   const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
    
Don Gagne's avatar
Don Gagne committed
224
    _statusText->setProperty("text", state->instructions);
225 226
    
    _setHelpImage(state->image);
Don Gagne's avatar
Don Gagne committed
227
    
Don Gagne's avatar
Don Gagne committed
228
    _stickDetectChannel = _chanMax();
Don Gagne's avatar
Don Gagne committed
229 230 231 232
    _stickDetectSettleStarted = false;
    
    _rcCalSaveCurrentValues();
    
Don Gagne's avatar
Don Gagne committed
233 234
    _nextButton->setEnabled(state->nextFn != NULL);
    _skipButton->setEnabled(state->skipFn != NULL);
Don Gagne's avatar
Don Gagne committed
235 236
}

Don Gagne's avatar
Don Gagne committed
237 238
/// Connected to Vehicle::rcChannelsChanged signal
void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
239
{
Don Gagne's avatar
Don Gagne committed
240 241 242 243 244 245 246 247 248 249 250 251 252 253
    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
254
                case rcCalFunctionRoll:
Don Gagne's avatar
Don Gagne committed
255
                    emit rollChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
256 257
                    break;
                case rcCalFunctionPitch:
Don Gagne's avatar
Don Gagne committed
258
                    emit pitchChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
259 260
                    break;
                case rcCalFunctionYaw:
Don Gagne's avatar
Don Gagne committed
261
                    emit yawChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
262 263
                    break;
                case rcCalFunctionThrottle:
Don Gagne's avatar
Don Gagne committed
264
                    emit throttleChannelRCValueChanged(channelValue);
Don Gagne's avatar
Don Gagne committed
265 266 267 268
                    break;
                default:
                    break;

Don Gagne's avatar
Don Gagne committed
269
                }
Don Gagne's avatar
Don Gagne committed
270
            }
Don Gagne's avatar
Don Gagne committed
271 272 273 274 275 276 277 278 279 280 281 282

            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
283
            }
Don Gagne's avatar
Don Gagne committed
284 285 286 287
        }
    }
}

Don Gagne's avatar
Don Gagne committed
288
void RadioComponentController::nextButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
289 290 291 292 293 294 295
{
    if (_currentStep == -1) {
        // Need to have enough channels
        if (_chanCount < _chanMinimum) {
            if (_unitTestMode) {
                emit nextButtonMessageBoxDisplayed();
            } else {
296
                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
297 298 299 300 301 302 303 304 305 306 307 308
            }
            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
309
void RadioComponentController::skipButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
310 311 312 313 314 315 316 317 318
{
    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
319
void RadioComponentController::cancelButtonClicked(void)
Don Gagne's avatar
Don Gagne committed
320
{
Don Gagne's avatar
Don Gagne committed
321
    _stopCalibration();
Don Gagne's avatar
Don Gagne committed
322 323
}

Don Gagne's avatar
Don Gagne committed
324
void RadioComponentController::_saveAllTrims(void)
Don Gagne's avatar
Don Gagne committed
325 326 327 328 329 330 331
{
    // 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
332
        qCDebug(RadioComponentControllerLog) << "_saveAllTrims trim" << _rcRawValue[i];
Don Gagne's avatar
Don Gagne committed
333 334
        _rgChannelInfo[i].rcTrim = _rcRawValue[i];
    }
Don Gagne's avatar
Don Gagne committed
335
    _advanceState();
Don Gagne's avatar
Don Gagne committed
336 337 338
}

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

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

        _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
374
            qCDebug(RadioComponentControllerLog) << "_stickSettleComplete starting settle timer, _stickDetectValue:value" << _stickDetectValue << value;
Don Gagne's avatar
Don Gagne committed
375 376 377 378 379 380 381 382 383
            
            _stickDetectSettleStarted = true;
            _stickDetectSettleElapsed.start();
        }
    }
    
    return false;
}

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

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

            // 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
474
            
Don Gagne's avatar
Don Gagne committed
475
            _advanceState();
Don Gagne's avatar
Don Gagne committed
476 477 478 479
        }
    }
}

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

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

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

Don Gagne's avatar
Don Gagne committed
556
void RadioComponentController::_inputSwitchDetect(enum rcCalFunctions function, int channel, int value)
Don Gagne's avatar
Don Gagne committed
557 558 559 560
{
    _switchDetect(function, channel, value, true /* move to next step after detection */);
}

561
/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
Don Gagne's avatar
Don Gagne committed
562
void RadioComponentController::_resetInternalCalibrationValues(void)
563 564
{
    // Set all raw channels to not reversed and center point values
Don Gagne's avatar
Don Gagne committed
565
    for (int i=0; i<_chanMax(); i++) {
566 567 568
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
        info->reversed = false;
Don Gagne's avatar
Don Gagne committed
569 570 571
        info->rcMin = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcMax = RadioComponentController::_rcCalPWMCenterPoint;
        info->rcTrim = RadioComponentController::_rcCalPWMCenterPoint;
572 573
    }
    
574
    // Initialize attitude function mapping to function channel not set
575
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
576
        _rgFunctionChannelMapping[i] = _chanMax();
577 578
    }
    
Don Gagne's avatar
Don Gagne committed
579 580
    if (_px4Vehicle()) {
        // Reserve the existing Flight Mode switch settings channels so we don't re-use them
581

Don Gagne's avatar
Don Gagne committed
582 583 584 585 586 587 588 589 590 591 592
        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];

593 594 595 596 597
            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[curFunction].parameterName);
            if (paramFact) {
                bool ok;
                int switchChannel = paramFact->rawValue().toInt(&ok);
                Q_ASSERT(ok);
Don Gagne's avatar
Don Gagne committed
598

599 600
                // Parameter: 1-based channel, 0=not mapped
                // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped
Don Gagne's avatar
Don Gagne committed
601

602 603 604 605 606
                if (switchChannel != 0) {
                    qCDebug(RadioComponentControllerLog) << "Reserving 0-based switch channel" << switchChannel - 1;
                    _rgFunctionChannelMapping[curFunction] = switchChannel - 1;
                    _rgChannelInfo[switchChannel - 1].function = curFunction;
                }
Don Gagne's avatar
Don Gagne committed
607
            }
608 609
        }
    }
Don Gagne's avatar
Don Gagne committed
610 611
    
    _signalAllAttiudeValueChanges();
Don Gagne's avatar
Don Gagne committed
612 613
}

Don Gagne's avatar
Don Gagne committed
614
/// @brief Sets internal calibration values from the stored parameters
Don Gagne's avatar
Don Gagne committed
615
void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
Don Gagne's avatar
Don Gagne committed
616
{
617 618
    // Initialize all function mappings to not set
    
Don Gagne's avatar
Don Gagne committed
619
    for (int i=0; i<_chanMax(); i++) {
620 621 622 623 624
        struct ChannelInfo* info = &_rgChannelInfo[i];
        info->function = rcCalFunctionMax;
    }
    
    for (size_t i=0; i<rcCalFunctionMax; i++) {
Don Gagne's avatar
Don Gagne committed
625
        _rgFunctionChannelMapping[i] = _chanMax();
626 627 628 629 630 631 632 633
    }
    
    // Pull parameters and update
    
    QString minTpl("RC%1_MIN");
    QString maxTpl("RC%1_MAX");
    QString trimTpl("RC%1_TRIM");
    QString revTpl("RC%1_REV");
634
    
635 636
    bool convertOk;
    
Don Gagne's avatar
Don Gagne committed
637
    for (int i = 0; i < _chanMax(); ++i) {
638
        struct ChannelInfo* info = &_rgChannelInfo[i];
639 640 641 642 643 644 645 646 647 648 649

        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
650
        
651 652 653 654 655
        Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1));
        if (paramFact) {
            info->rcTrim = paramFact->rawValue().toInt(&convertOk);
            Q_ASSERT(convertOk);
        }
Don Gagne's avatar
Don Gagne committed
656
        
657 658 659 660 661
        paramFact = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(i+1));
        if (paramFact) {
            info->rcMin = paramFact->rawValue().toInt(&convertOk);
            Q_ASSERT(convertOk);
        }
Don Gagne's avatar
Don Gagne committed
662

663 664 665 666 667
        paramFact = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1));
        if (paramFact) {
            info->rcMax = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1))->rawValue().toInt(&convertOk);
            Q_ASSERT(convertOk);
        }
Don Gagne's avatar
Don Gagne committed
668

669 670 671 672 673 674 675
        paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(i+1));
        if (paramFact) {
            float floatReversed = paramFact->rawValue().toFloat(&convertOk);
            Q_ASSERT(convertOk);
            Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f);
            info->reversed = floatReversed == -1.0f;
        }
676 677 678 679
    }
    
    for (int i=0; i<rcCalFunctionMax; i++) {
        int32_t paramChannel;
Don Gagne's avatar
Don Gagne committed
680
        
Don Gagne's avatar
Don Gagne committed
681 682
        const char* paramName = _functionInfo()[i].parameterName;
        if (paramName) {
683 684 685 686 687 688 689 690 691
            Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, paramName);
            if (paramFact) {
                paramChannel = paramFact->rawValue().toInt(&convertOk);
                Q_ASSERT(convertOk);

                if (paramChannel != 0) {
                    _rgFunctionChannelMapping[i] = paramChannel - 1;
                    _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i;
                }
Don Gagne's avatar
Don Gagne committed
692
            }
Don Gagne's avatar
Don Gagne committed
693 694
        }
    }
695
    
Don Gagne's avatar
Don Gagne committed
696
    _signalAllAttiudeValueChanges();
697 698
}

Don Gagne's avatar
Don Gagne committed
699
void RadioComponentController::spektrumBindMode(int mode)
Don Gagne's avatar
Don Gagne committed
700
{
Don Gagne's avatar
Don Gagne committed
701
    _uas->pairRX(0, mode);
702 703
}

Don Gagne's avatar
Don Gagne committed
704
/// @brief Validates the current settings against the calibration rules resetting values as necessary.
Don Gagne's avatar
Don Gagne committed
705
void RadioComponentController::_validateCalibration(void)
Don Gagne's avatar
Don Gagne committed
706
{
Don Gagne's avatar
Don Gagne committed
707
    for (int chan = 0; chan<_chanMax(); chan++) {
Don Gagne's avatar
Don Gagne committed
708 709 710 711 712 713
        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
714
                qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting channel" << chan;
Don Gagne's avatar
Don Gagne committed
715 716
                info->rcMin = _rcCalPWMDefaultMinValue;
                info->rcMax = _rcCalPWMDefaultMaxValue;
Don Gagne's avatar
Don Gagne committed
717 718 719 720 721 722 723
                info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
            } else {
                switch (_rgChannelInfo[chan].function) {
                    case rcCalFunctionThrottle:
                    case rcCalFunctionYaw:
                    case rcCalFunctionRoll:
                    case rcCalFunctionPitch:
724 725 726 727 728 729
                        // 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
730 731 732 733 734 735 736
                        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
737 738 739
            }
        } else {
            // Unavailable channels are set to defaults
Don Gagne's avatar
Don Gagne committed
740
            qCDebug(RadioComponentControllerLog) << "_validateCalibration resetting unavailable channel" << chan;
Don Gagne's avatar
Don Gagne committed
741 742
            info->rcMin = _rcCalPWMDefaultMinValue;
            info->rcMax = _rcCalPWMDefaultMaxValue;
Don Gagne's avatar
Don Gagne committed
743
            info->rcTrim = info->rcMin + ((info->rcMax - info->rcMin) / 2);
Don Gagne's avatar
Don Gagne committed
744 745 746 747 748 749
            info->reversed = false;
        }
    }
}


750
/// @brief Saves the rc calibration values to the board parameters.
Don Gagne's avatar
Don Gagne committed
751
void RadioComponentController::_writeCalibration(void)
752
{
753
    if (!_uas) return;
754
    
755 756 757
    if (_px4Vehicle()) {
        _uas->stopCalibration();
    }
758
    
Don Gagne's avatar
Don Gagne committed
759 760
    _validateCalibration();
    
761 762 763 764 765
    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
766
    // 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
767
    for (int chan = 0; chan<_chanMax(); chan++) {
Don Gagne's avatar
Don Gagne committed
768
        struct ChannelInfo* info = &_rgChannelInfo[chan];
769 770
        int                 oneBasedChannel = chan + 1;
        
771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787
        if (_px4Vehicle() && _apmPossibleMissingRCChannelParams.contains(chan+1) && !parameterExists(FactSystem::defaultComponentId, minTpl.arg(chan+1))) {
            // RC parameters for this channel are missing from this version of APM
            continue;
        }

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

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

820 821 822 823 824
            if (paramFact && paramFact->rawValue().toInt() != paramChannel) {
                paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName);
                if (paramFact) {
                    paramFact->setRawValue(paramChannel);
                }
Don Gagne's avatar
Don Gagne committed
825 826
            }
        }
827 828
    }
    
Don Gagne's avatar
Don Gagne committed
829 830 831 832 833
    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);
        }
834
    }
835
    
Don Gagne's avatar
Don Gagne committed
836
    _stopCalibration();
837
    _setInternalCalibrationValuesFromParameters();
838 839
}

Don Gagne's avatar
Don Gagne committed
840
/// @brief Starts the calibration process
Don Gagne's avatar
Don Gagne committed
841
void RadioComponentController::_startCalibration(void)
842 843 844
{
    Q_ASSERT(_chanCount >= _chanMinimum);
    
Don Gagne's avatar
Don Gagne committed
845 846
    _resetInternalCalibrationValues();
    
847
    // Let the mav known we are starting calibration. This should turn off motors and so forth.
848 849 850
    if (_px4Vehicle()) {
        _uas->startCalibration(UASInterface::StartCalibrationRadio);
    }
851
    
Don Gagne's avatar
Don Gagne committed
852 853
    _nextButton->setProperty("text", "Next");
    _cancelButton->setEnabled(true);
854
    
Don Gagne's avatar
Don Gagne committed
855 856
    _currentStep = 0;
    _setupCurrentState();
857 858
}

Don Gagne's avatar
Don Gagne committed
859
/// @brief Cancels the calibration process, setting things back to initial state.
Don Gagne's avatar
Don Gagne committed
860
void RadioComponentController::_stopCalibration(void)
861
{
Don Gagne's avatar
Don Gagne committed
862
    _currentStep = -1;
863
    
864
    if (_uas) {
865 866 867
        if (_px4Vehicle()) {
            _uas->stopCalibration();
        }
Don Gagne's avatar
Don Gagne committed
868 869 870
        _setInternalCalibrationValuesFromParameters();
    } else {
        _resetInternalCalibrationValues();
871 872
    }
    
Don Gagne's avatar
Don Gagne committed
873
    _statusText->setProperty("text", "");
874

Don Gagne's avatar
Don Gagne committed
875 876 877 878
    _nextButton->setProperty("text", "Calibrate");
    _nextButton->setEnabled(true);
    _cancelButton->setEnabled(false);
    _skipButton->setEnabled(false);
879 880
    
    _setHelpImage(_imageCenter);
881 882
}

Don Gagne's avatar
Don Gagne committed
883
/// @brief Saves the current channel values, so that we can detect when the use moves an input.
Don Gagne's avatar
Don Gagne committed
884
void RadioComponentController::_rcCalSaveCurrentValues(void)
885
{
Don Gagne's avatar
Don Gagne committed
886
	qCDebug(RadioComponentControllerLog) << "_rcCalSaveCurrentValues";
Don Gagne's avatar
Don Gagne committed
887
    for (int i = 0; i < _chanMax(); i++) {
Don Gagne's avatar
Don Gagne committed
888 889
        _rcValueSave[i] = _rcRawValue[i];
    }
890 891 892
}

/// @brief Set up the Save state of calibration.
Don Gagne's avatar
Don Gagne committed
893
void RadioComponentController::_rcCalSave(void)
894 895 896
{
    _rcCalState = rcCalStateSave;
    
Don Gagne's avatar
Don Gagne committed
897 898 899
    _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.");
900

Don Gagne's avatar
Don Gagne committed
901 902 903
    _nextButton->setEnabled(true);
    _skipButton->setEnabled(false);
    _cancelButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
904 905 906 907
    
    // 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
908 909
}

Don Gagne's avatar
Don Gagne committed
910
void RadioComponentController::_loadSettings(void)
911 912 913 914 915 916 917 918 919 920 921 922
{
    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
923
void RadioComponentController::_storeSettings(void)
924 925 926 927 928 929 930 931
{
    QSettings settings;
    
    settings.beginGroup(_settingsGroup);
    settings.setValue(_settingsKeyTransmitterMode, _transmitterMode);
    settings.endGroup();
}

Don Gagne's avatar
Don Gagne committed
932
void RadioComponentController::_setHelpImage(const char* imageFile)
933 934 935 936 937 938 939 940 941 942 943 944
{
    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
945
    qCDebug(RadioComponentControllerLog) << "_setHelpImage" << file;
946
    
Don Gagne's avatar
Don Gagne committed
947 948 949 950 951 952 953 954 955 956 957
    _imageHelp = file;
    emit imageHelpChanged(file);
}

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

int RadioComponentController::rollChannelRCValue(void)
{    
Don Gagne's avatar
Don Gagne committed
958
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
959 960 961 962 963 964 965 966
        return _rcRawValue[rcCalFunctionRoll];
    } else {
        return 1500;
    }
}

int RadioComponentController::pitchChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
967
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
968 969 970 971 972 973 974 975
        return _rcRawValue[rcCalFunctionPitch];
    } else {
        return 1500;
    }
}

int RadioComponentController::yawChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
976
    if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
977 978 979 980 981 982 983 984
        return _rcRawValue[rcCalFunctionYaw];
    } else {
        return 1500;
    }
}

int RadioComponentController::throttleChannelRCValue(void)
{
Don Gagne's avatar
Don Gagne committed
985
    if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
986 987 988 989 990 991 992 993
        return _rcRawValue[rcCalFunctionThrottle];
    } else {
        return 1500;
    }
}

bool RadioComponentController::rollChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
994
    return _rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax();
Don Gagne's avatar
Don Gagne committed
995 996 997 998
}

bool RadioComponentController::pitchChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
999
    return _rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax();
Don Gagne's avatar
Don Gagne committed
1000 1001 1002 1003
}

bool RadioComponentController::yawChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
1004
    return _rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax();
Don Gagne's avatar
Don Gagne committed
1005 1006 1007 1008
}

bool RadioComponentController::throttleChannelMapped(void)
{
Don Gagne's avatar
Don Gagne committed
1009
    return _rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax();
Don Gagne's avatar
Don Gagne committed
1010 1011 1012 1013
}

bool RadioComponentController::rollChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1014
    if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1015 1016 1017 1018 1019 1020 1021 1022
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionRoll]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::pitchChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1023
    if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1024 1025 1026 1027 1028 1029 1030 1031
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionPitch]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::yawChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1032
    if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1033 1034 1035 1036 1037 1038 1039 1040
        return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionYaw]].reversed;
    } else {
        return false;
    }
}

bool RadioComponentController::throttleChannelReversed(void)
{
Don Gagne's avatar
Don Gagne committed
1041
    if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) {
Don Gagne's avatar
Don Gagne committed
1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069
        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());
1070
}
Don Gagne's avatar
Don Gagne committed
1071 1072 1073

void RadioComponentController::copyTrims(void)
{
1074
    _uas->startCalibration(UASInterface::StartCalibrationCopyTrims);
Don Gagne's avatar
Don Gagne committed
1075
}
Don Gagne's avatar
Don Gagne committed
1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090

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