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

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

#include "FlightModesComponentController.h"
#include "QGCMAVLink.h"
#include "AutoPilotPluginManager.h"

#include <QVariant>
#include <QQmlProperty>

Don Gagne's avatar
Don Gagne committed
34
FlightModesComponentController::FlightModesComponentController(void) :
Don Gagne's avatar
Don Gagne committed
35
    _validConfiguration(false),
Don Gagne's avatar
Don Gagne committed
36 37 38 39 40 41 42 43 44 45 46
    _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
47
{
Don Gagne's avatar
Don Gagne committed
48
    QStringList usedParams;
Don Gagne's avatar
Don Gagne committed
49 50
    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
51 52 53
    if (!_allParametersExists(FactSystem::defaultComponentId, usedParams)) {
        return;
    }
Don Gagne's avatar
Don Gagne committed
54 55

    _init();
Don Gagne's avatar
Don Gagne committed
56
    _validateConfiguration();
Don Gagne's avatar
Don Gagne committed
57
    
Don Gagne's avatar
Don Gagne committed
58
    connect(_vehicle, &Vehicle::rcChannelsChanged, this, &FlightModesComponentController::_rcChannelsChanged);
Don Gagne's avatar
Don Gagne committed
59 60
}

Don Gagne's avatar
Don Gagne committed
61
void FlightModesComponentController::_init(void)
Don Gagne's avatar
Don Gagne committed
62
{
Don Gagne's avatar
Don Gagne committed
63
    // FIXME: What about VTOL? That confuses the whole Flight Mode naming scheme
64
    _fixedWing = _vehicle->vehicleType() == MAV_TYPE_FIXED_WING;
Don Gagne's avatar
Don Gagne committed
65 66 67 68 69 70 71 72 73 74 75
    
    // 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
76 77
        _rgRCMin[channel] = getParameterFact(FactSystem::defaultComponentId, rcMinParam)->rawValue().toInt();
        _rgRCMax[channel] = getParameterFact(FactSystem::defaultComponentId, rcMaxParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
78
        
Don Gagne's avatar
Don Gagne committed
79
        float floatReversed = getParameterFact(-1, rcRevParam)->rawValue().toFloat();
Don Gagne's avatar
Don Gagne committed
80 81 82
        _rgRCReversed[channel] = floatReversed == -1.0f;
        
        _rcValues[channel] = 0.0;
Don Gagne's avatar
Don Gagne committed
83
    }
Don Gagne's avatar
Don Gagne committed
84 85 86

    // 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
87
        _channelCount = getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
88 89 90 91 92 93 94 95
    } 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
96 97 98
    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
99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
    
    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";
122
    foreach(const QString &attitudeParam, attitudeParams) {
Don Gagne's avatar
Don Gagne committed
123
        int channel = getParameterFact(-1, attitudeParam)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
124 125 126 127 128 129 130
        if (channel != 0) {
            usedChannels << channel;
        }
    }
    
    _channelListModel << "Disabled";
    _channelListModelChannel << 0;
Don Gagne's avatar
Don Gagne committed
131
    for (int channel=1; channel<=_channelCount; channel++) {
Don Gagne's avatar
Don Gagne committed
132 133 134 135 136 137
        if (!usedChannels.contains(channel)) {
            _channelListModel << QString("Channel %1").arg(channel);
            _channelListModelChannel << channel;
        }
    }
    
138 139 140 141 142 143 144 145 146 147 148
    // 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
149
    _recalcModeRows();
Don Gagne's avatar
Don Gagne committed
150 151 152 153 154 155 156 157 158 159
}

/// This will look for parameter settings which would cause the config to not run correctly.
/// It will set _validConfiguration and _configurationErrors as needed.
void FlightModesComponentController::_validateConfiguration(void)
{
    _validConfiguration = true;
    
    // Make sure switches are valid and within channel range
    
Don Gagne's avatar
Don Gagne committed
160
    QStringList switchParams;
Don Gagne's avatar
Don Gagne committed
161 162
    QList<int> switchMappings;
    
Don Gagne's avatar
Don Gagne committed
163
    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
164 165
    
    for(int i=0; i<switchParams.count(); i++) {
Don Gagne's avatar
Don Gagne committed
166
        int map = getParameterFact(FactSystem::defaultComponentId, switchParams[i])->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
167 168 169 170
        switchMappings << map;
        
        if (map < 0 || map > _channelCount) {
            _validConfiguration = false;
Don Gagne's avatar
Don Gagne committed
171
            _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
172 173 174
        }
    }
    
Don Gagne's avatar
Don Gagne committed
175
    // Make sure mode switches are not double-mapped
Don Gagne's avatar
Don Gagne committed
176
    
Don Gagne's avatar
Don Gagne committed
177
    QStringList attitudeParams;
Don Gagne's avatar
Don Gagne committed
178
    
179
    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
180 181

    for (int i=0; i<attitudeParams.count(); i++) {
Don Gagne's avatar
Don Gagne committed
182
        int map = getParameterFact(FactSystem::defaultComponentId, attitudeParams[i])->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
183 184 185 186

        for (int j=0; j<switchParams.count(); j++) {
            if (map != 0 && map == switchMappings[j]) {
                _validConfiguration = false;
Don Gagne's avatar
Don Gagne committed
187
                _configurationErrors += QString("%1 is set to same channel as %2.\n").arg(switchParams[j]).arg(attitudeParams[i]);
Don Gagne's avatar
Don Gagne committed
188 189 190
            }
        }
    }
191
    
Don Gagne's avatar
Don Gagne committed
192
    // Validate thresholds within range
193
    
Don Gagne's avatar
Don Gagne committed
194
    QStringList thresholdParams;
195
    
Don Gagne's avatar
Don Gagne committed
196
    thresholdParams << "RC_ASSIST_TH" << "RC_AUTO_TH" << "RC_ACRO_TH" << "RC_POSCTL_TH" << "RC_LOITER_TH" << "RC_RETURN_TH" << "RC_OFFB_TH";
197
    
198
    foreach(const QString &thresholdParam, thresholdParams) {
Don Gagne's avatar
Don Gagne committed
199
        float threshold = getParameterFact(-1, thresholdParam)->rawValue().toFloat();
Don Gagne's avatar
Don Gagne committed
200 201 202
        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
203 204 205 206
        }
    }
}

Don Gagne's avatar
Don Gagne committed
207 208
/// Connected to Vehicle::rcChannelsChanged signal
void FlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
209
{
Don Gagne's avatar
Don Gagne committed
210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227
    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
228 229
    }
    
Don Gagne's avatar
Don Gagne committed
230 231
    _recalcModeSelections();
    
Don Gagne's avatar
Don Gagne committed
232 233 234 235 236 237 238
    emit switchLiveRangeChanged();
}

double FlightModesComponentController::_switchLiveRange(const QString& param)
{
    QVariant value;
    
Don Gagne's avatar
Don Gagne committed
239
    int channel = getParameterFact(-1, param)->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
240
    if (channel == 0) {
Don Gagne's avatar
Don Gagne committed
241
        return 0.0;
Don Gagne's avatar
Don Gagne committed
242 243 244 245 246
    } else {
        return _rcValues[channel - 1];
    }
}

Don Gagne's avatar
Don Gagne committed
247
double FlightModesComponentController::manualModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
248 249 250 251
{
    return _switchLiveRange("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268
double FlightModesComponentController::assistModeRcValue(void)
{
    return manualModeRcValue();
}

double FlightModesComponentController::autoModeRcValue(void)
{
    return manualModeRcValue();
}

double FlightModesComponentController::acroModeRcValue(void)
{
    return _switchLiveRange("RC_MAP_ACRO_SW");
}

double FlightModesComponentController::altCtlModeRcValue(void)
{
Don Gagne's avatar
Don Gagne committed
269
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
270 271 272 273 274 275 276 277 278 279 280 281 282 283 284
    
    if (posCtlSwitchChannel == 0) {
        return _switchLiveRange("RC_MAP_MODE_SW");
    } else {
        return _switchLiveRange("RC_MAP_POSCTL_SW");
    }
}

double FlightModesComponentController::posCtlModeRcValue(void)
{
    return _switchLiveRange("RC_MAP_POSCTL_SW");
}

double FlightModesComponentController::missionModeRcValue(void)
{
Don Gagne's avatar
Don Gagne committed
285 286
    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
287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310

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

double FlightModesComponentController::loiterModeRcValue(void)
{
    return _switchLiveRange("RC_MAP_LOITER_SW");
}

double FlightModesComponentController::returnModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
311 312 313 314
{
    return _switchLiveRange("RC_MAP_RETURN_SW");
}

Don Gagne's avatar
Don Gagne committed
315
double FlightModesComponentController::offboardModeRcValue(void)
316 317 318 319
{
    return _switchLiveRange("RC_MAP_OFFB_SW");
}

Don Gagne's avatar
Don Gagne committed
320
void FlightModesComponentController::_recalcModeSelections(void)
Don Gagne's avatar
Don Gagne committed
321
{
Don Gagne's avatar
Don Gagne committed
322 323 324 325 326 327 328 329 330 331 332 333
    _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
334 335 336 337 338 339 340 341 342 343 344 345 346 347
    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
348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377
    
    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
378 379
}

Don Gagne's avatar
Don Gagne committed
380
void FlightModesComponentController::_recalcModeRows(void)
Don Gagne's avatar
Don Gagne committed
381
{
Don Gagne's avatar
Don Gagne committed
382 383 384 385 386 387
    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
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 455 456 457 458 459 460 461 462 463 464 465 466 467
    
    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
468 469
}

Don Gagne's avatar
Don Gagne committed
470
double FlightModesComponentController::manualModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
471
{
Don Gagne's avatar
Don Gagne committed
472 473 474 475 476
    return 0.0;
}

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

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

double FlightModesComponentController::acroModeThreshold(void)
{
Don Gagne's avatar
Don Gagne committed
487
    return getParameterFact(-1, "RC_ACRO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
488 489 490 491
}

double FlightModesComponentController::altCtlModeThreshold(void)
{
Don Gagne's avatar
Don Gagne committed
492
    return _assistModeVisible ? 0.0 : getParameterFact(-1, "RC_ASSIST_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
493 494 495 496
}

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

double FlightModesComponentController::missionModeThreshold(void)
{
Don Gagne's avatar
Don Gagne committed
502
    return _autoModeVisible ? 0.0 : getParameterFact(-1, "RC_AUTO_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
503 504 505 506 507
}


double FlightModesComponentController::loiterModeThreshold(void)
{
Don Gagne's avatar
Don Gagne committed
508
    return getParameterFact(-1, "RC_LOITER_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
509 510 511 512
}

double FlightModesComponentController::returnModeThreshold(void)
{
Don Gagne's avatar
Don Gagne committed
513
    return getParameterFact(-1, "RC_RETURN_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
514 515 516 517
}

double FlightModesComponentController::offboardModeThreshold(void)
{
Don Gagne's avatar
Don Gagne committed
518
    return getParameterFact(-1, "RC_OFFB_TH")->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
519 520 521 522
}

void FlightModesComponentController::setAssistModeThreshold(double threshold)
{
Don Gagne's avatar
Don Gagne committed
523
    getParameterFact(-1, "RC_ASSIST_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
524 525 526 527 528
    _recalcModeSelections();
}

void FlightModesComponentController::setAutoModeThreshold(double threshold)
{
Don Gagne's avatar
Don Gagne committed
529
    getParameterFact(-1, "RC_AUTO_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
530 531 532 533 534
    _recalcModeSelections();
}

void FlightModesComponentController::setAcroModeThreshold(double threshold)
{
Don Gagne's avatar
Don Gagne committed
535
    getParameterFact(-1, "RC_ACRO_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
536 537 538 539 540 541 542 543 544 545
    _recalcModeSelections();
}

void FlightModesComponentController::setAltCtlModeThreshold(double threshold)
{
    setAssistModeThreshold(threshold);
}

void FlightModesComponentController::setPosCtlModeThreshold(double threshold)
{
Don Gagne's avatar
Don Gagne committed
546
    getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
547 548 549 550 551 552 553 554 555 556
    _recalcModeSelections();
}

void FlightModesComponentController::setMissionModeThreshold(double threshold)
{
    setAutoModeThreshold(threshold);
}

void FlightModesComponentController::setLoiterModeThreshold(double threshold)
{
Don Gagne's avatar
Don Gagne committed
557
    getParameterFact(-1, "RC_LOITER_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
558 559 560 561 562
    _recalcModeSelections();
}

void FlightModesComponentController::setReturnModeThreshold(double threshold)
{
Don Gagne's avatar
Don Gagne committed
563
    getParameterFact(-1, "RC_RETURN_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
564 565 566 567 568
    _recalcModeSelections();
}

void FlightModesComponentController::setOffboardModeThreshold(double threshold)
{
Don Gagne's avatar
Don Gagne committed
569
    getParameterFact(-1, "RC_OFFB_TH")->setRawValue(threshold);
Don Gagne's avatar
Don Gagne committed
570 571 572 573 574 575 576 577 578 579
    _recalcModeSelections();
}

int FlightModesComponentController::_channelToChannelIndex(int channel)
{
    return _channelListModelChannel.lastIndexOf(channel);
}

int FlightModesComponentController::_channelToChannelIndex(const QString& channelParam)
{
Don Gagne's avatar
Don Gagne committed
580
    return _channelToChannelIndex(getParameterFact(-1, channelParam)->rawValue().toInt());
Don Gagne's avatar
Don Gagne committed
581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604
}

int FlightModesComponentController::manualModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

int FlightModesComponentController::assistModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

int FlightModesComponentController::autoModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_MODE_SW");
}

int FlightModesComponentController::acroModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_ACRO_SW");
}

int FlightModesComponentController::altCtlModeChannelIndex(void)
{
Don Gagne's avatar
Don Gagne committed
605
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625
    
    if (posCtlSwitchChannel == 0) {
        return _channelToChannelIndex("RC_MAP_MODE_SW");
    } else {
        return _channelToChannelIndex(posCtlSwitchChannel);
    }
}

int FlightModesComponentController::posCtlModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_POSCTL_SW");
}

int FlightModesComponentController::loiterModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_LOITER_SW");
}

int FlightModesComponentController::missionModeChannelIndex(void)
{
Don Gagne's avatar
Don Gagne committed
626
    int loiterSwitchChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651
    
    if (loiterSwitchChannel == 0) {
        return _channelToChannelIndex("RC_MAP_MODE_SW");
    } else {
        return _channelToChannelIndex(loiterSwitchChannel);
    }
}

int FlightModesComponentController::returnModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_RETURN_SW");
}

int FlightModesComponentController::offboardModeChannelIndex(void)
{
    return _channelToChannelIndex("RC_MAP_OFFB_SW");
}

int FlightModesComponentController::_channelIndexToChannel(int index)
{
    return _channelListModelChannel[index];
}

void FlightModesComponentController::setManualModeChannelIndex(int index)
{
Don Gagne's avatar
Don Gagne committed
652
    getParameterFact(-1, "RC_MAP_MODE_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
653 654 655
    
    _recalcModeSelections();
    _recalcModeRows();
656 657
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
658 659 660 661
}

void FlightModesComponentController::setAcroModeChannelIndex(int index)
{
Don Gagne's avatar
Don Gagne committed
662
    getParameterFact(-1, "RC_MAP_ACRO_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
663 664 665 666 667 668 669 670 671
    
    _recalcModeSelections();
    _recalcModeRows();
}

void FlightModesComponentController::setPosCtlModeChannelIndex(int index)
{
    int channel = _channelIndexToChannel(index);
    
Don Gagne's avatar
Don Gagne committed
672
    getParameterFact(-1, "RC_MAP_POSCTL_SW")->setRawValue(channel);
Don Gagne's avatar
Don Gagne committed
673 674 675 676 677 678
    
    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
679
        _assistModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
680 681 682 683 684 685
    }
    
    emit modesVisibleChanged();

    _recalcModeSelections();
    _recalcModeRows();
686 687
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
688 689 690 691 692 693
}

void FlightModesComponentController::setLoiterModeChannelIndex(int index)
{
    int channel = _channelIndexToChannel(index);
    
Don Gagne's avatar
Don Gagne committed
694
    getParameterFact(-1, "RC_MAP_LOITER_SW")->setRawValue(channel);
Don Gagne's avatar
Don Gagne committed
695 696 697 698 699 700
    
    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
701
        _autoModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->rawValue().toInt();
Don Gagne's avatar
Don Gagne committed
702 703 704 705 706 707
    }
    
    emit modesVisibleChanged();
    
    _recalcModeSelections();
    _recalcModeRows();
708 709
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
710 711 712 713
}

void FlightModesComponentController::setReturnModeChannelIndex(int index)
{
Don Gagne's avatar
Don Gagne committed
714
    getParameterFact(-1, "RC_MAP_RETURN_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
715 716
    _recalcModeSelections();
    _recalcModeRows();
717 718
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
719 720 721 722
}

void FlightModesComponentController::setOffboardModeChannelIndex(int index)
{
Don Gagne's avatar
Don Gagne committed
723
    getParameterFact(-1, "RC_MAP_OFFB_SW")->setRawValue(_channelIndexToChannel(index));
Don Gagne's avatar
Don Gagne committed
724 725
    _recalcModeSelections();
    _recalcModeRows();
726 727
    emit channelIndicesChanged();

Don Gagne's avatar
Don Gagne committed
728 729 730 731 732 733 734 735 736 737
}

void FlightModesComponentController::generateThresholds(void)
{
    // 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";
    
738
    foreach(const QString &thresholdParam, thresholdParams) {
Don Gagne's avatar
Don Gagne committed
739
        getParameterFact(-1, thresholdParam)->setRawValue(0.0f);
Don Gagne's avatar
Don Gagne committed
740 741 742 743
    }
    
    // Redistribute
    
Don Gagne's avatar
Don Gagne committed
744 745 746 747 748 749
    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
750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770
    
    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
771
            getParameterFact(-1, "RC_ACRO_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
772 773 774 775
            acroChannel = 0;
        }
        
        currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
776
        getParameterFact(-1, "RC_ASSIST_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
777 778
        if (posCtlOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
779
            getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
780 781 782 783
            posCtlChannel = 0;
        }
        
        currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
784
        getParameterFact(-1, "RC_AUTO_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
785 786
        if (loiterOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
787
            getParameterFact(-1, "RC_LOITER_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
788 789 790 791 792
            loiterChannel = 0;
        }
        
        if (returnOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
793
            getParameterFact(-1, "RC_RETURN_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
794 795 796 797 798
            returnChannel = 0;
        }
        
        if (offboardOnModeSwitch) {
            currentThreshold += increment;
Don Gagne's avatar
Don Gagne committed
799
            getParameterFact(-1, "RC_OFFB_TH")->setRawValue(currentThreshold);
Don Gagne's avatar
Don Gagne committed
800 801 802 803 804 805 806
            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
807
        getParameterFact(-1, "RC_ACRO_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
808 809 810
    }
    
    if (posCtlChannel != 0) {
Don Gagne's avatar
Don Gagne committed
811
        getParameterFact(-1, "RC_POSCTL_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
812 813 814
    }
    
    if (loiterChannel != 0) {
Don Gagne's avatar
Don Gagne committed
815
        getParameterFact(-1, "RC_LOITER_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
816 817 818
    }
    
    if (returnChannel != 0) {
Don Gagne's avatar
Don Gagne committed
819
        getParameterFact(-1, "RC_RETURN_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
820 821 822
    }
    
    if (offboardChannel != 0) {
Don Gagne's avatar
Don Gagne committed
823
        getParameterFact(-1, "RC_OFFB_TH")->setRawValue(0.25f);
Don Gagne's avatar
Don Gagne committed
824 825 826
    }
    
    emit thresholdsChanged();
Don Gagne's avatar
Don Gagne committed
827
}