PX4AdvancedFlightModesController.cc 27.3 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

Don Gagne's avatar
Don Gagne committed
10 11 12 13

/// @file
///     @author Don Gagne <don@thegagnes.com>

Don Gagne's avatar
Don Gagne committed
14
#include "PX4AdvancedFlightModesController.h"
Don Gagne's avatar
Don Gagne committed
15 16 17 18 19
#include "QGCMAVLink.h"

#include <QVariant>
#include <QQmlProperty>

Don Gagne's avatar
Don Gagne committed
20
PX4AdvancedFlightModesController::PX4AdvancedFlightModesController(void) :
Don Gagne's avatar
Don Gagne committed
21
    _validConfiguration(false),
Don Gagne's avatar
Don Gagne committed
22 23 24 25 26 27 28 29 30 31 32
    _channelCount(18),
    _manualModeSelected(false),
    _assistModeSelected(false),
    _autoModeSelected(false),
    _acroModeSelected(false),
    _altCtlModeSelected(false),
    _posCtlModeSelected(false),
    _missionModeSelected(false),
    _loiterModeSelected(false),
    _returnModeSelected(false),
    _offboardModeSelected(false)
Don Gagne's avatar
Don Gagne committed
33
{
Don Gagne's avatar
Don Gagne committed
34
    QStringList usedParams;
Don Gagne's avatar
Don Gagne committed
35 36
    usedParams << "RC_MAP_THROTTLE" << "RC_MAP_YAW" << "RC_MAP_PITCH" << "RC_MAP_ROLL" << "RC_MAP_FLAPS" << "RC_MAP_AUX1" << "RC_MAP_AUX2" <<
        "RC_MAP_MODE_SW" << "RC_MAP_RETURN_SW" << "RC_MAP_LOITER_SW" << "RC_MAP_POSCTL_SW" << "RC_MAP_OFFB_SW" << "RC_MAP_ACRO_SW";
Don Gagne's avatar
Don Gagne committed
37 38 39
    if (!_allParametersExists(FactSystem::defaultComponentId, usedParams)) {
        return;
    }
Don Gagne's avatar
Don Gagne committed
40 41

    _init();
Don Gagne's avatar
Don Gagne committed
42
    _validateConfiguration();
Don Gagne's avatar
Don Gagne committed
43
    
Don Gagne's avatar
Don Gagne committed
44
    connect(_vehicle, &Vehicle::rcChannelsChanged, this, &PX4AdvancedFlightModesController::_rcChannelsChanged);
Don Gagne's avatar
Don Gagne committed
45 46
}

Don Gagne's avatar
Don Gagne committed
47
void PX4AdvancedFlightModesController::_init(void)
Don Gagne's avatar
Don Gagne committed
48
{
Don Gagne's avatar
Don Gagne committed
49
    // FIXME: What about VTOL? That confuses the whole Flight Mode naming scheme
Don Gagne's avatar
Don Gagne committed
50
    _fixedWing = _vehicle->fixedWing();
Don Gagne's avatar
Don Gagne committed
51 52 53 54 55 56 57 58 59 60 61
    
    // We need to know min and max for channel in order to calculate percentage range
    for (int channel=0; channel<_chanMax; channel++) {
        QString rcMinParam, rcMaxParam, rcRevParam;
        
        rcMinParam = QString("RC%1_MIN").arg(channel+1);
        rcMaxParam = QString("RC%1_MAX").arg(channel+1);
        rcRevParam = QString("RC%1_REV").arg(channel+1);
        
        QVariant value;
        
Don Gagne's avatar
Don Gagne committed
62 63
        _rgRCMin[channel] = getParameterFact(FactSystem::defaultComponentId, rcMinParam)->rawValue().toInt();
        _rgRCMax[channel] = getParameterFact(FactSystem::defaultComponentId, rcMaxParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
64
        
Don Gagne's avatar
Don Gagne committed
65
        float floatReversed = getParameterFact(-1, rcRevParam)->rawValue().toFloat();
Don Gagne's avatar
Don Gagne committed
66 67 68
        _rgRCReversed[channel] = floatReversed == -1.0f;
        
        _rcValues[channel] = 0.0;
Don Gagne's avatar
Don Gagne committed
69
    }
Don Gagne's avatar
Don Gagne committed
70 71 72

    // RC_CHAN_CNT parameter is set by Radio Cal to specify the number of radio channels.
    if (parameterExists(FactSystem::defaultComponentId, "RC_CHAN_CNT")) {
Don Gagne's avatar
Don Gagne committed
73
        _channelCount = getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
74 75 76 77 78 79 80 81
    } else {
        _channelCount =_chanMax;
    }
    if (_channelCount <= 0 || _channelCount > _chanMax) {
        // Parameter exists, but has not yet been set or is invalid. Use default
        _channelCount = _chanMax;
    }
    
Don Gagne's avatar
Don Gagne committed
82 83 84
    int modeChannel = getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
    int posCtlChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
    int loiterChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
85 86 87 88 89
    
    if (posCtlChannel == 0) {
        // PosCtl disabled so AltCtl must move back to main Mode switch
        _assistModeVisible = false;
    } else {
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
90
        // Assist mode is visible if AltCtl/PosCtl are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
91 92 93 94 95 96 97
        _assistModeVisible = posCtlChannel != modeChannel;
    }
    
    if (loiterChannel == 0) {
        // Loiter disabled so Mission must move back to main Mode switch
        _autoModeVisible = false;
    } else {
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
98
        // Auto mode is visible if Mission/Loiter are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
99 100 101 102 103 104 105 106 107
        _autoModeVisible = loiterChannel != modeChannel;
    }
    
    // Setup the channel combobox model
    
    QList<int> usedChannels;
    QStringList attitudeParams;
    
    attitudeParams << "RC_MAP_THROTTLE" << "RC_MAP_YAW" << "RC_MAP_PITCH" << "RC_MAP_ROLL" << "RC_MAP_FLAPS" << "RC_MAP_AUX1" << "RC_MAP_AUX2";
108
    foreach(const QString &attitudeParam, attitudeParams) {
Don Gagne's avatar
Don Gagne committed
109
        int channel = getParameterFact(-1, attitudeParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
110 111 112 113 114 115 116
        if (channel != 0) {
            usedChannels << channel;
        }
    }
    
    _channelListModel << "Disabled";
    _channelListModelChannel << 0;
Don Gagne's avatar
Don Gagne committed
117
    for (int channel=1; channel<=_channelCount; channel++) {
Don Gagne's avatar
Don Gagne committed
118 119 120 121 122 123
        if (!usedChannels.contains(channel)) {
            _channelListModel << QString("Channel %1").arg(channel);
            _channelListModelChannel << channel;
        }
    }
    
124 125 126 127 128 129 130 131 132 133 134
    // Setup reserved channels string for ui
    
    bool first = true;
    foreach (int usedChannel, usedChannels) {
        if (!first) {
            _reservedChannels += ", ";
        }
        _reservedChannels += QString("%1").arg(usedChannel);
        first = false;
    }
    
Don Gagne's avatar
Don Gagne committed
135
    _recalcModeRows();
Don Gagne's avatar
Don Gagne committed
136 137 138 139
}

/// This will look for parameter settings which would cause the config to not run correctly.
/// It will set _validConfiguration and _configurationErrors as needed.
Don Gagne's avatar
Don Gagne committed
140
void PX4AdvancedFlightModesController::_validateConfiguration(void)
Don Gagne's avatar
Don Gagne committed
141 142 143 144 145
{
    _validConfiguration = true;
    
    // Make sure switches are valid and within channel range
    
Don Gagne's avatar
Don Gagne committed
146
    QStringList switchParams;
Don Gagne's avatar
Don Gagne committed
147 148
    QList<int> switchMappings;
    
Don Gagne's avatar
Don Gagne committed
149
    switchParams << "RC_MAP_MODE_SW"  << "RC_MAP_ACRO_SW"  << "RC_MAP_POSCTL_SW" << "RC_MAP_LOITER_SW" << "RC_MAP_RETURN_SW" << "RC_MAP_OFFB_SW";
Don Gagne's avatar
Don Gagne committed
150 151
    
    for(int i=0; i<switchParams.count(); i++) {
Don Gagne's avatar
Don Gagne committed
152
        int map = getParameterFact(FactSystem::defaultComponentId, switchParams[i])->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
153 154 155 156
        switchMappings << map;
        
        if (map < 0 || map > _channelCount) {
            _validConfiguration = false;
157
            _configurationErrors += tr("%1 is set to %2. Mapping must between 0 and %3 (inclusive).\n").arg(switchParams[i]).arg(map).arg(_channelCount);
Don Gagne's avatar
Don Gagne committed
158 159 160
        }
    }
    
Don Gagne's avatar
Don Gagne committed
161
    // Make sure mode switches are not double-mapped
Don Gagne's avatar
Don Gagne committed
162
    
Don Gagne's avatar
Don Gagne committed
163
    QStringList attitudeParams;
Don Gagne's avatar
Don Gagne committed
164
    
165
    attitudeParams << "RC_MAP_THROTTLE" << "RC_MAP_YAW" << "RC_MAP_PITCH" << "RC_MAP_ROLL" << "RC_MAP_FLAPS" << "RC_MAP_AUX1" << "RC_MAP_AUX2";
Don Gagne's avatar
Don Gagne committed
166 167

    for (int i=0; i<attitudeParams.count(); i++) {
Don Gagne's avatar
Don Gagne committed
168
        int map = getParameterFact(FactSystem::defaultComponentId, attitudeParams[i])->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
169 170 171 172

        for (int j=0; j<switchParams.count(); j++) {
            if (map != 0 && map == switchMappings[j]) {
                _validConfiguration = false;
173
                _configurationErrors += tr("%1 is set to same channel as %2.\n").arg(switchParams[j]).arg(attitudeParams[i]);
Don Gagne's avatar
Don Gagne committed
174 175 176
            }
        }
    }
177
    
Don Gagne's avatar
Don Gagne committed
178
    // Validate thresholds within range
179
    
Don Gagne's avatar
Don Gagne committed
180
    QStringList thresholdParams;
181
    
Don Gagne's avatar
Don Gagne committed
182
    thresholdParams << "RC_ASSIST_TH" << "RC_AUTO_TH" << "RC_ACRO_TH" << "RC_POSCTL_TH" << "RC_LOITER_TH" << "RC_RETURN_TH" << "RC_OFFB_TH";
183
    
184
    foreach(const QString &thresholdParam, thresholdParams) {
Don Gagne's avatar
Don Gagne committed
185
        float threshold = getParameterFact(-1, thresholdParam)->rawValue().toFloat();
Don Gagne's avatar
Don Gagne committed
186 187
        if (threshold < 0.0f || threshold > 1.0f) {
            _validConfiguration = false;
188
            _configurationErrors += tr("%1 is set to %2. Threshold must between 0.0 and 1.0 (inclusive).\n").arg(thresholdParam).arg(threshold);
Don Gagne's avatar
Don Gagne committed
189 190 191 192
        }
    }
}

Don Gagne's avatar
Don Gagne committed
193
/// Connected to Vehicle::rcChannelsChanged signal
Don Gagne's avatar
Don Gagne committed
194
void PX4AdvancedFlightModesController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
195
{
Don Gagne's avatar
Don Gagne committed
196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213
    for (int channel=0; channel<channelCount; channel++) {
        int channelValue = pwmValues[channel];

        if (channelValue != -1) {
            if (channelValue < _rgRCMin[channel]) {
                channelValue= _rgRCMin[channel];
            }
            if (channelValue > _rgRCMax[channel]) {
                channelValue= _rgRCMax[channel];
            }

            float percentRange = (channelValue - _rgRCMin[channel]) / (float)(_rgRCMax[channel] - _rgRCMin[channel]);
            if (_rgRCReversed[channel]) {
                percentRange = 1.0 - percentRange;
            }

            _rcValues[channel] = percentRange;
        }
Don Gagne's avatar
Don Gagne committed
214 215
    }
    
Don Gagne's avatar
Don Gagne committed
216 217
    _recalcModeSelections();
    
Don Gagne's avatar
Don Gagne committed
218 219 220
    emit switchLiveRangeChanged();
}

Don Gagne's avatar
Don Gagne committed
221
double PX4AdvancedFlightModesController::_switchLiveRange(const QString& param)
Don Gagne's avatar
Don Gagne committed
222 223 224
{
    QVariant value;
    
Don Gagne's avatar
Don Gagne committed
225
    int channel = getParameterFact(-1, param)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
226
    if (channel == 0) {
Don Gagne's avatar
Don Gagne committed
227
        return 0.0;
Don Gagne's avatar
Don Gagne committed
228 229 230 231 232
    } else {
        return _rcValues[channel - 1];
    }
}

Don Gagne's avatar
Don Gagne committed
233
double PX4AdvancedFlightModesController::manualModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
234 235 236 237
{
    return _switchLiveRange("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
238
double PX4AdvancedFlightModesController::assistModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
239 240 241 242
{
    return manualModeRcValue();
}

Don Gagne's avatar
Don Gagne committed
243
double PX4AdvancedFlightModesController::autoModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
244 245 246 247
{
    return manualModeRcValue();
}

Don Gagne's avatar
Don Gagne committed
248
double PX4AdvancedFlightModesController::acroModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
249 250 251 252
{
    return _switchLiveRange("RC_MAP_ACRO_SW");
}

Don Gagne's avatar
Don Gagne committed
253
double PX4AdvancedFlightModesController::altCtlModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
254
{
Don Gagne's avatar
Don Gagne committed
255
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
256 257 258 259 260 261 262 263
    
    if (posCtlSwitchChannel == 0) {
        return _switchLiveRange("RC_MAP_MODE_SW");
    } else {
        return _switchLiveRange("RC_MAP_POSCTL_SW");
    }
}

Don Gagne's avatar
Don Gagne committed
264
double PX4AdvancedFlightModesController::posCtlModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
265 266 267 268
{
    return _switchLiveRange("RC_MAP_POSCTL_SW");
}

Don Gagne's avatar
Don Gagne committed
269
double PX4AdvancedFlightModesController::missionModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
270
{
Don Gagne's avatar
Don Gagne committed
271 272
    int returnSwitchChannel = getParameterFact(-1, "RC_MAP_RETURN_SW")->rawValue().toInt();
    int loiterSwitchChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290

    const char* switchChannelParam = "RC_MAP_MODE_SW";
    
    if (returnSwitchChannel == 0) {
        if (loiterSwitchChannel != 0) {
            switchChannelParam = "RC_MAP_LOITER_SW";
        }
    } else {
        if (loiterSwitchChannel == 0) {
            switchChannelParam = "RC_MAP_RETURN_SW";
        } else {
            switchChannelParam = "RC_MAP_LOITER_SW";
        }
    }
    
    return _switchLiveRange(switchChannelParam);
}

Don Gagne's avatar
Don Gagne committed
291
double PX4AdvancedFlightModesController::loiterModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
292 293 294 295
{
    return _switchLiveRange("RC_MAP_LOITER_SW");
}

Don Gagne's avatar
Don Gagne committed
296
double PX4AdvancedFlightModesController::returnModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
297 298 299 300
{
    return _switchLiveRange("RC_MAP_RETURN_SW");
}

Don Gagne's avatar
Don Gagne committed
301
double PX4AdvancedFlightModesController::offboardModeRcValue(void)
302 303 304 305
{
    return _switchLiveRange("RC_MAP_OFFB_SW");
}

Don Gagne's avatar
Don Gagne committed
306
void PX4AdvancedFlightModesController::_recalcModeSelections(void)
Don Gagne's avatar
Don Gagne committed
307
{
Don Gagne's avatar
Don Gagne committed
308 309 310 311 312 313 314 315 316 317 318 319
    _manualModeSelected = false;
    _assistModeSelected = false;
    _autoModeSelected = false;
    _acroModeSelected = false;
    _altCtlModeSelected = false;
    _posCtlModeSelected = false;
    _missionModeSelected = false;
    _loiterModeSelected = false;
    _returnModeSelected = false;
    _offboardModeSelected = false;
    
    // Convert channels to 0-based, -1 signals not mapped
Don Gagne's avatar
Don Gagne committed
320 321 322 323 324 325 326 327 328 329 330 331 332 333
    int modeSwitchChannel =     getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt() - 1;
    int acroSwitchChannel =     getParameterFact(-1, "RC_MAP_ACRO_SW")->rawValue().toInt() - 1;
    int posCtlSwitchChannel =   getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt() - 1;
    int loiterSwitchChannel =   getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt() - 1;
    int returnSwitchChannel =   getParameterFact(-1, "RC_MAP_RETURN_SW")->rawValue().toInt() - 1;
    int offboardSwitchChannel = getParameterFact(-1, "RC_MAP_OFFB_SW")->rawValue().toInt() - 1;
    
    double autoThreshold =      getParameterFact(-1, "RC_AUTO_TH")->rawValue().toDouble();
    double assistThreshold =    getParameterFact(-1, "RC_ASSIST_TH")->rawValue().toDouble();
    double acroThreshold =      getParameterFact(-1, "RC_ACRO_TH")->rawValue().toDouble();
    double posCtlThreshold =    getParameterFact(-1, "RC_POSCTL_TH")->rawValue().toDouble();
    double loiterThreshold =    getParameterFact(-1, "RC_LOITER_TH")->rawValue().toDouble();
    double returnThreshold =    getParameterFact(-1, "RC_RETURN_TH")->rawValue().toDouble();
    double offboardThreshold =  getParameterFact(-1, "RC_OFFB_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363
    
    if (modeSwitchChannel >= 0) {
        if (offboardSwitchChannel >= 0 && _rcValues[offboardSwitchChannel] >= offboardThreshold) {
            _offboardModeSelected = true;
        } else if (returnSwitchChannel >= 0 && _rcValues[returnSwitchChannel] >= returnThreshold) {
            _returnModeSelected = true;
        } else {
            if (_rcValues[modeSwitchChannel] >= autoThreshold) {
                _autoModeSelected = true;
                if (loiterSwitchChannel >= 0 && _rcValues[loiterSwitchChannel] >= loiterThreshold) {
                    _loiterModeSelected = true;
                } else {
                    _missionModeSelected = true;
                }
            } else if (_rcValues[modeSwitchChannel] >= assistThreshold) {
                _assistModeSelected = true;
                if (posCtlSwitchChannel >= 0 && _rcValues[posCtlSwitchChannel] >= posCtlThreshold) {
                    _posCtlModeSelected = true;
                } else {
                    _altCtlModeSelected = true;
                }
            } else if (acroSwitchChannel >= 0 && _rcValues[acroSwitchChannel] >= acroThreshold) {
                _acroModeSelected = true;
            } else {
                _manualModeSelected = true;
            }
        }
    }
    
    emit modesSelectedChanged();
Don Gagne's avatar
Don Gagne committed
364 365
}

Don Gagne's avatar
Don Gagne committed
366
void PX4AdvancedFlightModesController::_recalcModeRows(void)
Don Gagne's avatar
Don Gagne committed
367
{
Don Gagne's avatar
Don Gagne committed
368 369 370 371 372 373
    int modeSwitchChannel =     getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
    int acroSwitchChannel =     getParameterFact(-1, "RC_MAP_ACRO_SW")->rawValue().toInt();
    int posCtlSwitchChannel =   getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
    int loiterSwitchChannel =   getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt();
    int returnSwitchChannel =   getParameterFact(-1, "RC_MAP_RETURN_SW")->rawValue().toInt();
    int offboardSwitchChannel = getParameterFact(-1, "RC_MAP_OFFB_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453
    
    if (modeSwitchChannel == 0) {
        _acroModeRow =      0;
        _assistModeRow =    1;
        _autoModeRow =      2;
        _altCtlModeRow =    3;
        _posCtlModeRow =    4;
        _loiterModeRow =    5;
        _missionModeRow =   6;
        _returnModeRow =    7;
        _offboardModeRow =  8;
    } else {
        int row = 0;
        
        // First set is all switches on main mode channel
        
        if (acroSwitchChannel == modeSwitchChannel) {
            _acroModeRow = row++;
        }
        _assistModeRow = row++;
        if (posCtlSwitchChannel == modeSwitchChannel) {
            _altCtlModeRow = row++;
            _posCtlModeRow = row++;
        } else if (posCtlSwitchChannel == 0) {
            _altCtlModeRow = row++;
        }
        _autoModeRow = row++;
        if (loiterSwitchChannel == modeSwitchChannel) {
            _missionModeRow = row++;
            _loiterModeRow = row++;
        } else if (loiterSwitchChannel == 0) {
            _missionModeRow = row++;
        }
        if (returnSwitchChannel == modeSwitchChannel) {
            _returnModeRow = row++;
        }
        if (offboardSwitchChannel == modeSwitchChannel) {
            _offboardModeRow = row++;
        }
        
        // Now individual enabled switches not on main mode channel
        
        if (acroSwitchChannel != 0 && acroSwitchChannel != modeSwitchChannel) {
            _acroModeRow = row++;
        }
        if (posCtlSwitchChannel != 0 && posCtlSwitchChannel != modeSwitchChannel) {
            _altCtlModeRow = row++;
            _posCtlModeRow = row++;
        }
        if (loiterSwitchChannel != 0 && loiterSwitchChannel != modeSwitchChannel) {
            _missionModeRow = row++;
            _loiterModeRow = row++;
        }
        if (returnSwitchChannel != 0 && returnSwitchChannel != modeSwitchChannel) {
            _returnModeRow = row++;
        }
        if (offboardSwitchChannel != 0 && offboardSwitchChannel != modeSwitchChannel) {
            _offboardModeRow = row++;
        }
        
        // Now disabled switches
        
        if (acroSwitchChannel == 0) {
            _acroModeRow = row++;
        }
        if (posCtlSwitchChannel == 0) {
            _posCtlModeRow = row++;
        }
        if (loiterSwitchChannel == 0) {
            _loiterModeRow = row++;
        }
        if (returnSwitchChannel == 0) {
            _returnModeRow = row++;
        }
        if (offboardSwitchChannel == 0) {
            _offboardModeRow = row++;
        }
    }
    
    emit modeRowsChanged();
Don Gagne's avatar
Don Gagne committed
454 455
}

Don Gagne's avatar
Don Gagne committed
456
double PX4AdvancedFlightModesController::manualModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
457
{
Don Gagne's avatar
Don Gagne committed
458 459 460
    return 0.0;
}

Don Gagne's avatar
Don Gagne committed
461
double PX4AdvancedFlightModesController::assistModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
462
{
Don Gagne's avatar
Don Gagne committed
463
    return getParameterFact(-1, "RC_ASSIST_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
464 465
}

Don Gagne's avatar
Don Gagne committed
466
double PX4AdvancedFlightModesController::autoModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
467
{
Don Gagne's avatar
Don Gagne committed
468
    return getParameterFact(-1, "RC_AUTO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
469 470
}

Don Gagne's avatar
Don Gagne committed
471
double PX4AdvancedFlightModesController::acroModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
472
{
Don Gagne's avatar
Don Gagne committed
473
    return getParameterFact(-1, "RC_ACRO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
474 475
}

Don Gagne's avatar
Don Gagne committed
476
double PX4AdvancedFlightModesController::altCtlModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
477
{
Don Gagne's avatar
Don Gagne committed
478
    return _assistModeVisible ? 0.0 : getParameterFact(-1, "RC_ASSIST_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
479 480
}

Don Gagne's avatar
Don Gagne committed
481
double PX4AdvancedFlightModesController::posCtlModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
482
{
Don Gagne's avatar
Don Gagne committed
483
    return getParameterFact(-1, "RC_POSCTL_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
484 485
}

Don Gagne's avatar
Don Gagne committed
486
double PX4AdvancedFlightModesController::missionModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
487
{
Don Gagne's avatar
Don Gagne committed
488
    return _autoModeVisible ? 0.0 : getParameterFact(-1, "RC_AUTO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
489 490 491
}


Don Gagne's avatar
Don Gagne committed
492
double PX4AdvancedFlightModesController::loiterModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
493
{
Don Gagne's avatar
Don Gagne committed
494
    return getParameterFact(-1, "RC_LOITER_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
495 496
}

Don Gagne's avatar
Don Gagne committed
497
double PX4AdvancedFlightModesController::returnModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
498
{
Don Gagne's avatar
Don Gagne committed
499
    return getParameterFact(-1, "RC_RETURN_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
500 501
}

Don Gagne's avatar
Don Gagne committed
502
double PX4AdvancedFlightModesController::offboardModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
503
{
Don Gagne's avatar
Don Gagne committed
504
    return getParameterFact(-1, "RC_OFFB_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
505 506
}

Don Gagne's avatar
Don Gagne committed
507
void PX4AdvancedFlightModesController::setAssistModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
508
{
Don Gagne's avatar
Don Gagne committed
509
    getParameterFact(-1, "RC_ASSIST_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
510 511 512
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
513
void PX4AdvancedFlightModesController::setAutoModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
514
{
Don Gagne's avatar
Don Gagne committed
515
    getParameterFact(-1, "RC_AUTO_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
516 517 518
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
519
void PX4AdvancedFlightModesController::setAcroModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
520
{
Don Gagne's avatar
Don Gagne committed
521
    getParameterFact(-1, "RC_ACRO_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
522 523 524
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
525
void PX4AdvancedFlightModesController::setAltCtlModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
526 527 528 529
{
    setAssistModeThreshold(threshold);
}

Don Gagne's avatar
Don Gagne committed
530
void PX4AdvancedFlightModesController::setPosCtlModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
531
{
Don Gagne's avatar
Don Gagne committed
532
    getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
533 534 535
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
536
void PX4AdvancedFlightModesController::setMissionModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
537 538 539 540
{
    setAutoModeThreshold(threshold);
}

Don Gagne's avatar
Don Gagne committed
541
void PX4AdvancedFlightModesController::setLoiterModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
542
{
Don Gagne's avatar
Don Gagne committed
543
    getParameterFact(-1, "RC_LOITER_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
544 545 546
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
547
void PX4AdvancedFlightModesController::setReturnModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
548
{
Don Gagne's avatar
Don Gagne committed
549
    getParameterFact(-1, "RC_RETURN_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
550 551 552
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
553
void PX4AdvancedFlightModesController::setOffboardModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
554
{
Don Gagne's avatar
Don Gagne committed
555
    getParameterFact(-1, "RC_OFFB_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
556 557 558
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
559
int PX4AdvancedFlightModesController::_channelToChannelIndex(int channel)
Don Gagne's avatar
Don Gagne committed
560 561 562 563
{
    return _channelListModelChannel.lastIndexOf(channel);
}

Don Gagne's avatar
Don Gagne committed
564
int PX4AdvancedFlightModesController::_channelToChannelIndex(const QString& channelParam)
Don Gagne's avatar
Don Gagne committed
565
{
Don Gagne's avatar
Don Gagne committed
566
    return _channelToChannelIndex(getParameterFact(-1, channelParam)->rawValue().toInt());
Don Gagne's avatar
Don Gagne committed
567 568
}

Don Gagne's avatar
Don Gagne committed
569
int PX4AdvancedFlightModesController::manualModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
570 571 572 573
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
574
int PX4AdvancedFlightModesController::assistModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
575 576 577 578
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
579
int PX4AdvancedFlightModesController::autoModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
580 581 582 583
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
584
int PX4AdvancedFlightModesController::acroModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
585 586 587 588
{
    return _channelToChannelIndex("RC_MAP_ACRO_SW");
}

Don Gagne's avatar
Don Gagne committed
589
int PX4AdvancedFlightModesController::altCtlModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
590
{
Don Gagne's avatar
Don Gagne committed
591
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
592 593 594 595 596 597 598 599
    
    if (posCtlSwitchChannel == 0) {
        return _channelToChannelIndex("RC_MAP_MODE_SW");
    } else {
        return _channelToChannelIndex(posCtlSwitchChannel);
    }
}

Don Gagne's avatar
Don Gagne committed
600
int PX4AdvancedFlightModesController::posCtlModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
601 602 603 604
{
    return _channelToChannelIndex("RC_MAP_POSCTL_SW");
}

Don Gagne's avatar
Don Gagne committed
605
int PX4AdvancedFlightModesController::loiterModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
606 607 608 609
{
    return _channelToChannelIndex("RC_MAP_LOITER_SW");
}

Don Gagne's avatar
Don Gagne committed
610
int PX4AdvancedFlightModesController::missionModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
611
{
Don Gagne's avatar
Don Gagne committed
612
    int loiterSwitchChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
613 614 615 616 617 618 619 620
    
    if (loiterSwitchChannel == 0) {
        return _channelToChannelIndex("RC_MAP_MODE_SW");
    } else {
        return _channelToChannelIndex(loiterSwitchChannel);
    }
}

Don Gagne's avatar
Don Gagne committed
621
int PX4AdvancedFlightModesController::returnModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
622 623 624 625
{
    return _channelToChannelIndex("RC_MAP_RETURN_SW");
}

Don Gagne's avatar
Don Gagne committed
626
int PX4AdvancedFlightModesController::offboardModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
627 628 629 630
{
    return _channelToChannelIndex("RC_MAP_OFFB_SW");
}

Don Gagne's avatar
Don Gagne committed
631
int PX4AdvancedFlightModesController::_channelIndexToChannel(int index)
Don Gagne's avatar
Don Gagne committed
632 633 634 635
{
    return _channelListModelChannel[index];
}

Don Gagne's avatar
Don Gagne committed
636
void PX4AdvancedFlightModesController::setManualModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
637
{
Don Gagne's avatar
Don Gagne committed
638
    getParameterFact(-1, "RC_MAP_MODE_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
639 640 641
    
    _recalcModeSelections();
    _recalcModeRows();
642 643
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
644 645
}

Don Gagne's avatar
Don Gagne committed
646
void PX4AdvancedFlightModesController::setAcroModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
647
{
Don Gagne's avatar
Don Gagne committed
648
    getParameterFact(-1, "RC_MAP_ACRO_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
649 650 651 652 653
    
    _recalcModeSelections();
    _recalcModeRows();
}

Don Gagne's avatar
Don Gagne committed
654
void PX4AdvancedFlightModesController::setPosCtlModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
655 656 657
{
    int channel = _channelIndexToChannel(index);
    
Don Gagne's avatar
Don Gagne committed
658
    getParameterFact(-1, "RC_MAP_POSCTL_SW")->setRawValue(channel);
Don Gagne's avatar
Don Gagne committed
659 660 661 662 663
    
    if (channel == 0) {
        // PosCtl disabled so AltCtl must move back to main Mode switch
        _assistModeVisible = false;
    } else {
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
664
        // Assist mode is visible if AltCtl/PosCtl are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
665
        _assistModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
666 667 668 669 670 671
    }
    
    emit modesVisibleChanged();

    _recalcModeSelections();
    _recalcModeRows();
672 673
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
674 675
}

Don Gagne's avatar
Don Gagne committed
676
void PX4AdvancedFlightModesController::setLoiterModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
677 678 679
{
    int channel = _channelIndexToChannel(index);
    
Don Gagne's avatar
Don Gagne committed
680
    getParameterFact(-1, "RC_MAP_LOITER_SW")->setRawValue(channel);
Don Gagne's avatar
Don Gagne committed
681 682 683 684 685
    
    if (channel == 0) {
        // Loiter disabled so Mission must move back to main Mode switch
        _autoModeVisible = false;
    } else {
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
686
        // Auto mode is visible if Mission/Loiter are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
687
        _autoModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
688 689 690 691 692 693
    }
    
    emit modesVisibleChanged();
    
    _recalcModeSelections();
    _recalcModeRows();
694 695
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
696 697
}

Don Gagne's avatar
Don Gagne committed
698
void PX4AdvancedFlightModesController::setReturnModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
699
{
Don Gagne's avatar
Don Gagne committed
700
    getParameterFact(-1, "RC_MAP_RETURN_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
701 702
    _recalcModeSelections();
    _recalcModeRows();
703 704
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
705 706
}

Don Gagne's avatar
Don Gagne committed
707
void PX4AdvancedFlightModesController::setOffboardModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
708
{
Don Gagne's avatar
Don Gagne committed
709
    getParameterFact(-1, "RC_MAP_OFFB_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
710 711
    _recalcModeSelections();
    _recalcModeRows();
712 713
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
714 715
}

Don Gagne's avatar
Don Gagne committed
716
void PX4AdvancedFlightModesController::generateThresholds(void)
Don Gagne's avatar
Don Gagne committed
717 718 719 720 721 722 723
{
    // Reset all thresholds to 0.0
    
    QStringList thresholdParams;
    
    thresholdParams << "RC_ASSIST_TH" << "RC_AUTO_TH" << "RC_ACRO_TH" << "RC_POSCTL_TH" << "RC_LOITER_TH" << "RC_RETURN_TH" << "RC_OFFB_TH";
    
724
    foreach(const QString &thresholdParam, thresholdParams) {
Don Gagne's avatar
Don Gagne committed
725
        getParameterFact(-1, thresholdParam)->setRawValue(0.0f);
Don Gagne's avatar
Don Gagne committed
726 727 728 729
    }
    
    // Redistribute
    
Don Gagne's avatar
Don Gagne committed
730 731 732 733 734 735
    int modeChannel =       getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
    int acroChannel =       getParameterFact(-1, "RC_MAP_ACRO_SW")->rawValue().toInt();
    int posCtlChannel =     getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
    int loiterChannel =     getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt();
    int returnChannel =     getParameterFact(-1, "RC_MAP_RETURN_SW")->rawValue().toInt();
    int offboardChannel =   getParameterFact(-1, "RC_MAP_OFFB_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756
    
    if (modeChannel != 0) {
        int positions = 3;  // Manual/Assist/Auto always exist
        
        bool acroOnModeSwitch = modeChannel == acroChannel;
        bool posCtlOnModeSwitch = modeChannel == posCtlChannel;
        bool loiterOnModeSwitch = modeChannel == loiterChannel;
        bool returnOnModeSwitch = modeChannel == returnChannel;
        bool offboardOnModeSwitch = modeChannel == offboardChannel;
        
        positions += acroOnModeSwitch ? 1 : 0;
        positions += posCtlOnModeSwitch ? 1 : 0;
        positions += loiterOnModeSwitch ? 1 : 0;
        positions += returnOnModeSwitch ? 1 : 0;
        positions += offboardOnModeSwitch ? 1 : 0;
        
        float increment = 1.0f / positions;
        float currentThreshold = 0.0f;
        
        if (acroOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
757
            getParameterFact(-1, "RC_ACRO_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
758 759 760 761
            acroChannel = 0;
        }
        
        currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
762
        getParameterFact(-1, "RC_ASSIST_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
763 764
        if (posCtlOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
765
            getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
766 767 768 769
            posCtlChannel = 0;
        }
        
        currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
770
        getParameterFact(-1, "RC_AUTO_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
771 772
        if (loiterOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
773
            getParameterFact(-1, "RC_LOITER_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
774 775 776 777 778
            loiterChannel = 0;
        }
        
        if (returnOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
779
            getParameterFact(-1, "RC_RETURN_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
780 781 782 783 784
            returnChannel = 0;
        }
        
        if (offboardOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
785
            getParameterFact(-1, "RC_OFFB_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
786 787 788 789 790 791 792
            offboardChannel = 0;
        }
    }
    
    if (acroChannel != 0) {
        // If only two positions don't set threshold at midrange. Setting to 0.25
        // allows for this channel to work with either a two or three position switch
Don Gagne's avatar
Don Gagne committed
793
        getParameterFact(-1, "RC_ACRO_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
794 795 796
    }
    
    if (posCtlChannel != 0) {
Don Gagne's avatar
Don Gagne committed
797
        getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
798 799 800
    }
    
    if (loiterChannel != 0) {
Don Gagne's avatar
Don Gagne committed
801
        getParameterFact(-1, "RC_LOITER_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
802 803 804
    }
    
    if (returnChannel != 0) {
Don Gagne's avatar
Don Gagne committed
805
        getParameterFact(-1, "RC_RETURN_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
806 807 808
    }
    
    if (offboardChannel != 0) {
Don Gagne's avatar
Don Gagne committed
809
        getParameterFact(-1, "RC_OFFB_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
810 811 812
    }
    
    emit thresholdsChanged();
Don Gagne's avatar
Don Gagne committed
813
}