PX4AdvancedFlightModesController.cc 27 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9
 *
 * 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
{
34 35 36 37
    QStringList usedParams = QStringList({
        "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();
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
    
    // 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);
60

Don Gagne's avatar
Don Gagne committed
61 62
        _rgRCMin[channel] = getParameterFact(FactSystem::defaultComponentId, rcMinParam)->rawValue().toInt();
        _rgRCMax[channel] = getParameterFact(FactSystem::defaultComponentId, rcMaxParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
63
        
Don Gagne's avatar
Don Gagne committed
64
        float floatReversed = getParameterFact(-1, rcRevParam)->rawValue().toFloat();
Don Gagne's avatar
Don Gagne committed
65 66 67
        _rgRCReversed[channel] = floatReversed == -1.0f;
        
        _rcValues[channel] = 0.0;
Don Gagne's avatar
Don Gagne committed
68
    }
Don Gagne's avatar
Don Gagne committed
69 70 71

    // 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
72
        _channelCount = getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
73 74 75 76 77 78 79 80
    } 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
81 82 83
    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
84 85 86 87 88
    
    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
89
        // Assist mode is visible if AltCtl/PosCtl are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
90 91 92 93 94 95 96
        _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
97
        // Auto mode is visible if Mission/Loiter are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
98 99
        _autoModeVisible = loiterChannel != modeChannel;
    }
100

Don Gagne's avatar
Don Gagne committed
101
    // Setup the channel combobox model
102 103 104
    QVector<int> usedChannels;

    for (const QString &attitudeParam : {"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
105
        int channel = getParameterFact(-1, attitudeParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
106 107 108 109
        if (channel != 0) {
            usedChannels << channel;
        }
    }
110

Don Gagne's avatar
Don Gagne committed
111 112
    _channelListModel << "Disabled";
    _channelListModelChannel << 0;
Don Gagne's avatar
Don Gagne committed
113
    for (int channel=1; channel<=_channelCount; channel++) {
Don Gagne's avatar
Don Gagne committed
114 115 116 117 118 119
        if (!usedChannels.contains(channel)) {
            _channelListModel << QString("Channel %1").arg(channel);
            _channelListModelChannel << channel;
        }
    }
    
120 121 122 123 124 125 126 127 128 129 130
    // 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
131
    _recalcModeRows();
Don Gagne's avatar
Don Gagne committed
132 133 134 135
}

/// 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
136
void PX4AdvancedFlightModesController::_validateConfiguration(void)
Don Gagne's avatar
Don Gagne committed
137 138 139 140
{
    _validConfiguration = true;
    
    // Make sure switches are valid and within channel range
141 142

    const QStringList 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
143
    QList<int> switchMappings;
144 145

    for(int i=0, end = switchParams.count(); i < end; i++) {
Don Gagne's avatar
Don Gagne committed
146
        int map = getParameterFact(FactSystem::defaultComponentId, switchParams[i])->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
147 148 149 150
        switchMappings << map;
        
        if (map < 0 || map > _channelCount) {
            _validConfiguration = false;
151
            _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
152 153 154
        }
    }
    
Don Gagne's avatar
Don Gagne committed
155
    // Make sure mode switches are not double-mapped
Don Gagne's avatar
Don Gagne committed
156

157 158
    const QStringList attitudeParams = {"RC_MAP_THROTTLE", "RC_MAP_YAW", "RC_MAP_PITCH", "RC_MAP_ROLL", "RC_MAP_FLAPS", "RC_MAP_AUX1", "RC_MAP_AUX2"};
    for (int i=0, end = attitudeParams.count(); i < end; i++) {
Don Gagne's avatar
Don Gagne committed
159
        int map = getParameterFact(FactSystem::defaultComponentId, attitudeParams[i])->rawValue().toInt();
160 161 162
        if (map == 0) {
            continue;
        }
Don Gagne's avatar
Don Gagne committed
163 164

        for (int j=0; j<switchParams.count(); j++) {
165
            if (map == switchMappings[j]) {
Don Gagne's avatar
Don Gagne committed
166
                _validConfiguration = false;
167
                _configurationErrors += tr("%1 is set to same channel as %2.\n").arg(switchParams[j], attitudeParams[i]);
Don Gagne's avatar
Don Gagne committed
168 169 170
            }
        }
    }
171
    
Don Gagne's avatar
Don Gagne committed
172
    // Validate thresholds within range
173
    for(const QString &thresholdParam : {"RC_ASSIST_TH", "RC_AUTO_TH", "RC_ACRO_TH", "RC_POSCTL_TH", "RC_LOITER_TH", "RC_RETURN_TH", "RC_OFFB_TH"}) {
Don Gagne's avatar
Don Gagne committed
174
        float threshold = getParameterFact(-1, thresholdParam)->rawValue().toFloat();
Don Gagne's avatar
Don Gagne committed
175 176
        if (threshold < 0.0f || threshold > 1.0f) {
            _validConfiguration = false;
177
            _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
178 179 180 181
        }
    }
}

Don Gagne's avatar
Don Gagne committed
182
/// Connected to Vehicle::rcChannelsChanged signal
Don Gagne's avatar
Don Gagne committed
183
void PX4AdvancedFlightModesController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
184
{
Don Gagne's avatar
Don Gagne committed
185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202
    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
203 204
    }
    
Don Gagne's avatar
Don Gagne committed
205 206
    _recalcModeSelections();
    
Don Gagne's avatar
Don Gagne committed
207 208 209
    emit switchLiveRangeChanged();
}

Don Gagne's avatar
Don Gagne committed
210
double PX4AdvancedFlightModesController::_switchLiveRange(const QString& param)
Don Gagne's avatar
Don Gagne committed
211
{
Don Gagne's avatar
Don Gagne committed
212
    int channel = getParameterFact(-1, param)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
213
    if (channel == 0) {
Don Gagne's avatar
Don Gagne committed
214
        return 0.0;
Don Gagne's avatar
Don Gagne committed
215 216 217 218 219
    } else {
        return _rcValues[channel - 1];
    }
}

Don Gagne's avatar
Don Gagne committed
220
double PX4AdvancedFlightModesController::manualModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
221 222 223 224
{
    return _switchLiveRange("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
225
double PX4AdvancedFlightModesController::assistModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
226 227 228 229
{
    return manualModeRcValue();
}

Don Gagne's avatar
Don Gagne committed
230
double PX4AdvancedFlightModesController::autoModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
231 232 233 234
{
    return manualModeRcValue();
}

Don Gagne's avatar
Don Gagne committed
235
double PX4AdvancedFlightModesController::acroModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
236 237 238 239
{
    return _switchLiveRange("RC_MAP_ACRO_SW");
}

Don Gagne's avatar
Don Gagne committed
240
double PX4AdvancedFlightModesController::altCtlModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
241
{
Don Gagne's avatar
Don Gagne committed
242
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
243 244 245 246 247 248 249 250
    
    if (posCtlSwitchChannel == 0) {
        return _switchLiveRange("RC_MAP_MODE_SW");
    } else {
        return _switchLiveRange("RC_MAP_POSCTL_SW");
    }
}

Don Gagne's avatar
Don Gagne committed
251
double PX4AdvancedFlightModesController::posCtlModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
252 253 254 255
{
    return _switchLiveRange("RC_MAP_POSCTL_SW");
}

Don Gagne's avatar
Don Gagne committed
256
double PX4AdvancedFlightModesController::missionModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
257
{
Don Gagne's avatar
Don Gagne committed
258 259
    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
260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277

    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
278
double PX4AdvancedFlightModesController::loiterModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
279 280 281 282
{
    return _switchLiveRange("RC_MAP_LOITER_SW");
}

Don Gagne's avatar
Don Gagne committed
283
double PX4AdvancedFlightModesController::returnModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
284 285 286 287
{
    return _switchLiveRange("RC_MAP_RETURN_SW");
}

Don Gagne's avatar
Don Gagne committed
288
double PX4AdvancedFlightModesController::offboardModeRcValue(void)
289 290 291 292
{
    return _switchLiveRange("RC_MAP_OFFB_SW");
}

Don Gagne's avatar
Don Gagne committed
293
void PX4AdvancedFlightModesController::_recalcModeSelections(void)
Don Gagne's avatar
Don Gagne committed
294
{
Don Gagne's avatar
Don Gagne committed
295 296 297 298 299 300 301 302 303 304 305 306
    _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
307 308 309 310 311 312 313 314 315 316 317 318 319 320
    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
321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350
    
    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
351 352
}

Don Gagne's avatar
Don Gagne committed
353
void PX4AdvancedFlightModesController::_recalcModeRows(void)
Don Gagne's avatar
Don Gagne committed
354
{
Don Gagne's avatar
Don Gagne committed
355 356 357 358 359 360
    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
361 362 363 364 365 366 367 368 369 370 371 372 373 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
    
    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
441 442
}

Don Gagne's avatar
Don Gagne committed
443
double PX4AdvancedFlightModesController::manualModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
444
{
Don Gagne's avatar
Don Gagne committed
445 446 447
    return 0.0;
}

Don Gagne's avatar
Don Gagne committed
448
double PX4AdvancedFlightModesController::assistModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
449
{
Don Gagne's avatar
Don Gagne committed
450
    return getParameterFact(-1, "RC_ASSIST_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
451 452
}

Don Gagne's avatar
Don Gagne committed
453
double PX4AdvancedFlightModesController::autoModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
454
{
Don Gagne's avatar
Don Gagne committed
455
    return getParameterFact(-1, "RC_AUTO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
456 457
}

Don Gagne's avatar
Don Gagne committed
458
double PX4AdvancedFlightModesController::acroModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
459
{
Don Gagne's avatar
Don Gagne committed
460
    return getParameterFact(-1, "RC_ACRO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
461 462
}

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

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

Don Gagne's avatar
Don Gagne committed
473
double PX4AdvancedFlightModesController::missionModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
474
{
Don Gagne's avatar
Don Gagne committed
475
    return _autoModeVisible ? 0.0 : getParameterFact(-1, "RC_AUTO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
476 477 478
}


Don Gagne's avatar
Don Gagne committed
479
double PX4AdvancedFlightModesController::loiterModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
480
{
Don Gagne's avatar
Don Gagne committed
481
    return getParameterFact(-1, "RC_LOITER_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
482 483
}

Don Gagne's avatar
Don Gagne committed
484
double PX4AdvancedFlightModesController::returnModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
485
{
Don Gagne's avatar
Don Gagne committed
486
    return getParameterFact(-1, "RC_RETURN_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
487 488
}

Don Gagne's avatar
Don Gagne committed
489
double PX4AdvancedFlightModesController::offboardModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
490
{
Don Gagne's avatar
Don Gagne committed
491
    return getParameterFact(-1, "RC_OFFB_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
492 493
}

Don Gagne's avatar
Don Gagne committed
494
void PX4AdvancedFlightModesController::setAssistModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
495
{
Don Gagne's avatar
Don Gagne committed
496
    getParameterFact(-1, "RC_ASSIST_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
497 498 499
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
500
void PX4AdvancedFlightModesController::setAutoModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
501
{
Don Gagne's avatar
Don Gagne committed
502
    getParameterFact(-1, "RC_AUTO_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
503 504 505
    _recalcModeSelections();
}

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

Don Gagne's avatar
Don Gagne committed
512
void PX4AdvancedFlightModesController::setAltCtlModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
513 514 515 516
{
    setAssistModeThreshold(threshold);
}

Don Gagne's avatar
Don Gagne committed
517
void PX4AdvancedFlightModesController::setPosCtlModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
518
{
Don Gagne's avatar
Don Gagne committed
519
    getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
520 521 522
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
523
void PX4AdvancedFlightModesController::setMissionModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
524 525 526 527
{
    setAutoModeThreshold(threshold);
}

Don Gagne's avatar
Don Gagne committed
528
void PX4AdvancedFlightModesController::setLoiterModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
529
{
Don Gagne's avatar
Don Gagne committed
530
    getParameterFact(-1, "RC_LOITER_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
531 532 533
    _recalcModeSelections();
}

Don Gagne's avatar
Don Gagne committed
534
void PX4AdvancedFlightModesController::setReturnModeThreshold(double threshold)
Don Gagne's avatar
Don Gagne committed
535
{
Don Gagne's avatar
Don Gagne committed
536
    getParameterFact(-1, "RC_RETURN_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
537 538 539
    _recalcModeSelections();
}

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

Don Gagne's avatar
Don Gagne committed
546
int PX4AdvancedFlightModesController::_channelToChannelIndex(int channel)
Don Gagne's avatar
Don Gagne committed
547 548 549 550
{
    return _channelListModelChannel.lastIndexOf(channel);
}

Don Gagne's avatar
Don Gagne committed
551
int PX4AdvancedFlightModesController::_channelToChannelIndex(const QString& channelParam)
Don Gagne's avatar
Don Gagne committed
552
{
Don Gagne's avatar
Don Gagne committed
553
    return _channelToChannelIndex(getParameterFact(-1, channelParam)->rawValue().toInt());
Don Gagne's avatar
Don Gagne committed
554 555
}

Don Gagne's avatar
Don Gagne committed
556
int PX4AdvancedFlightModesController::manualModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
557 558 559 560
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
561
int PX4AdvancedFlightModesController::assistModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
562 563 564 565
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
566
int PX4AdvancedFlightModesController::autoModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
567 568 569 570
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
571
int PX4AdvancedFlightModesController::acroModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
572 573 574 575
{
    return _channelToChannelIndex("RC_MAP_ACRO_SW");
}

Don Gagne's avatar
Don Gagne committed
576
int PX4AdvancedFlightModesController::altCtlModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
577
{
Don Gagne's avatar
Don Gagne committed
578
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
579 580 581 582 583 584 585 586
    
    if (posCtlSwitchChannel == 0) {
        return _channelToChannelIndex("RC_MAP_MODE_SW");
    } else {
        return _channelToChannelIndex(posCtlSwitchChannel);
    }
}

Don Gagne's avatar
Don Gagne committed
587
int PX4AdvancedFlightModesController::posCtlModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
588 589 590 591
{
    return _channelToChannelIndex("RC_MAP_POSCTL_SW");
}

Don Gagne's avatar
Don Gagne committed
592
int PX4AdvancedFlightModesController::loiterModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
593 594 595 596
{
    return _channelToChannelIndex("RC_MAP_LOITER_SW");
}

Don Gagne's avatar
Don Gagne committed
597
int PX4AdvancedFlightModesController::missionModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
598
{
Don Gagne's avatar
Don Gagne committed
599
    int loiterSwitchChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
600 601 602 603 604 605 606 607
    
    if (loiterSwitchChannel == 0) {
        return _channelToChannelIndex("RC_MAP_MODE_SW");
    } else {
        return _channelToChannelIndex(loiterSwitchChannel);
    }
}

Don Gagne's avatar
Don Gagne committed
608
int PX4AdvancedFlightModesController::returnModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
609 610 611 612
{
    return _channelToChannelIndex("RC_MAP_RETURN_SW");
}

Don Gagne's avatar
Don Gagne committed
613
int PX4AdvancedFlightModesController::offboardModeChannelIndex(void)
Don Gagne's avatar
Don Gagne committed
614 615 616 617
{
    return _channelToChannelIndex("RC_MAP_OFFB_SW");
}

Don Gagne's avatar
Don Gagne committed
618
int PX4AdvancedFlightModesController::_channelIndexToChannel(int index)
Don Gagne's avatar
Don Gagne committed
619 620 621 622
{
    return _channelListModelChannel[index];
}

Don Gagne's avatar
Don Gagne committed
623
void PX4AdvancedFlightModesController::setManualModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
624
{
Don Gagne's avatar
Don Gagne committed
625
    getParameterFact(-1, "RC_MAP_MODE_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
626 627 628
    
    _recalcModeSelections();
    _recalcModeRows();
629 630
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
631 632
}

Don Gagne's avatar
Don Gagne committed
633
void PX4AdvancedFlightModesController::setAcroModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
634
{
Don Gagne's avatar
Don Gagne committed
635
    getParameterFact(-1, "RC_MAP_ACRO_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
636 637 638 639 640
    
    _recalcModeSelections();
    _recalcModeRows();
}

Don Gagne's avatar
Don Gagne committed
641
void PX4AdvancedFlightModesController::setPosCtlModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
642 643 644
{
    int channel = _channelIndexToChannel(index);
    
Don Gagne's avatar
Don Gagne committed
645
    getParameterFact(-1, "RC_MAP_POSCTL_SW")->setRawValue(channel);
Don Gagne's avatar
Don Gagne committed
646 647 648 649 650
    
    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
651
        // Assist mode is visible if AltCtl/PosCtl are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
652
        _assistModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
653 654 655 656 657 658
    }
    
    emit modesVisibleChanged();

    _recalcModeSelections();
    _recalcModeRows();
659 660
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
661 662
}

Don Gagne's avatar
Don Gagne committed
663
void PX4AdvancedFlightModesController::setLoiterModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
664 665 666
{
    int channel = _channelIndexToChannel(index);
    
Don Gagne's avatar
Don Gagne committed
667
    getParameterFact(-1, "RC_MAP_LOITER_SW")->setRawValue(channel);
Don Gagne's avatar
Don Gagne committed
668 669 670 671 672
    
    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
673
        // Auto mode is visible if Mission/Loiter are on separate channel from main Mode switch
Don Gagne's avatar
Don Gagne committed
674
        _autoModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
675 676 677 678 679 680
    }
    
    emit modesVisibleChanged();
    
    _recalcModeSelections();
    _recalcModeRows();
681 682
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
683 684
}

Don Gagne's avatar
Don Gagne committed
685
void PX4AdvancedFlightModesController::setReturnModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
686
{
Don Gagne's avatar
Don Gagne committed
687
    getParameterFact(-1, "RC_MAP_RETURN_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
688 689
    _recalcModeSelections();
    _recalcModeRows();
690 691
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
692 693
}

Don Gagne's avatar
Don Gagne committed
694
void PX4AdvancedFlightModesController::setOffboardModeChannelIndex(int index)
Don Gagne's avatar
Don Gagne committed
695
{
Don Gagne's avatar
Don Gagne committed
696
    getParameterFact(-1, "RC_MAP_OFFB_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
697 698
    _recalcModeSelections();
    _recalcModeRows();
699 700
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
701 702
}

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