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

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14 15 16 17

#include "APMFlightModesComponentController.h"
#include "QGCMAVLink.h"

#include <QVariant>
#include <QQmlProperty>

APMFlightModesComponentController::APMFlightModesComponentController(void)
Don Gagne's avatar
Don Gagne committed
18 19
    : _activeFlightMode(0)
    , _channelCount(Vehicle::cMaxRcChannels)
Don Gagne's avatar
Don Gagne committed
20
{
Don Gagne's avatar
Don Gagne committed
21 22
    _modeParamPrefix = _vehicle->rover() ? QStringLiteral("MODE") : QStringLiteral("FLTMODE");
    _modeChannelParam = _vehicle->rover() ? QStringLiteral("MODE_CH") : QStringLiteral("FLTMODE_CH");
Don Gagne's avatar
Don Gagne committed
23

Don Gagne's avatar
Don Gagne committed
24
    QStringList usedParams;
Don Gagne's avatar
Don Gagne committed
25 26 27
    for (int i=1; i<7; i++) {
        usedParams << QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i);
    }
Don Gagne's avatar
Don Gagne committed
28 29 30 31
    if (!_allParametersExists(FactSystem::defaultComponentId, usedParams)) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
32
    _rgChannelOptionEnabled << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false);
Don Gagne's avatar
Don Gagne committed
33 34 35 36
    
    connect(_vehicle, &Vehicle::rcChannelsChanged, this, &APMFlightModesComponentController::_rcChannelsChanged);
}

Don Gagne's avatar
Don Gagne committed
37 38
/// Connected to Vehicle::rcChannelsChanged signal
void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
39
{
40 41
    int flightModeChannel = 4;

Don Gagne's avatar
Don Gagne committed
42 43
    if (parameterExists(FactSystem::defaultComponentId, _modeChannelParam)) {
        flightModeChannel = getParameterFact(FactSystem::defaultComponentId, _modeChannelParam)->rawValue().toInt() - 1;
44 45 46
    }

    if (flightModeChannel >= channelCount) {
47 48
        return;
    }
Don Gagne's avatar
Don Gagne committed
49

Don Gagne's avatar
Don Gagne committed
50
    _activeFlightMode = 0;
51
    int channelValue = pwmValues[flightModeChannel];
Don Gagne's avatar
Don Gagne committed
52
    if (channelValue != -1) {
Don Gagne's avatar
Don Gagne committed
53
        bool found = false;
Don Gagne's avatar
Don Gagne committed
54 55 56 57
        int rgThreshold[] = { 1230, 1360, 1490, 1620, 1749 };
        for (int i=0; i<5; i++) {
            if (channelValue <= rgThreshold[i]) {
                _activeFlightMode = i + 1;
Don Gagne's avatar
Don Gagne committed
58
                found = true;
Don Gagne's avatar
Don Gagne committed
59
                break;
Don Gagne's avatar
Don Gagne committed
60 61
            }
        }
Don Gagne's avatar
Don Gagne committed
62 63 64
        if (!found) {
            _activeFlightMode = 6;
        }
Don Gagne's avatar
Don Gagne committed
65
    }
Don Gagne's avatar
Don Gagne committed
66
    emit activeFlightModeChanged(_activeFlightMode);
Don Gagne's avatar
Don Gagne committed
67

Don Gagne's avatar
Don Gagne committed
68 69
    for (int i=0; i<6; i++) {
        _rgChannelOptionEnabled[i] = QVariant(false);
Don Gagne's avatar
Don Gagne committed
70
        channelValue = pwmValues[i+6];
71
        if (channelValue > 1800) {
Don Gagne's avatar
Don Gagne committed
72 73
            _rgChannelOptionEnabled[i] = QVariant(true);
        }
Don Gagne's avatar
Don Gagne committed
74
    }
Don Gagne's avatar
Don Gagne committed
75
    emit channelOptionEnabledChanged();
Don Gagne's avatar
Don Gagne committed
76
}