From f90d1b9852483b2a48efdc6a6aa22681dadcf984 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sat, 4 May 2019 11:31:55 -0700 Subject: [PATCH] Update for correct simple parameter semantics --- .../APM/APMFlightModesComponent.qml | 4 +- .../APM/APMFlightModesComponentController.cc | 73 +++++++------------ .../APM/APMFlightModesComponentController.h | 3 +- 3 files changed, 29 insertions(+), 51 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml b/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml index 5b9b85827..e02dbe776 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml @@ -150,7 +150,7 @@ SetupPage { Layout.alignment: Qt.AlignHCenter visible: _customSimpleMode checked: modelData - onClicked: controller.setSimpleMode(index, checked) + onClicked: controller.setSuperSimpleMode(index, checked) } } @@ -166,7 +166,7 @@ SetupPage { spacing: _margins visible: controller.simpleModesSupported - QGCLabel { text: qsTr("Set All Flight Modes To") } + QGCLabel { text: qsTr("Simple Mode") } QGCComboBox { model: controller.simpleModeNames diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc index 83ea9f5dd..af00bf41d 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc @@ -39,13 +39,7 @@ APMFlightModesComponentController::APMFlightModesComponentController(void) } if (!_rover) { - for (int i=0; i<_cFltModes; i++) { - Fact* fltModeFact = getParameterFact(-1, QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i+1)); - _rgFltModeFacts.append(fltModeFact); - connect(fltModeFact, &Fact::rawValueChanged, this, &APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams); - } - - _adjustSimpleModeEnabledFromParams(); + _setupSimpleModeEnabled(); uint8_t simpleModeValue = static_cast(_simpleModeFact->rawValue().toUInt()); uint8_t superSimpleModeValue = static_cast(_superSimpleModeFact->rawValue().toUInt()); @@ -59,9 +53,7 @@ APMFlightModesComponentController::APMFlightModesComponentController(void) _simpleMode = SimpleModeCustom; } - connect(this, &APMFlightModesComponentController::simpleModeChanged, this, &APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode); - connect(_simpleModeFact, &Fact::rawValueChanged, this, &APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams); - connect(_superSimpleModeFact, &Fact::rawValueChanged, this, &APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams); + connect(this, &APMFlightModesComponentController::simpleModeChanged, this, &APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode); } QStringList usedParams; @@ -131,6 +123,13 @@ void APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode(void) newSuperSimpleModeValue = _allSimpleBits; } + for (int i=0; i<_cFltModes; i++) { + _simpleModeEnabled[i] = false; + _superSimpleModeEnabled[i] = false; + } + emit simpleModeEnabledChanged(); + emit superSimpleModeEnabledChanged(); + _simpleModeFact->setRawValue(newSimpleModeValue); _superSimpleModeFact->setRawValue(newSuperSimpleModeValue); } @@ -138,58 +137,38 @@ void APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode(void) void APMFlightModesComponentController::setSimpleMode(int fltModeIndex, bool enabled) { if (fltModeIndex < _cFltModes) { - int fltMode = _rgFltModeFacts[fltModeIndex]->rawValue().toInt(); - if (fltMode < _cSimpleModeBits) { - uint8_t mode = static_cast(_simpleModeFact->rawValue().toInt()); - if (enabled) { - mode |= 1 << fltMode; - } else { - mode &= ~(1 << fltMode); - } - _simpleModeFact->setRawValue(mode); + uint8_t mode = static_cast(_simpleModeFact->rawValue().toInt()); + if (enabled) { + mode |= 1 << fltModeIndex; + } else { + mode &= ~(1 << fltModeIndex); } + _simpleModeFact->setRawValue(mode); } } void APMFlightModesComponentController::setSuperSimpleMode(int fltModeIndex, bool enabled) { if (fltModeIndex < _cFltModes) { - int fltMode = _rgFltModeFacts[fltModeIndex]->rawValue().toInt(); - if (fltMode < _cSimpleModeBits) { - uint8_t mode = static_cast(_superSimpleModeFact->rawValue().toInt()); - if (enabled) { - mode |= 1 << fltMode; - } else { - mode &= ~(1 << fltMode); - } - _superSimpleModeFact->setRawValue(mode); + uint8_t mode = static_cast(_superSimpleModeFact->rawValue().toInt()); + if (enabled) { + mode |= 1 << fltModeIndex; + } else { + mode &= ~(1 << fltModeIndex); } + _superSimpleModeFact->setRawValue(mode); } } -void APMFlightModesComponentController::_adjustSimpleModeEnabledFromParams(void) +void APMFlightModesComponentController::_setupSimpleModeEnabled(void) { uint8_t simpleMode = static_cast(_simpleModeFact->rawValue().toUInt()); uint8_t superSimpleMode = static_cast(_superSimpleModeFact->rawValue().toUInt()); - for (int fltModeIndex=0; fltModeIndex<_cFltModes; fltModeIndex++) { - _simpleModeEnabled[fltModeIndex] = false; - _superSimpleModeEnabled[fltModeIndex] = false; - } - - for (int fltModeBit=0; fltModeBit<_cSimpleModeBits; fltModeBit++) { - for (int fltModeIndex=0; fltModeIndex<_cFltModes; fltModeIndex++) { - int fltModeValue = _rgFltModeFacts[fltModeIndex]->rawValue().toInt(); - uint8_t bitSet = 1 << fltModeBit; - bool simpleModeBitEnabled = !!(simpleMode & bitSet) && fltModeValue == fltModeBit; - bool superSimpleModeBitEnabled = !!(superSimpleMode & bitSet) && fltModeValue == fltModeBit; - if (simpleModeBitEnabled) { - _simpleModeEnabled[fltModeIndex] = true; - } - if (superSimpleModeBitEnabled) { - _superSimpleModeEnabled[fltModeIndex] = true; - } - } + for (int i=0; i<_cFltModes; i++) { + uint8_t bitSet = static_cast(1 << i); + _simpleModeEnabled[i] = !!(simpleMode & bitSet); + _superSimpleModeEnabled[i] = !!(superSimpleMode & bitSet); } emit simpleModeEnabledChanged(); diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h index dedc1d262..e7a9e4412 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h @@ -64,7 +64,7 @@ signals: private slots: void _rcChannelsChanged (int channelCount, int pwmValues[Vehicle::cMaxRcChannels]); void _updateSimpleParamsFromSimpleMode (void); - void _adjustSimpleModeEnabledFromParams (void); + void _setupSimpleModeEnabled (void); private: bool _rover; @@ -75,7 +75,6 @@ private: QVariantList _rgChannelOptionEnabled; QStringList _simpleModeNames; int _simpleMode; - QList _rgFltModeFacts; Fact* _simpleModeFact; Fact* _superSimpleModeFact; bool _simpleModesSupported; -- 2.22.0