FlightModesComponentController.cc 27.6 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 34
/*=====================================================================
 
 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 "UASManager.h"
#include "AutoPilotPluginManager.h"

#include <QVariant>
#include <QQmlProperty>

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

    _init();
Don Gagne's avatar
Don Gagne committed
57
    _validateConfiguration();
Don Gagne's avatar
Don Gagne committed
58 59
    
    connect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
Don Gagne's avatar
Don Gagne committed
60 61 62 63
}

FlightModesComponentController::~FlightModesComponentController()
{
Don Gagne's avatar
Don Gagne committed
64
    disconnect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
Don Gagne's avatar
Don Gagne committed
65 66
}

Don Gagne's avatar
Don Gagne committed
67
void FlightModesComponentController::_init(void)
Don Gagne's avatar
Don Gagne committed
68
{
Don Gagne's avatar
Don Gagne committed
69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
    // FIXME: What about VTOL? That confuses the whole Flight Mode naming scheme
    _fixedWing = _uas->getSystemType() == MAV_TYPE_FIXED_WING;
    
    // 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;
        
        _rgRCMin[channel] = getParameterFact(FactSystem::defaultComponentId, rcMinParam)->value().toInt();
        _rgRCMax[channel] = getParameterFact(FactSystem::defaultComponentId, rcMaxParam)->value().toInt();
        
        float floatReversed = getParameterFact(-1, rcRevParam)->value().toFloat();
        _rgRCReversed[channel] = floatReversed == -1.0f;
        
        _rcValues[channel] = 0.0;
Don Gagne's avatar
Don Gagne committed
89
    }
Don Gagne's avatar
Don Gagne committed
90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136

    // RC_CHAN_CNT parameter is set by Radio Cal to specify the number of radio channels.
    if (parameterExists(FactSystem::defaultComponentId, "RC_CHAN_CNT")) {
        _channelCount = getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->value().toInt();
    } else {
        _channelCount =_chanMax;
    }
    if (_channelCount <= 0 || _channelCount > _chanMax) {
        // Parameter exists, but has not yet been set or is invalid. Use default
        _channelCount = _chanMax;
    }
    
    int modeChannel = getParameterFact(-1, "RC_MAP_MODE_SW")->value().toInt();
    int posCtlChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->value().toInt();
    int loiterChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->value().toInt();
    
    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";
    foreach(QString attitudeParam, attitudeParams) {
        int channel = getParameterFact(-1, attitudeParam)->value().toInt();
        if (channel != 0) {
            usedChannels << channel;
        }
    }
    
    _channelListModel << "Disabled";
    _channelListModelChannel << 0;
Don Gagne's avatar
Don Gagne committed
137
    for (int channel=1; channel<=_channelCount; channel++) {
Don Gagne's avatar
Don Gagne committed
138 139 140 141 142 143
        if (!usedChannels.contains(channel)) {
            _channelListModel << QString("Channel %1").arg(channel);
            _channelListModelChannel << channel;
        }
    }
    
144 145 146 147 148 149 150 151 152 153 154
    // 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
155
    _recalcModeRows();
Don Gagne's avatar
Don Gagne committed
156 157 158 159 160 161 162 163 164 165
}

/// 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
166
    QStringList switchParams;
Don Gagne's avatar
Don Gagne committed
167 168
    QList<int> switchMappings;
    
Don Gagne's avatar
Don Gagne committed
169
    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
170 171
    
    for(int i=0; i<switchParams.count(); i++) {
Don Gagne's avatar
Don Gagne committed
172
        int map = getParameterFact(FactSystem::defaultComponentId, switchParams[i])->value().toInt();
Don Gagne's avatar
Don Gagne committed
173 174 175 176
        switchMappings << map;
        
        if (map < 0 || map > _channelCount) {
            _validConfiguration = false;
Don Gagne's avatar
Don Gagne committed
177
            _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
178 179 180
        }
    }
    
Don Gagne's avatar
Don Gagne committed
181
    // Make sure mode switches are not double-mapped
Don Gagne's avatar
Don Gagne committed
182
    
Don Gagne's avatar
Don Gagne committed
183
    QStringList attitudeParams;
Don Gagne's avatar
Don Gagne committed
184
    
185
    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
186 187

    for (int i=0; i<attitudeParams.count(); i++) {
Don Gagne's avatar
Don Gagne committed
188
        int map = getParameterFact(FactSystem::defaultComponentId, attitudeParams[i])->value().toInt();
Don Gagne's avatar
Don Gagne committed
189 190 191 192

        for (int j=0; j<switchParams.count(); j++) {
            if (map != 0 && map == switchMappings[j]) {
                _validConfiguration = false;
Don Gagne's avatar
Don Gagne committed
193
                _configurationErrors += QString("%1 is set to same channel as %2.\n").arg(switchParams[j]).arg(attitudeParams[i]);
Don Gagne's avatar
Don Gagne committed
194 195 196
            }
        }
    }
197
    
Don Gagne's avatar
Don Gagne committed
198
    // Validate thresholds within range
199
    
Don Gagne's avatar
Don Gagne committed
200
    QStringList thresholdParams;
201
    
Don Gagne's avatar
Don Gagne committed
202
    thresholdParams << "RC_ASSIST_TH" << "RC_AUTO_TH" << "RC_ACRO_TH" << "RC_POSCTL_TH" << "RC_LOITER_TH" << "RC_RETURN_TH" << "RC_OFFB_TH";
203
    
Don Gagne's avatar
Don Gagne committed
204 205 206 207 208
    foreach(QString thresholdParam, thresholdParams) {
        float threshold = getParameterFact(-1, thresholdParam)->value().toFloat();
        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
209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232
        }
    }
}

/// @brief This routine is called whenever a raw value for an RC channel changes.
///     @param chan RC channel on which signal is coming from (0-based)
///     @param fval Current value for channel
void FlightModesComponentController::_remoteControlChannelRawChanged(int chan, float fval)
{
    Q_ASSERT(chan >= 0 && chan <= _chanMax);
    
    if (fval < _rgRCMin[chan]) {
        fval= _rgRCMin[chan];
    }
    if (fval > _rgRCMax[chan]) {
        fval= _rgRCMax[chan];
    }
    
    float percentRange = (fval - _rgRCMin[chan]) / (float)(_rgRCMax[chan] - _rgRCMin[chan]);
    if (_rgRCReversed[chan]) {
        percentRange = 1.0 - percentRange;
    }
    
    _rcValues[chan] = percentRange;
Don Gagne's avatar
Don Gagne committed
233 234 235
    
    _recalcModeSelections();
    
Don Gagne's avatar
Don Gagne committed
236 237 238 239 240 241 242
    emit switchLiveRangeChanged();
}

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

Don Gagne's avatar
Don Gagne committed
251
double FlightModesComponentController::manualModeRcValue(void)
Don Gagne's avatar
Don Gagne committed
252 253 254 255
{
    return _switchLiveRange("RC_MAP_MODE_SW");
}

Don Gagne's avatar
Don Gagne committed
256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314
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)
{
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->value().toInt();
    
    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)
{
    int returnSwitchChannel = getParameterFact(-1, "RC_MAP_RETURN_SW")->value().toInt();
    int loiterSwitchChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->value().toInt();

    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
315 316 317 318
{
    return _switchLiveRange("RC_MAP_RETURN_SW");
}

Don Gagne's avatar
Don Gagne committed
319
double FlightModesComponentController::offboardModeRcValue(void)
320 321 322 323
{
    return _switchLiveRange("RC_MAP_OFFB_SW");
}

Don Gagne's avatar
Don Gagne committed
324
void FlightModesComponentController::_recalcModeSelections(void)
Don Gagne's avatar
Don Gagne committed
325
{
Don Gagne's avatar
Don Gagne committed
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 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 378 379 380 381
    _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
    int modeSwitchChannel =     getParameterFact(-1, "RC_MAP_MODE_SW")->value().toInt() - 1;
    int acroSwitchChannel =     getParameterFact(-1, "RC_MAP_ACRO_SW")->value().toInt() - 1;
    int posCtlSwitchChannel =   getParameterFact(-1, "RC_MAP_POSCTL_SW")->value().toInt() - 1;
    int loiterSwitchChannel =   getParameterFact(-1, "RC_MAP_LOITER_SW")->value().toInt() - 1;
    int returnSwitchChannel =   getParameterFact(-1, "RC_MAP_RETURN_SW")->value().toInt() - 1;
    int offboardSwitchChannel = getParameterFact(-1, "RC_MAP_OFFB_SW")->value().toInt() - 1;
    
    double autoThreshold =      getParameterFact(-1, "RC_AUTO_TH")->value().toDouble();
    double assistThreshold =    getParameterFact(-1, "RC_ASSIST_TH")->value().toDouble();
    double acroThreshold =      getParameterFact(-1, "RC_ACRO_TH")->value().toDouble();
    double posCtlThreshold =    getParameterFact(-1, "RC_POSCTL_TH")->value().toDouble();
    double loiterThreshold =    getParameterFact(-1, "RC_LOITER_TH")->value().toDouble();
    double returnThreshold =    getParameterFact(-1, "RC_RETURN_TH")->value().toDouble();
    double offboardThreshold =  getParameterFact(-1, "RC_OFFB_TH")->value().toDouble();
    
    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
382 383
}

Don Gagne's avatar
Don Gagne committed
384
void FlightModesComponentController::_recalcModeRows(void)
Don Gagne's avatar
Don Gagne committed
385
{
Don Gagne's avatar
Don Gagne committed
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 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471
    int modeSwitchChannel =     getParameterFact(-1, "RC_MAP_MODE_SW")->value().toInt();
    int acroSwitchChannel =     getParameterFact(-1, "RC_MAP_ACRO_SW")->value().toInt();
    int posCtlSwitchChannel =   getParameterFact(-1, "RC_MAP_POSCTL_SW")->value().toInt();
    int loiterSwitchChannel =   getParameterFact(-1, "RC_MAP_LOITER_SW")->value().toInt();
    int returnSwitchChannel =   getParameterFact(-1, "RC_MAP_RETURN_SW")->value().toInt();
    int offboardSwitchChannel = getParameterFact(-1, "RC_MAP_OFFB_SW")->value().toInt();
    
    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
472 473
}

Don Gagne's avatar
Don Gagne committed
474
double FlightModesComponentController::manualModeThreshold(void)
Don Gagne's avatar
Don Gagne committed
475
{
Don Gagne's avatar
Don Gagne committed
476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 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 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822
    return 0.0;
}

double FlightModesComponentController::assistModeThreshold(void)
{
    return getParameterFact(-1, "RC_ASSIST_TH")->value().toDouble();
}

double FlightModesComponentController::autoModeThreshold(void)
{
    return getParameterFact(-1, "RC_AUTO_TH")->value().toDouble();
}

double FlightModesComponentController::acroModeThreshold(void)
{
    return getParameterFact(-1, "RC_ACRO_TH")->value().toDouble();
}

double FlightModesComponentController::altCtlModeThreshold(void)
{
    return _assistModeVisible ? 0.0 : getParameterFact(-1, "RC_ASSIST_TH")->value().toDouble();
}

double FlightModesComponentController::posCtlModeThreshold(void)
{
    return getParameterFact(-1, "RC_POSCTL_TH")->value().toDouble();
}

double FlightModesComponentController::missionModeThreshold(void)
{
    return _autoModeVisible ? 0.0 : getParameterFact(-1, "RC_AUTO_TH")->value().toDouble();
}


double FlightModesComponentController::loiterModeThreshold(void)
{
    return getParameterFact(-1, "RC_LOITER_TH")->value().toDouble();
}

double FlightModesComponentController::returnModeThreshold(void)
{
    return getParameterFact(-1, "RC_RETURN_TH")->value().toDouble();
}

double FlightModesComponentController::offboardModeThreshold(void)
{
    return getParameterFact(-1, "RC_OFFB_TH")->value().toDouble();
}

void FlightModesComponentController::setAssistModeThreshold(double threshold)
{
    getParameterFact(-1, "RC_ASSIST_TH")->setValue(threshold);
    _recalcModeSelections();
}

void FlightModesComponentController::setAutoModeThreshold(double threshold)
{
    getParameterFact(-1, "RC_AUTO_TH")->setValue(threshold);
    _recalcModeSelections();
}

void FlightModesComponentController::setAcroModeThreshold(double threshold)
{
    getParameterFact(-1, "RC_ACRO_TH")->setValue(threshold);
    _recalcModeSelections();
}

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

void FlightModesComponentController::setPosCtlModeThreshold(double threshold)
{
    getParameterFact(-1, "RC_POSCTL_TH")->setValue(threshold);
    _recalcModeSelections();
}

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

void FlightModesComponentController::setLoiterModeThreshold(double threshold)
{
    getParameterFact(-1, "RC_LOITER_TH")->setValue(threshold);
    _recalcModeSelections();
}

void FlightModesComponentController::setReturnModeThreshold(double threshold)
{
    getParameterFact(-1, "RC_RETURN_TH")->setValue(threshold);
    _recalcModeSelections();
}

void FlightModesComponentController::setOffboardModeThreshold(double threshold)
{
    getParameterFact(-1, "RC_OFFB_TH")->setValue(threshold);
    _recalcModeSelections();
}

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

int FlightModesComponentController::_channelToChannelIndex(const QString& channelParam)
{
    return _channelToChannelIndex(getParameterFact(-1, channelParam)->value().toInt());
}

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)
{
    int posCtlSwitchChannel = getParameterFact(-1, "RC_MAP_POSCTL_SW")->value().toInt();
    
    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)
{
    int loiterSwitchChannel = getParameterFact(-1, "RC_MAP_LOITER_SW")->value().toInt();
    
    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)
{
    getParameterFact(-1, "RC_MAP_MODE_SW")->setValue(_channelIndexToChannel(index));
    
    _recalcModeSelections();
    _recalcModeRows();
}

void FlightModesComponentController::setAcroModeChannelIndex(int index)
{
    getParameterFact(-1, "RC_MAP_ACRO_SW")->setValue(_channelIndexToChannel(index));
    
    _recalcModeSelections();
    _recalcModeRows();
}

void FlightModesComponentController::setPosCtlModeChannelIndex(int index)
{
    int channel = _channelIndexToChannel(index);
    
    getParameterFact(-1, "RC_MAP_POSCTL_SW")->setValue(channel);
    
    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
        _assistModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->value().toInt();
    }
    
    emit altCtlModeChannelIndexChanged(index);
    emit modesVisibleChanged();

    _recalcModeSelections();
    _recalcModeRows();
}

void FlightModesComponentController::setLoiterModeChannelIndex(int index)
{
    int channel = _channelIndexToChannel(index);
    
    getParameterFact(-1, "RC_MAP_LOITER_SW")->setValue(channel);
    
    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
        _autoModeVisible = channel != getParameterFact(-1, "RC_MAP_MODE_SW")->value().toInt();
    }
    
    emit missionModeChannelIndexChanged(index);
    emit modesVisibleChanged();
    
    _recalcModeSelections();
    _recalcModeRows();
}

void FlightModesComponentController::setReturnModeChannelIndex(int index)
{
    getParameterFact(-1, "RC_MAP_RETURN_SW")->setValue(_channelIndexToChannel(index));
    _recalcModeSelections();
    _recalcModeRows();
}

void FlightModesComponentController::setOffboardModeChannelIndex(int index)
{
    getParameterFact(-1, "RC_MAP_OFFB_SW")->setValue(_channelIndexToChannel(index));
    _recalcModeSelections();
    _recalcModeRows();
}

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";
    
    foreach(QString thresholdParam, thresholdParams) {
        getParameterFact(-1, thresholdParam)->setValue(0.0f);
    }
    
    // Redistribute
    
    int modeChannel =       getParameterFact(-1, "RC_MAP_MODE_SW")->value().toInt();
    int acroChannel =       getParameterFact(-1, "RC_MAP_ACRO_SW")->value().toInt();
    int posCtlChannel =     getParameterFact(-1, "RC_MAP_POSCTL_SW")->value().toInt();
    int loiterChannel =     getParameterFact(-1, "RC_MAP_LOITER_SW")->value().toInt();
    int returnChannel =     getParameterFact(-1, "RC_MAP_RETURN_SW")->value().toInt();
    int offboardChannel =   getParameterFact(-1, "RC_MAP_OFFB_SW")->value().toInt();
    
    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;
            getParameterFact(-1, "RC_ACRO_TH")->setValue(currentThreshold);
            acroChannel = 0;
        }
        
        currentThreshold += increment;
        getParameterFact(-1, "RC_ASSIST_TH")->setValue(currentThreshold);
        if (posCtlOnModeSwitch) {
            currentThreshold += increment;
            getParameterFact(-1, "RC_POSCTL_TH")->setValue(currentThreshold);
            posCtlChannel = 0;
        }
        
        currentThreshold += increment;
        getParameterFact(-1, "RC_AUTO_TH")->setValue(currentThreshold);
        if (loiterOnModeSwitch) {
            currentThreshold += increment;
            getParameterFact(-1, "RC_LOITER_TH")->setValue(currentThreshold);
            loiterChannel = 0;
        }
        
        if (returnOnModeSwitch) {
            currentThreshold += increment;
            getParameterFact(-1, "RC_RETURN_TH")->setValue(currentThreshold);
            returnChannel = 0;
        }
        
        if (offboardOnModeSwitch) {
            currentThreshold += increment;
            getParameterFact(-1, "RC_OFFB_TH")->setValue(currentThreshold);
            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
        getParameterFact(-1, "RC_ACRO_TH")->setValue(0.25f);
    }
    
    if (posCtlChannel != 0) {
        getParameterFact(-1, "RC_POSCTL_TH")->setValue(0.25f);
    }
    
    if (loiterChannel != 0) {
        getParameterFact(-1, "RC_LOITER_TH")->setValue(0.25f);
    }
    
    if (returnChannel != 0) {
        getParameterFact(-1, "RC_RETURN_TH")->setValue(0.25f);
    }
    
    if (offboardChannel != 0) {
        getParameterFact(-1, "RC_OFFB_TH")->setValue(0.25f);
    }
    
    emit thresholdsChanged();
Don Gagne's avatar
Don Gagne committed
823
}