From 3089da58b4d6ddab9daf74f6a0a095fde36357ef Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 7 Sep 2019 11:01:57 -0700 Subject: [PATCH] Enable ArduPilot motor testing --- .../APM/APMAutoPilotPlugin.cc | 2 +- src/AutoPilotPlugins/APM/APMMotorComponent.cc | 9 +- src/AutoPilotPlugins/APM/APMMotorComponent.h | 6 +- .../APM/APMMotorComponent.qml | 147 ++++++++++++++++++ src/FirmwarePlugin/APM/APMResources.qrc | 1 + src/Vehicle/Vehicle.cc | 4 +- src/Vehicle/Vehicle.h | 3 +- src/VehicleSetup/SetupView.qml | 2 + 8 files changed, 168 insertions(+), 6 deletions(-) create mode 100644 src/AutoPilotPlugins/APM/APMMotorComponent.qml diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc index 7dab87766..f311bbd68 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc @@ -91,7 +91,7 @@ const QVariantList& APMAutoPilotPlugin::vehicleComponents(void) _powerComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_powerComponent)); - if (_vehicle->sub() && _vehicle->versionCompare(3, 5, 3) >= 0) { + if (!_vehicle->sub() || (_vehicle->sub() && _vehicle->versionCompare(3, 5, 3) >= 0)) { _motorComponent = new APMMotorComponent(_vehicle, this); _motorComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_motorComponent)); diff --git a/src/AutoPilotPlugins/APM/APMMotorComponent.cc b/src/AutoPilotPlugins/APM/APMMotorComponent.cc index ce54a1646..58cbad74a 100644 --- a/src/AutoPilotPlugins/APM/APMMotorComponent.cc +++ b/src/AutoPilotPlugins/APM/APMMotorComponent.cc @@ -22,6 +22,13 @@ QUrl APMMotorComponent::setupSource(void) const case MAV_TYPE_SUBMARINE: return QUrl::fromUserInput(QStringLiteral("qrc:/qml/APMSubMotorComponent.qml")); default: - return QUrl::fromUserInput(QStringLiteral("qrc:/qml/MotorComponent.qml")); + return QUrl::fromUserInput(QStringLiteral("qrc:/qml/APMMotorComponent.qml")); } } + +QString APMMotorComponent::motorIndexToLetter(int index) +{ + char letter = 'A'; + + return QString(letter + index); +} diff --git a/src/AutoPilotPlugins/APM/APMMotorComponent.h b/src/AutoPilotPlugins/APM/APMMotorComponent.h index bed9f974f..269810604 100644 --- a/src/AutoPilotPlugins/APM/APMMotorComponent.h +++ b/src/AutoPilotPlugins/APM/APMMotorComponent.h @@ -20,7 +20,11 @@ class APMMotorComponent : public MotorComponent public: APMMotorComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = nullptr); - QUrl setupSource(void) const final; + // VehicleComponent overrides + QUrl setupSource (void) const override; + bool allowSetupWhileArmed (void) const override { return true; } + + Q_INVOKABLE QString motorIndexToLetter(int index); private: const QString _name; diff --git a/src/AutoPilotPlugins/APM/APMMotorComponent.qml b/src/AutoPilotPlugins/APM/APMMotorComponent.qml new file mode 100644 index 000000000..5492f8909 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMMotorComponent.qml @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Dialogs 1.2 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.ScreenTools 1.0 + +SetupPage { + id: motorPage + pageComponent: pageComponent + + readonly property int _barHeight: 10 + readonly property int _barWidth: 5 + readonly property int _sliderHeight: 10 + readonly property int _motorTimeoutSecs: 3 + + FactPanelController { + id: controller + } + + Component { + id: pageComponent + + Column { + spacing: ScreenTools.defaultFontPixelHeight + + QGCLabel { + text: qsTr("Warning: Unable to determine motor count") + color: qgcPal.warningText + visible: controller.vehicle.motorCount == -1 + } + + Row { + id: motorSliders + enabled: safetySwitch.checked + spacing: ScreenTools.defaultFontPixelWidth * 4 + + Repeater { + id: sliderRepeater + model: controller.vehicle.motorCount == -1 ? 8 : controller.vehicle.motorCount + + Column { + property alias motorSlider: slider + + QGCLabel { + anchors.horizontalCenter: parent.horizontalCenter + text: vehicleComponent.motorIndexToLetter(index) + } + + QGCSlider { + id: slider + height: ScreenTools.defaultFontPixelHeight * _sliderHeight + orientation: Qt.Vertical + minimumValue: 0 + maximumValue: 100 + stepSize: 1 + value: 0 + updateValueWhileDragging: false + + onValueChanged: { + controller.vehicle.motorTest(index + 1, value, value == 0 ? 0 : _motorTimeoutSecs) + if (value != 0) { + motorTimer.restart() + } + } + + Timer { + id: motorTimer + interval: _motorTimeoutSecs * 1000 + repeat: false + running: false + + onTriggered: { + allSlider.value = 0 + slider.value = 0 + } + } + } + } // Column + } // Repeater + + Column { + QGCLabel { + anchors.horizontalCenter: parent.horizontalCenter + text: qsTr("All") + } + + QGCSlider { + id: allSlider + height: ScreenTools.defaultFontPixelHeight * _sliderHeight + orientation: Qt.Vertical + minimumValue: 0 + maximumValue: 100 + stepSize: 1 + value: 0 + updateValueWhileDragging: false + + onValueChanged: { + for (var sliderIndex=0; sliderIndex../../AutoPilotPlugins/APM/APMHeliComponent.qml ../../AutoPilotPlugins/APM/APMLightsComponent.qml ../../AutoPilotPlugins/APM/APMLightsComponentSummary.qml + ../../AutoPilotPlugins/APM/APMMotorComponent.qml ../../AutoPilotPlugins/APM/APMSubFrameComponent.qml ../../AutoPilotPlugins/APM/APMSubFrameComponentSummary.qml ../../AutoPilotPlugins/APM/APMSubMotorComponent.qml diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2419ed135..23bc05d47 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3487,9 +3487,9 @@ void Vehicle::setSoloFirmware(bool soloFirmware) } } -void Vehicle::motorTest(int motor, int percent) +void Vehicle::motorTest(int motor, int percent, int timeoutSecs) { - sendMavCommand(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, true, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, 0, 0, MOTOR_TEST_ORDER_BOARD); + sendMavCommand(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, true, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs, 0, MOTOR_TEST_ORDER_BOARD); } QString Vehicle::brandImageIndoor(void) const diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 56d0fa254..2876044cc 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -765,7 +765,8 @@ public: /// Test motor /// @param motor Motor number, 1-based /// @param percent 0-no power, 100-full power - Q_INVOKABLE void motorTest(int motor, int percent); + /// @param timeoutSec Disabled motor after this amount of time + Q_INVOKABLE void motorTest(int motor, int percent, int timeoutSecs); Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning); diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml index ab366415b..a7feef84e 100644 --- a/src/VehicleSetup/SetupView.qml +++ b/src/VehicleSetup/SetupView.qml @@ -337,11 +337,13 @@ Rectangle { anchors.bottom: parent.bottom function setSource(source, vehicleComponent) { + panelLoader.source = "" panelLoader.vehicleComponent = vehicleComponent panelLoader.source = source } function setSourceComponent(sourceComponent, vehicleComponent) { + panelLoader.sourceComponent = undefined panelLoader.vehicleComponent = vehicleComponent panelLoader.sourceComponent = sourceComponent } -- 2.22.0