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 20
#include "QGCMAVLink.h"
#include "AutoPilotPluginManager.h"

#include <QVariant>
#include <QQmlProperty>

Don Gagne's avatar
Don Gagne committed
21
PX4AdvancedFlightModesController::PX4AdvancedFlightModesController(void) :
Don Gagne's avatar
Don Gagne committed
22
    _validConfiguration(false),
Don Gagne's avatar
Don Gagne committed
23 24 25 26 27 28 29 30 31 32 33
    _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
34
{
Don Gagne's avatar
Don Gagne committed
35
    QStringList usedParams;
Don Gagne's avatar
Don Gagne committed
36 37
    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
38 39 40
    if (!_allParametersExists(FactSystem::defaultComponentId, usedParams)) {
        return;
    }
Don Gagne's avatar
Don Gagne committed
41 42

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

Don Gagne's avatar
Don Gagne committed
48
void PX4AdvancedFlightModesController::_init(void)
Don Gagne's avatar
Don Gagne committed
49
{
Don Gagne's avatar
Don Gagne committed
50
    // FIXME: What about VTOL? That confuses the whole Flight Mode naming scheme
Don Gagne's avatar
Don Gagne committed
51
    _fixedWing = _vehicle->fixedWing();
Don Gagne's avatar
Don Gagne committed
52 53 54 55 56 57 58 59 60 61 62
    
    // 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
63 64
        _rgRCMin[channel] = getParameterFact(FactSystem::defaultComponentId, rcMinParam)->rawValue().toInt();
        _rgRCMax[channel] = getParameterFact(FactSystem::defaultComponentId, rcMaxParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
65
        
Don Gagne's avatar
Don Gagne committed
66
        float floatReversed = getParameterFact(-1, rcRevParam)->rawValue().toFloat();
Don Gagne's avatar
Don Gagne committed
67 68 69
        _rgRCReversed[channel] = floatReversed == -1.0f;
        
        _rcValues[channel] = 0.0;
Don Gagne's avatar
Don Gagne committed
70
    }
Don Gagne's avatar
Don Gagne committed
71 72 73

    // 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
74
        _channelCount = getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
75 76 77 78 79 80 81 82
    } 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
83 84 85
    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
86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
    
    if (posCtlChannel == 0) {
        // PosCtl disabled so AltCtl must move back to main Mode switch
        _assistModeVisible = false;
    } else {
        // Assist mode is visible if AltCtl/PosCtl are on seperate channel from main Mode switch
        _assistModeVisible = posCtlChannel != modeChannel;
    }
    
    if (loiterChannel == 0) {
        // Loiter disabled so Mission must move back to main Mode switch
        _autoModeVisible = false;
    } else {
        // Auto mode is visible if Mission/Loiter are on seperate channel from main Mode switch
        _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";
109
    foreach(const QString &attitudeParam, attitudeParams) {
Don Gagne's avatar
Don Gagne committed
110
        int channel = getParameterFact(-1, attitudeParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
111 112 113 114 115 116 117
        if (channel != 0) {
            usedChannels << channel;
        }
    }
    
    _channelListModel << "Disabled";
    _channelListModelChannel << 0;
Don Gagne's avatar
Don Gagne committed
118
    for (int channel=1; channel<=_channelCount; channel++) {
Don Gagne's avatar
Don Gagne committed
119 120 121 122 123 124
        if (!usedChannels.contains(channel)) {
            _channelListModel << QString("Channel %1").arg(channel);
            _channelListModelChannel << channel;
        }
    }
    
125 126 127 128 129 130 131 132 133 134 135
    // 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
136
    _recalcModeRows();
Don Gagne's avatar
Don Gagne committed
137 138 139 140
}

/// 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
141
void PX4AdvancedFlightModesController::_validateConfiguration(void)
Don Gagne's avatar
Don Gagne committed
142 143 144 145 146
{
    _validConfiguration = true;
    
    // Make sure switches are valid and within channel range
    
Don Gagne's avatar
Don Gagne committed
147
    QStringList switchParams;
Don Gagne's avatar
Don Gagne committed
148 149
    QList<int> switchMappings;
    
Don Gagne's avatar
Don Gagne committed
150
    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
151 152
    
    for(int i=0; i<switchParams.count(); i++) {
Don Gagne's avatar
Don Gagne committed
153
        int map = getParameterFact(FactSystem::defaultComponentId, switchParams[i])->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
154 155 156 157
        switchMappings << map;
        
        if (map < 0 || map > _channelCount) {
            _validConfiguration = false;
Don Gagne's avatar
Don Gagne committed
158
            _configurationErrors += QString("%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
159 160 161
        }
    }
    
Don Gagne's avatar
Don Gagne committed
162
    // Make sure mode switches are not double-mapped
Don Gagne's avatar
Don Gagne committed
163
    
Don Gagne's avatar
Don Gagne committed
164
    QStringList attitudeParams;
Don Gagne's avatar
Don Gagne committed
165
    
166
    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
167 168

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

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

Don Gagne's avatar
Don Gagne committed
194
/// Connected to Vehicle::rcChannelsChanged signal
Don Gagne's avatar
Don Gagne committed
195
void PX4AdvancedFlightModesController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
196
{
Don Gagne's avatar
Don Gagne committed
197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214
    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
215 216
    }
    
Don Gagne's avatar
Don Gagne committed
217 218
    _recalcModeSelections();
    
Don Gagne's avatar
Don Gagne committed
219 220 221
    emit switchLiveRangeChanged();
}

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

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

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

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

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

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

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

Don Gagne's avatar
Don Gagne committed
270
double PX4AdvancedFlightModesController::missionModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
271
{
Don Gagne's avatar
Don Gagne committed
272 273
    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
274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291

    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
292
double PX4AdvancedFlightModesController::loiterModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
293 294 295 296
{
    return _switchLiveRange("RC_MAP_LOITER_SW");
}

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

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

Don Gagne's avatar
Don Gagne committed
307
void PX4AdvancedFlightModesController::_recalcModeSelections(void)
Don Gagne's avatar
Don Gagne committed
308
{
Don Gagne's avatar
Don Gagne committed
309 310 311 312 313 314 315 316 317 318 319 320
    _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
321 322 323 324 325 326 327 328 329 330 331 332 333 334
    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
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 364
    
    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
365 366
}

Don Gagne's avatar
Don Gagne committed
367
void PX4AdvancedFlightModesController::_recalcModeRows(void)
Don Gagne's avatar
Don Gagne committed
368
{
Don Gagne's avatar
Don Gagne committed
369 370 371 372 373 374
    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
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 454
    
    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
455 456
}

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Don Gagne's avatar
Don Gagne committed
645 646
}

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

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

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

Don Gagne's avatar
Don Gagne committed
675 676
}

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

Don Gagne's avatar
Don Gagne committed
697 698
}

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

Don Gagne's avatar
Don Gagne committed
706 707
}

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

Don Gagne's avatar
Don Gagne committed
715 716
}

Don Gagne's avatar
Don Gagne committed
717
void PX4AdvancedFlightModesController::generateThresholds(void)
Don Gagne's avatar
Don Gagne committed
718 719 720 721 722 723 724
{
    // 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";
    
725
    foreach(const QString &thresholdParam, thresholdParams) {
Don Gagne's avatar
Don Gagne committed
726
        getParameterFact(-1, thresholdParam)->setRawValue(0.0f);
Don Gagne's avatar
Don Gagne committed
727 728 729 730
    }
    
    // Redistribute
    
Don Gagne's avatar
Don Gagne committed
731 732 733 734 735 736
    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
737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757
    
    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
758
            getParameterFact(-1, "RC_ACRO_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
759 760 761 762
            acroChannel = 0;
        }
        
        currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
763
        getParameterFact(-1, "RC_ASSIST_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
764 765
        if (posCtlOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
766
            getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
767 768 769 770
            posCtlChannel = 0;
        }
        
        currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
771
        getParameterFact(-1, "RC_AUTO_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
772 773
        if (loiterOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
774
            getParameterFact(-1, "RC_LOITER_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
775 776 777 778 779
            loiterChannel = 0;
        }
        
        if (returnOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
780
            getParameterFact(-1, "RC_RETURN_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
781 782 783 784 785
            returnChannel = 0;
        }
        
        if (offboardOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
786
            getParameterFact(-1, "RC_OFFB_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
787 788 789 790 791 792 793
            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
794
        getParameterFact(-1, "RC_ACRO_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
795 796 797
    }
    
    if (posCtlChannel != 0) {
Don Gagne's avatar
Don Gagne committed
798
        getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
799 800 801
    }
    
    if (loiterChannel != 0) {
Don Gagne's avatar
Don Gagne committed
802
        getParameterFact(-1, "RC_LOITER_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
803 804 805
    }
    
    if (returnChannel != 0) {
Don Gagne's avatar
Don Gagne committed
806
        getParameterFact(-1, "RC_RETURN_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
807 808 809
    }
    
    if (offboardChannel != 0) {
Don Gagne's avatar
Don Gagne committed
810
        getParameterFact(-1, "RC_OFFB_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
811 812 813
    }
    
    emit thresholdsChanged();
Don Gagne's avatar
Don Gagne committed
814
}