From 33c9059d721b08984a289a8f563fe7edc55f5294 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Dec 2015 21:09:33 -0800 Subject: [PATCH] Live flight mode config --- .../APM/APMFlightModesComponent.qml | 59 ++----------- .../APM/APMFlightModesComponentController.cc | 87 +++++-------------- .../APM/APMFlightModesComponentController.h | 29 ++++--- 3 files changed, 48 insertions(+), 127 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml b/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml index 9caf04f69..9c2a57ec5 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml @@ -77,11 +77,14 @@ QGCView { Row { spacing: ScreenTools.defaultFontPixelWidth - property int index: modelData + 1 + property int index: modelData + 1 + property var pwmStrings: [ "PWM 0 - 1230", "PWM 1231 - 1360", "PWM 1361 - 1490", "PWM 1491 - 1620", "PWM 1621 - 1749", "PWM 1750 +"] + QGCLabel { anchors.baseline: modeCombo.baseline text: "Flight Mode " + index + ":" + color: controller.activeFlightMode == index ? qgcPal.buttonHighlight : qgcPal.text } FactComboBox { @@ -91,56 +94,9 @@ QGCView { indexModel: false } - QGCCheckBox { - id: simple - anchors.baseline: modeCombo.baseline - text: "Simple Mode" - visible: !controller.fixedWing - checked: isChecked() - - onClicked: { - var simpleFact = controller.getParameterFact(-1, "SIMPLE") - if (checked) { - simpleFact.value |= 1 << modelData - } else { - simpleFact.value &= ~modelData - } - } - - function isChecked() { - if (simple.visible) { - var simpleFact = controller.getParameterFact(-1, "SIMPLE") - return simpleFact.value & (1 << modelData) - } else { - return false; - } - } - } - - QGCCheckBox { - id: superSimple + QGCLabel { anchors.baseline: modeCombo.baseline - text: "Super Simple Mode" - visible: !controller.fixedWing - checked: isChecked() - - onClicked: { - var simpleFact = controller.getParameterFact(-1, "SUPER_SIMPLE") - if (checked) { - simpleFact.value |= 1 << modelData - } else { - simpleFact.value &= ~modelData - } - } - - function isChecked() { - if (superSimple.visible) { - var simpleFact = controller.getParameterFact(-1, "SUPER_SIMPLE") - return simpleFact.value & (1 << modelData) - } else { - return false; - } - } + text: pwmStrings[modelData] } } } // Repeater - Flight Modes @@ -176,6 +132,9 @@ QGCView { QGCLabel { anchors.baseline: optCombo.baseline text: "Channel option " + index + ":" + color: controller.channelOptionEnabled[modelData] ? qgcPal.buttonHighlight : qgcPal.text + + Component.onCompleted: console.log(index, controller.channelOptionEnabled[modelData]) } FactComboBox { diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc index b1a3dbe45..e6a5f079e 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc @@ -29,7 +29,9 @@ #include APMFlightModesComponentController::APMFlightModesComponentController(void) - : _channelCount(Vehicle::cMaxRcChannels) + : _activeFlightMode(0) + , _channelCount(Vehicle::cMaxRcChannels) + , _fixedWing(_vehicle->vehicleType() == MAV_TYPE_FIXED_WING) { QStringList usedParams; usedParams << "FLTMODE1" << "FLTMODE2" << "FLTMODE3" << "FLTMODE4" << "FLTMODE5" << "FLTMODE6"; @@ -37,78 +39,37 @@ APMFlightModesComponentController::APMFlightModesComponentController(void) return; } - _init(); - _validateConfiguration(); + _rgChannelOptionEnabled << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false); connect(_vehicle, &Vehicle::rcChannelsChanged, this, &APMFlightModesComponentController::_rcChannelsChanged); } -void APMFlightModesComponentController::_init(void) +/// Connected to Vehicle::rcChannelsChanged signal +void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]) { - _fixedWing = _vehicle->vehicleType() == MAV_TYPE_FIXED_WING; -} + Q_UNUSED(channelCount); -/// This will look for parameter settings which would cause the config to not run correctly. -/// It will set _validConfiguration and _configurationErrors as needed. -void APMFlightModesComponentController::_validateConfiguration(void) -{ - _validConfiguration = true; -#if 0 - - // Make sure switches are valid and within channel range - - QStringList switchParams; - QList switchMappings; - - 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"; - - for(int i=0; irawValue().toInt(); - switchMappings << map; - - if (map < 0 || map > _channelCount) { - _validConfiguration = false; - _configurationErrors += QString("%1 is set to %2. Mapping must between 0 and %3 (inclusive).\n").arg(switchParams[i]).arg(map).arg(_channelCount); - } - } - - // Make sure mode switches are not double-mapped - - QStringList attitudeParams; - - attitudeParams << "RC_MAP_THROTTLE" << "RC_MAP_YAW" << "RC_MAP_PITCH" << "RC_MAP_ROLL" << "RC_MAP_FLAPS" << "RC_MAP_AUX1" << "RC_MAP_AUX2"; + _activeFlightMode = 0; - for (int i=0; irawValue().toInt(); - - for (int j=0; jrawValue().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); - } - } -#endif -} + emit activeFlightModeChanged(_activeFlightMode); -/// Connected to Vehicle::rcChannelsChanged signal -void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]) -{ - for (int channel=0; channel 1800) { + _rgChannelOptionEnabled[i] = QVariant(true); + } } + emit channelOptionEnabledChanged(); } diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h index 886a7dba3..127d669c3 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h @@ -42,25 +42,26 @@ class APMFlightModesComponentController : public FactPanelController public: APMFlightModesComponentController(void); - Q_PROPERTY(int channelCount MEMBER _channelCount CONSTANT) - Q_PROPERTY(bool fixedWing MEMBER _fixedWing CONSTANT) - Q_PROPERTY(QString reservedChannels MEMBER _reservedChannels CONSTANT) + Q_PROPERTY(int activeFlightMode READ activeFlightMode NOTIFY activeFlightModeChanged) + Q_PROPERTY(int channelCount MEMBER _channelCount CONSTANT) + Q_PROPERTY(QVariantList channelOptionEnabled READ channelOptionEnabled NOTIFY channelOptionEnabledChanged) + Q_PROPERTY(bool fixedWing MEMBER _fixedWing CONSTANT) + + int activeFlightMode(void) { return _activeFlightMode; } + QVariantList channelOptionEnabled(void) { return _rgChannelOptionEnabled; } + +signals: + void activeFlightModeChanged(int activeFlightMode); + void channelOptionEnabledChanged(void); private slots: void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]); private: - void _init(void); - void _validateConfiguration(void); - - bool _fixedWing; - - int _rcValues[Vehicle::cMaxRcChannels]; - - bool _validConfiguration; - QString _configurationErrors; - int _channelCount; - QString _reservedChannels; + int _activeFlightMode; + int _channelCount; + QVariantList _rgChannelOptionEnabled; + bool _fixedWing; }; #endif -- 2.22.0