APMFlightModesComponentController.cc 6.5 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

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

#include <QVariant>
#include <QQmlProperty>

17 18
bool APMFlightModesComponentController::_typeRegistered = false;

19 20 21
const char* APMFlightModesComponentController::_simpleParamName =       "SIMPLE";
const char* APMFlightModesComponentController::_superSimpleParamName =  "SUPER_SIMPLE";

Don Gagne's avatar
Don Gagne committed
22
APMFlightModesComponentController::APMFlightModesComponentController(void)
23
    : _activeFlightMode     (0)
24 25
    , _channelCount         (Vehicle::cMaxRcChannels)
    , _simpleMode           (SimpleModeStandard)
26 27
    , _simpleModeFact       (parameterExists(-1, _simpleParamName)      ? getParameterFact(-1, _simpleParamName) : nullptr)
    , _superSimpleModeFact  (parameterExists(-1, _superSimpleParamName) ? getParameterFact(-1, _superSimpleParamName) : nullptr)
28
    , _simpleModesSupported (_simpleModeFact && _superSimpleModeFact)
Don Gagne's avatar
Don Gagne committed
29
{
30 31 32 33
    if (!_typeRegistered) {
        qmlRegisterUncreatableType<APMFlightModesComponentController>("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController", "Reference only");
    }

34 35 36
    bool arduRoverFirmware = parameterExists(-1, QStringLiteral("MODE1"));
    _modeParamPrefix = arduRoverFirmware ? QStringLiteral("MODE") : QStringLiteral("FLTMODE");
    _modeChannelParam = arduRoverFirmware ? QStringLiteral("MODE_CH") : QStringLiteral("FLTMODE_CH");
Don Gagne's avatar
Don Gagne committed
37

38 39 40 41 42 43
    _simpleModeNames << tr("Off") << tr("Simple") << tr("Super-Simple") << tr("Custom");
    for (int i=0; i<_cFltModes; i++) {
        _simpleModeEnabled.append(QVariant(false));
        _superSimpleModeEnabled.append(QVariant(false));
    }

44
    if (_simpleModesSupported) {
45
        _setupSimpleModeEnabled();
46 47 48 49 50 51 52 53 54 55 56 57 58

        uint8_t simpleModeValue = static_cast<uint8_t>(_simpleModeFact->rawValue().toUInt());
        uint8_t superSimpleModeValue = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toUInt());
        if (simpleModeValue == 0 && superSimpleModeValue == 0) {
            _simpleMode = SimpleModeStandard;
        } else if (simpleModeValue == _allSimpleBits && superSimpleModeValue == 0) {
            _simpleMode = SimpleModeSimple;
        } else if (simpleModeValue == 0 && superSimpleModeValue == _allSimpleBits) {
            _simpleMode = SimpleModeSuperSimple;
        } else {
            _simpleMode = SimpleModeCustom;
        }

59
        connect(this, &APMFlightModesComponentController::simpleModeChanged, this, &APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode);
60 61
    }

Don Gagne's avatar
Don Gagne committed
62
    QStringList usedParams;
Don Gagne's avatar
Don Gagne committed
63 64 65
    for (int i=1; i<7; i++) {
        usedParams << QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i);
    }
Don Gagne's avatar
Don Gagne committed
66 67 68 69
    if (!_allParametersExists(FactSystem::defaultComponentId, usedParams)) {
        return;
    }

70 71 72 73
    for (int i=0; i<_cChannelOptions; i++) {
        _rgChannelOptionEnabled.append(QVariant(false));
    }

Don Gagne's avatar
Don Gagne committed
74 75 76
    connect(_vehicle, &Vehicle::rcChannelsChanged, this, &APMFlightModesComponentController::_rcChannelsChanged);
}

Don Gagne's avatar
Don Gagne committed
77 78
/// Connected to Vehicle::rcChannelsChanged signal
void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
Don Gagne's avatar
Don Gagne committed
79
{
80 81
    int flightModeChannel = 4;

Don Gagne's avatar
Don Gagne committed
82 83
    if (parameterExists(FactSystem::defaultComponentId, _modeChannelParam)) {
        flightModeChannel = getParameterFact(FactSystem::defaultComponentId, _modeChannelParam)->rawValue().toInt() - 1;
84 85 86
    }

    if (flightModeChannel >= channelCount) {
87 88
        return;
    }
Don Gagne's avatar
Don Gagne committed
89

Don Gagne's avatar
Don Gagne committed
90
    _activeFlightMode = 0;
91
    int channelValue = pwmValues[flightModeChannel];
Don Gagne's avatar
Don Gagne committed
92
    if (channelValue != -1) {
Don Gagne's avatar
Don Gagne committed
93
        bool found = false;
Don Gagne's avatar
Don Gagne committed
94 95 96 97
        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
98
                found = true;
Don Gagne's avatar
Don Gagne committed
99
                break;
Don Gagne's avatar
Don Gagne committed
100 101
            }
        }
Don Gagne's avatar
Don Gagne committed
102 103 104
        if (!found) {
            _activeFlightMode = 6;
        }
Don Gagne's avatar
Don Gagne committed
105
    }
Don Gagne's avatar
Don Gagne committed
106
    emit activeFlightModeChanged(_activeFlightMode);
Don Gagne's avatar
Don Gagne committed
107

108
    for (int i=0; i<_cChannelOptions; i++) {
Don Gagne's avatar
Don Gagne committed
109
        _rgChannelOptionEnabled[i] = QVariant(false);
Don Gagne's avatar
Don Gagne committed
110
        channelValue = pwmValues[i+6];
111
        if (channelValue > 1800) {
Don Gagne's avatar
Don Gagne committed
112 113
            _rgChannelOptionEnabled[i] = QVariant(true);
        }
Don Gagne's avatar
Don Gagne committed
114
    }
Don Gagne's avatar
Don Gagne committed
115
    emit channelOptionEnabledChanged();
Don Gagne's avatar
Don Gagne committed
116
}
117 118 119 120 121 122 123 124 125 126 127 128

void APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode(void)
{
    int newSimpleModeValue = 0;
    int newSuperSimpleModeValue = 0;

    if (_simpleMode == SimpleModeSimple) {
        newSimpleModeValue = _allSimpleBits;
    } else if (_simpleMode == SimpleModeSuperSimple) {
        newSuperSimpleModeValue = _allSimpleBits;
    }

129 130 131 132 133 134 135
    for (int i=0; i<_cFltModes; i++) {
        _simpleModeEnabled[i] =         false;
        _superSimpleModeEnabled[i] =    false;
    }
    emit simpleModeEnabledChanged();
    emit superSimpleModeEnabledChanged();

136 137 138 139 140 141 142
    _simpleModeFact->setRawValue(newSimpleModeValue);
    _superSimpleModeFact->setRawValue(newSuperSimpleModeValue);
}

void APMFlightModesComponentController::setSimpleMode(int fltModeIndex, bool enabled)
{
    if (fltModeIndex < _cFltModes) {
143 144 145 146 147
        uint8_t mode = static_cast<uint8_t>(_simpleModeFact->rawValue().toInt());
        if (enabled) {
            mode |= 1 << fltModeIndex;
        } else {
            mode &= ~(1 << fltModeIndex);
148
        }
149
        _simpleModeFact->setRawValue(mode);
150 151 152 153 154 155
    }
}

void APMFlightModesComponentController::setSuperSimpleMode(int fltModeIndex, bool enabled)
{
    if (fltModeIndex < _cFltModes) {
156 157 158 159 160
        uint8_t mode = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toInt());
        if (enabled) {
            mode |= 1 << fltModeIndex;
        } else {
            mode &= ~(1 << fltModeIndex);
161
        }
162
        _superSimpleModeFact->setRawValue(mode);
163 164 165
    }
}

166
void APMFlightModesComponentController::_setupSimpleModeEnabled(void)
167 168 169 170
{
    uint8_t simpleMode =        static_cast<uint8_t>(_simpleModeFact->rawValue().toUInt());
    uint8_t superSimpleMode =   static_cast<uint8_t>(_superSimpleModeFact->rawValue().toUInt());

171 172 173 174
    for (int i=0; i<_cFltModes; i++) {
        uint8_t bitSet = static_cast<uint8_t>(1 << i);
        _simpleModeEnabled[i] = !!(simpleMode & bitSet);
        _superSimpleModeEnabled[i] = !!(superSimpleMode & bitSet);
175 176 177 178 179
    }

    emit simpleModeEnabledChanged();
    emit superSimpleModeEnabledChanged();
}