From b8e679802b4b15b0fcd123785e0e4f29d3d7553e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Dec 2015 14:56:03 -0800 Subject: [PATCH] APM Radio Config Refactored PX4 radio config to work with both PX4 and APM --- qgroundcontrol.pro | 15 +- qgroundcontrol.qrc | 5 +- .../APM/APMAutoPilotPlugin.cc | 5 + src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h | 5 + src/AutoPilotPlugins/APM/APMRadioComponent.cc | 120 +++++++ src/AutoPilotPlugins/APM/APMRadioComponent.h | 56 +++ .../APM/APMRadioComponentSummary.qml | 46 +++ .../{PX4 => Common}/RadioComponent.qml | 49 +-- .../RadioComponentController.cc | 329 +++++++++++------- .../RadioComponentController.h | 29 +- .../PX4/PX4AutoPilotPlugin.cc | 2 +- src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h | 6 +- ...RadioComponent.cc => PX4RadioComponent.cc} | 33 +- .../{RadioComponent.h => PX4RadioComponent.h} | 13 +- ...mmary.qml => PX4RadioComponentSummary.qml} | 0 src/comm/LinkManager.h | 5 + ...CCalibrationTest.cc => RadioConfigTest.cc} | 229 +++++++----- ...4RCCalibrationTest.h => RadioConfigTest.h} | 43 ++- 18 files changed, 690 insertions(+), 300 deletions(-) create mode 100644 src/AutoPilotPlugins/APM/APMRadioComponent.cc create mode 100644 src/AutoPilotPlugins/APM/APMRadioComponent.h create mode 100644 src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml rename src/AutoPilotPlugins/{PX4 => Common}/RadioComponent.qml (94%) rename src/AutoPilotPlugins/{PX4 => Common}/RadioComponentController.cc (75%) rename src/AutoPilotPlugins/{PX4 => Common}/RadioComponentController.h (91%) rename src/AutoPilotPlugins/PX4/{RadioComponent.cc => PX4RadioComponent.cc} (77%) rename src/AutoPilotPlugins/PX4/{RadioComponent.h => PX4RadioComponent.h} (82%) rename src/AutoPilotPlugins/PX4/{RadioComponentSummary.qml => PX4RadioComponentSummary.qml} (100%) rename src/qgcunittest/{PX4RCCalibrationTest.cc => RadioConfigTest.cc} (68%) rename src/qgcunittest/{PX4RCCalibrationTest.h => RadioConfigTest.h} (80%) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 98e0bd5f3..a56e3401b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -499,7 +499,7 @@ HEADERS += \ src/qgcunittest/MavlinkLogTest.h \ src/qgcunittest/MessageBoxTest.h \ src/qgcunittest/MultiSignalSpy.h \ - src/qgcunittest/PX4RCCalibrationTest.h \ + src/qgcunittest/RadioConfigTest.h \ src/qgcunittest/TCPLinkTest.h \ src/qgcunittest/TCPLoopBackServer.h \ src/qgcunittest/UnitTest.h \ @@ -522,7 +522,7 @@ SOURCES += \ src/qgcunittest/MavlinkLogTest.cc \ src/qgcunittest/MessageBoxTest.cc \ src/qgcunittest/MultiSignalSpy.cc \ - src/qgcunittest/PX4RCCalibrationTest.cc \ + src/qgcunittest/RadioConfigTest.cc \ src/qgcunittest/TCPLinkTest.cc \ src/qgcunittest/TCPLoopBackServer.cc \ src/qgcunittest/UnitTest.cc \ @@ -536,6 +536,7 @@ SOURCES += \ # INCLUDEPATH += \ + src/AutoPilotPlugins/Common \ src/AutoPilotPlugins/PX4 \ src/FirmwarePlugin \ src/Vehicle \ @@ -547,6 +548,8 @@ HEADERS+= \ src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h \ src/AutoPilotPlugins/APM/APMAirframeComponent.h \ src/AutoPilotPlugins/APM/APMComponent.h \ + src/AutoPilotPlugins/APM/APMRadioComponent.h \ + src/AutoPilotPlugins/Common/RadioComponentController.h \ src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h \ src/AutoPilotPlugins/PX4/AirframeComponent.h \ src/AutoPilotPlugins/PX4/AirframeComponentAirframes.h \ @@ -557,8 +560,7 @@ HEADERS+= \ src/AutoPilotPlugins/PX4/PowerComponentController.h \ src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h \ src/AutoPilotPlugins/PX4/PX4Component.h \ - src/AutoPilotPlugins/PX4/RadioComponent.h \ - src/AutoPilotPlugins/PX4/RadioComponentController.h \ + src/AutoPilotPlugins/PX4/PX4RadioComponent.h \ src/AutoPilotPlugins/PX4/SafetyComponent.h \ src/AutoPilotPlugins/PX4/SensorsComponent.h \ src/AutoPilotPlugins/PX4/SensorsComponentController.h \ @@ -591,6 +593,8 @@ SOURCES += \ src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc \ src/AutoPilotPlugins/APM/APMAirframeComponent.cc \ src/AutoPilotPlugins/APM/APMComponent.cc \ + src/AutoPilotPlugins/APM/APMRadioComponent.cc \ + src/AutoPilotPlugins/Common/RadioComponentController.cc \ src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc \ src/AutoPilotPlugins/PX4/AirframeComponent.cc \ src/AutoPilotPlugins/PX4/AirframeComponentAirframes.cc \ @@ -601,8 +605,7 @@ SOURCES += \ src/AutoPilotPlugins/PX4/PowerComponentController.cc \ src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc \ src/AutoPilotPlugins/PX4/PX4Component.cc \ - src/AutoPilotPlugins/PX4/RadioComponent.cc \ - src/AutoPilotPlugins/PX4/RadioComponentController.cc \ + src/AutoPilotPlugins/PX4/PX4RadioComponent.cc \ src/AutoPilotPlugins/PX4/SafetyComponent.cc \ src/AutoPilotPlugins/PX4/SensorsComponent.cc \ src/AutoPilotPlugins/PX4/SensorsComponentController.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 230f97571..76e196dd6 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -97,8 +97,9 @@ src/QmlControls/QGroundControl.ScreenTools.qmldir src/QmlControls/ScreenTools.qml src/QmlControls/QmlTest.qml - src/AutoPilotPlugins/PX4/RadioComponent.qml - src/AutoPilotPlugins/PX4/RadioComponentSummary.qml + src/AutoPilotPlugins/Common/RadioComponent.qml + src/AutoPilotPlugins/PX4/PX4RadioComponentSummary.qml + src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml src/AutoPilotPlugins/PX4/SafetyComponent.qml src/AutoPilotPlugins/PX4/SafetyComponentSummary.qml src/AutoPilotPlugins/PX4/SensorsComponent.qml diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc index d79966a96..1b1ef574e 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc @@ -52,6 +52,11 @@ const QVariantList& APMAutoPilotPlugin::vehicleComponents(void) Q_CHECK_PTR(_airframeComponent); _airframeComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); + + _radioComponent = new APMRadioComponent(_vehicle, this); + Q_CHECK_PTR(_radioComponent); + _radioComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); } else { qWarning() << "Call to vehicleCompenents prior to parametersReady"; } diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h index 43c639736..02b2987b7 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.h @@ -27,6 +27,7 @@ #include "AutoPilotPlugin.h" #include "Vehicle.h" #include "APMAirframeComponent.h" +#include "APMRadioComponent.h" /// This is the APM specific implementation of the AutoPilot class. class APMAutoPilotPlugin : public AutoPilotPlugin @@ -40,6 +41,9 @@ public: // Overrides from AutoPilotPlugin virtual const QVariantList& vehicleComponents(void); + APMAirframeComponent* airframeComponent(void) { return _airframeComponent; } + APMRadioComponent* radioComponent(void) { return _radioComponent; } + public slots: // FIXME: This is public until we restructure AutoPilotPlugin/FirmwarePlugin/Vehicle void _parametersReadyPreChecks(bool missingParameters); @@ -48,6 +52,7 @@ private: bool _incorrectParameterVersion; ///< true: parameter version incorrect, setup not allowed QVariantList _components; APMAirframeComponent* _airframeComponent; + APMRadioComponent* _radioComponent; }; #endif diff --git a/src/AutoPilotPlugins/APM/APMRadioComponent.cc b/src/AutoPilotPlugins/APM/APMRadioComponent.cc new file mode 100644 index 000000000..0c2e8b9b7 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMRadioComponent.cc @@ -0,0 +1,120 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "APMRadioComponent.h" +#include "APMAutoPilotPlugin.h" + +APMRadioComponent::APMRadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : + APMComponent(vehicle, autopilot, parent), + _name(tr("Radio")) +{ +} + +QString APMRadioComponent::name(void) const +{ + return _name; +} + +QString APMRadioComponent::description(void) const +{ + return tr("The Radio Component is used to setup which channels on your RC Transmitter you will use for each vehicle control such as Roll, Pitch, Yaw and Throttle. " + "It also allows you to assign switches and dials to the various flight modes. " + "Prior to flight you must also calibrate the extents for all of your channels."); +} + +QString APMRadioComponent::iconResource(void) const +{ + return "/qmlimages/RadioComponentIcon.png"; +} + +bool APMRadioComponent::requiresSetup(void) const +{ + return true; +} + +bool APMRadioComponent::setupComplete(void) const +{ + // The best we can do to detect the need for a radio calibration is look for attitude + // controls to be mapped. + QStringList attitudeMappings; + attitudeMappings << "RCMAP_ROLL" << "RCMAP_PITCH" << "RCMAP_YAW" << "RCMAP_THROTTLE"; + foreach(QString mapParam, attitudeMappings) { + if (_autopilot->getParameterFact(FactSystem::defaultComponentId, mapParam)->rawValue().toInt() == 0) { + return false; + } + } + + return true; +} + +QString APMRadioComponent::setupStateDescription(void) const +{ + const char* stateDescription; + + if (requiresSetup()) { + stateDescription = "Requires calibration"; + } else { + stateDescription = "Calibrated"; + } + return QString(stateDescription); +} + +QStringList APMRadioComponent::setupCompleteChangedTriggerList(void) const +{ + QStringList triggers; + + triggers << "RCMAP_ROLL" << "RCMAP_PITCH" << "RCMAP_YAW" << "RCMAP_THROTTLE"; + + return triggers; +} + +QStringList APMRadioComponent::paramFilterList(void) const +{ + QStringList list; + + list << "RC*"; + + return list; +} + +QUrl APMRadioComponent::setupSource(void) const +{ + return QUrl::fromUserInput("qrc:/qml/RadioComponent.qml"); +} + +QUrl APMRadioComponent::summaryQmlSource(void) const +{ + return QUrl::fromUserInput("qrc:/qml/APMRadioComponentSummary.qml"); +} + +QString APMRadioComponent::prerequisiteSetup(void) const +{ + APMAutoPilotPlugin* plugin = dynamic_cast(_autopilot); + + if (!plugin->airframeComponent()->setupComplete()) { + return plugin->airframeComponent()->name(); + + } + + return QString(); +} diff --git a/src/AutoPilotPlugins/APM/APMRadioComponent.h b/src/AutoPilotPlugins/APM/APMRadioComponent.h new file mode 100644 index 000000000..44df877c4 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMRadioComponent.h @@ -0,0 +1,56 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#ifndef APMRadioComponent_H +#define APMRadioComponent_H + +#include "APMComponent.h" + +class APMRadioComponent : public APMComponent +{ + Q_OBJECT + +public: + APMRadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); + + // Virtuals from PX4Component + virtual QStringList setupCompleteChangedTriggerList(void) const; + + // Virtuals from VehicleComponent + virtual QString name(void) const; + virtual QString description(void) const; + virtual QString iconResource(void) const; + virtual bool requiresSetup(void) const; + virtual bool setupComplete(void) const; + virtual QString setupStateDescription(void) const; + virtual QUrl setupSource(void) const; + virtual QStringList paramFilterList(void) const; + virtual QUrl summaryQmlSource(void) const; + virtual QString prerequisiteSetup(void) const; + +private: + const QString _name; + QVariantList _summaryItems; +}; + +#endif diff --git a/src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml b/src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml new file mode 100644 index 000000000..1c529c3a1 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMRadioComponentSummary.qml @@ -0,0 +1,46 @@ +import QtQuick 2.2 +import QtQuick.Controls 1.2 + +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Palette 1.0 + +FactPanel { + id: panel + anchors.fill: parent + color: qgcPal.windowShadeDark + + QGCPalette { id: qgcPal; colorGroupEnabled: enabled } + FactPanelController { id: controller; factPanel: panel } + + property Fact mapRollFact: controller.getParameterFact(-1, "RCMAP_ROLL") + property Fact mapPitchFact: controller.getParameterFact(-1, "RCMAP_PITCH") + property Fact mapYawFact: controller.getParameterFact(-1, "RCMAP_YAW") + property Fact mapThrottleFact: controller.getParameterFact(-1, "RCMAP_THROTTLE") + + Column { + anchors.fill: parent + anchors.margins: 8 + + VehicleSummaryRow { + labelText: "Roll:" + valueText: mapRollFact.value == 0 ? "Setup required" : mapRollFact.valueString + } + + VehicleSummaryRow { + labelText: "Pitch:" + valueText: mapPitchFact.value == 0 ? "Setup required" : mapPitchFact.valueString + } + + VehicleSummaryRow { + labelText: "Yaw:" + valueText: mapYawFact.value == 0 ? "Setup required" : mapYawFact.valueString + } + + VehicleSummaryRow { + labelText: "Throttle:" + valueText: mapThrottleFact.value == 0 ? "Setup required" : mapThrottleFact.valueString + } + } +} diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.qml b/src/AutoPilotPlugins/Common/RadioComponent.qml similarity index 94% rename from src/AutoPilotPlugins/PX4/RadioComponent.qml rename to src/AutoPilotPlugins/Common/RadioComponent.qml index f8c743956..c62a9e909 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.qml +++ b/src/AutoPilotPlugins/Common/RadioComponent.qml @@ -21,20 +21,17 @@ ======================================================================*/ -/// @file -/// @brief Radio Calibration -/// @author Don Gagne - -import QtQuick 2.2 +import QtQuick 2.5 import QtQuick.Controls 1.2 -import QtQuick.Dialogs 1.2 +import QtQuick.Dialogs 1.2 -import QGroundControl.FactSystem 1.0 -import QGroundControl.FactControls 1.0 -import QGroundControl.Palette 1.0 -import QGroundControl.Controls 1.0 -import QGroundControl.ScreenTools 1.0 -import QGroundControl.Controllers 1.0 +import QGroundControl 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controllers 1.0 QGCView { id: rootQGCView @@ -42,19 +39,14 @@ QGCView { QGCPalette { id: qgcPal; colorGroupEnabled: panel.enabled } - readonly property string dialogTitle: "Radio" - readonly property real labelToMonitorMargin: defaultTextWidth * 3 - property bool controllerCompleted: false - property bool controllerAndViewReady: false + readonly property string dialogTitle: "Radio" + readonly property real labelToMonitorMargin: defaultTextWidth * 3 - property Fact rcInMode: controller.getParameterFact(-1, "COM_RC_IN_MODE") + property bool controllerCompleted: false + property bool controllerAndViewReady: false function updateChannelCount() { - if (controllerAndViewReady) { - if (rcInMode.value == 1) { - showDialog(joystickEnabledDialogComponent, dialogTitle, 50, 0) - } /* FIXME: Turned off for now, since it prevents binding. Need to restructure to allow binding and still check channel count @@ -64,7 +56,6 @@ QGCView { hideDialog() } */ - } } RadioComponentController { @@ -75,8 +66,6 @@ QGCView { nextButton: nextButton skipButton: skipButton - onChannelCountChanged: updateChannelCount() - Component.onCompleted: { controllerCompleted = true if (rootQGCView.completedSignalled) { @@ -85,6 +74,9 @@ QGCView { updateChannelCount() } } + + onChannelCountChanged: updateChannelCount() + onFunctionMappingChangedAPMReboot: showMessage("Reboot required", "Your stick mappings have changed, you must reboot the vehicle for correct operation.", StandardButton.Ok) } onCompleted: { @@ -133,14 +125,6 @@ QGCView { } } - Component { - id: joystickEnabledDialogComponent - - QGCViewMessage { - message: "Radio Config is disabled since you have a Joystick enabled." - } - } - Component { id: spektrumBindDialogComponent @@ -482,6 +466,7 @@ QGCView { QGCButton { showBorder: true text: "Copy Trims" + visible: QGroundControl.multiVehicleManager.activeVehicle.px4Firmware onClicked: showDialog(copyTrimsDialogComponent, dialogTitle, 50, StandardButton.Ok | StandardButton.Cancel) } diff --git a/src/AutoPilotPlugins/PX4/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc similarity index 75% rename from src/AutoPilotPlugins/PX4/RadioComponentController.cc rename to src/AutoPilotPlugins/Common/RadioComponentController.cc index 7fe94bbf2..d86da8f9b 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponentController.cc +++ b/src/AutoPilotPlugins/Common/RadioComponentController.cc @@ -70,7 +70,7 @@ const char* RadioComponentController::_imageSwitchMinMax = "radioSwitchMinMax.p const char* RadioComponentController::_settingsGroup = "RadioCalibration"; const char* RadioComponentController::_settingsKeyTransmitterMode = "TransmitterMode"; -const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfo[RadioComponentController::rcCalFunctionMax] = { +const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoPX4[RadioComponentController::rcCalFunctionMax] = { //Parameter required { "RC_MAP_ROLL" }, { "RC_MAP_PITCH" }, @@ -86,6 +86,22 @@ const struct RadioComponentController::FunctionInfo RadioComponentController::_r { "RC_MAP_AUX2" }, }; +const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoAPM[RadioComponentController::rcCalFunctionMax] = { + //Parameter required + { "RCMAP_ROLL" }, + { "RCMAP_PITCH" }, + { "RCMAP_YAW" }, + { "RCMAP_THROTTLE" }, + { NULL }, + { NULL }, + { NULL }, + { NULL }, + { NULL }, + { NULL }, + { NULL }, + { NULL }, +}; + RadioComponentController::RadioComponentController(void) : _currentStep(-1), _transmitterMode(2), @@ -102,7 +118,7 @@ RadioComponentController::RadioComponentController(void) : _unitTestController = this; #endif - connect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &RadioComponentController::_remoteControlChannelRawChanged); + connect(_vehicle, &Vehicle::rcChannelsChanged, this, &RadioComponentController::_rcChannelsChanged); _loadSettings(); _resetInternalCalibrationValues(); @@ -120,35 +136,38 @@ RadioComponentController::~RadioComponentController() } /// @brief Returns the state machine entry for the specified state. -const RadioComponentController::stateMachineEntry* RadioComponentController::_getStateMachineEntry(int step) +const RadioComponentController::stateMachineEntry* RadioComponentController::_getStateMachineEntry(int step) const { - static const char* msgBegin = "Lower the Throttle stick all the way down as shown in diagram.\nReset all transmitter trims to center.\n\n" - "It is recommended to disconnect all motors for additional safety, however, the system is designed to not arm during the calibration.\n\n" - "Click Next to continue"; - static const char* msgThrottleUp = "Move the Throttle stick all the way up and hold it there..."; - static const char* msgThrottleDown = "Move the Throttle stick all the way down and leave it there..."; - static const char* msgYawLeft = "Move the Yaw stick all the way to the left and hold it there..."; - static const char* msgYawRight = "Move the Yaw stick all the way to the right and hold it there..."; - static const char* msgRollLeft = "Move the Roll stick all the way to the left and hold it there..."; - static const char* msgRollRight = "Move the Roll stick all the way to the right and hold it there..."; - static const char* msgPitchDown = "Move the Pitch stick all the way down and hold it there..."; - static const char* msgPitchUp = "Move the Pitch stick all the way up and hold it there..."; - static const char* msgPitchCenter = "Allow the Pitch stick to move back to center..."; - static const char* msgAux1Switch = "Move the switch or dial you want to use for Aux1.\n\n" + static const char* msgBeginPX4 = "Lower the Throttle stick all the way down as shown in diagram.\nReset all transmitter trims to center.\n\n" + "It is recommended to disconnect all motors for additional safety, however, the system is designed to not arm during the calibration.\n\n" + "Click Next to continue"; + static const char* msgBeginAPM = "Lower the Throttle stick all the way down as shown in diagram.\nReset all transmitter trims to center.\n\n" + "Please ensure all motor power is disconnected AND all props are removed from the vehicle.\n\n" + "Click Next to continue"; + static const char* msgThrottleUp = "Move the Throttle stick all the way up and hold it there..."; + static const char* msgThrottleDown = "Move the Throttle stick all the way down and leave it there..."; + static const char* msgYawLeft = "Move the Yaw stick all the way to the left and hold it there..."; + static const char* msgYawRight = "Move the Yaw stick all the way to the right and hold it there..."; + static const char* msgRollLeft = "Move the Roll stick all the way to the left and hold it there..."; + static const char* msgRollRight = "Move the Roll stick all the way to the right and hold it there..."; + static const char* msgPitchDown = "Move the Pitch stick all the way down and hold it there..."; + static const char* msgPitchUp = "Move the Pitch stick all the way up and hold it there..."; + static const char* msgPitchCenter = "Allow the Pitch stick to move back to center..."; + static const char* msgAux1Switch = "Move the switch or dial you want to use for Aux1.\n\n" "You can click Skip if you don't want to assign."; - static const char* msgAux2Switch = "Move the switch or dial you want to use for Aux2.\n\n" + static const char* msgAux2Switch = "Move the switch or dial you want to use for Aux2.\n\n" "You can click Skip if you don't want to assign."; - static const char* msgSwitchMinMax = "Move all the transmitter switches and/or dials back and forth to their extreme positions."; - static const char* msgFlapsDetect = "Move the switch or dial you want to use for Flaps back and forth a few times. " + static const char* msgSwitchMinMax = "Move all the transmitter switches and/or dials back and forth to their extreme positions."; + static const char* msgFlapsDetect = "Move the switch or dial you want to use for Flaps back and forth a few times. " "Then leave the switch/dial at the position you want to use for Flaps fully extended.\n\n" "Click Next to continue.\n" "If you won't be using Flaps, click Skip."; - static const char* msgFlapsUp = "Move the switch or dial you want to use for Flaps to the position you want to use for Flaps fully retracted."; - static const char* msgComplete = "All settings have been captured. Click Next to write the new parameters to your board."; + static const char* msgFlapsUp = "Move the switch or dial you want to use for Flaps to the position you want to use for Flaps fully retracted."; + static const char* msgComplete = "All settings have been captured. Click Next to write the new parameters to your board."; - static const stateMachineEntry rgStateMachine[] = { + static const stateMachineEntry rgStateMachinePX4[] = { //Function - { rcCalFunctionMax, msgBegin, _imageHome, &RadioComponentController::_inputCenterWaitBegin, &RadioComponentController::_saveAllTrims, NULL }, + { rcCalFunctionMax, msgBeginPX4, _imageHome, &RadioComponentController::_inputCenterWaitBegin, &RadioComponentController::_saveAllTrims, NULL }, { rcCalFunctionThrottle, msgThrottleUp, _imageThrottleUp, &RadioComponentController::_inputStickDetect, NULL, NULL }, { rcCalFunctionThrottle, msgThrottleDown, _imageThrottleDown, &RadioComponentController::_inputStickMin, NULL, NULL }, { rcCalFunctionYaw, msgYawRight, _imageYawRight, &RadioComponentController::_inputStickDetect, NULL, NULL }, @@ -166,9 +185,43 @@ const RadioComponentController::stateMachineEntry* RadioComponentController::_ge { rcCalFunctionMax, msgComplete, _imageThrottleDown, NULL, &RadioComponentController::_writeCalibration, NULL }, }; - Q_ASSERT(step >=0 && step < (int)(sizeof(rgStateMachine) / sizeof(rgStateMachine[0]))); + static const stateMachineEntry rgStateMachineAPM[] = { + //Function + { rcCalFunctionMax, msgBeginAPM, _imageHome, &RadioComponentController::_inputCenterWaitBegin, &RadioComponentController::_saveAllTrims, NULL }, + { rcCalFunctionThrottle, msgThrottleUp, _imageThrottleUp, &RadioComponentController::_inputStickDetect, NULL, NULL }, + { rcCalFunctionThrottle, msgThrottleDown, _imageThrottleDown, &RadioComponentController::_inputStickMin, NULL, NULL }, + { rcCalFunctionYaw, msgYawRight, _imageYawRight, &RadioComponentController::_inputStickDetect, NULL, NULL }, + { rcCalFunctionYaw, msgYawLeft, _imageYawLeft, &RadioComponentController::_inputStickMin, NULL, NULL }, + { rcCalFunctionRoll, msgRollRight, _imageRollRight, &RadioComponentController::_inputStickDetect, NULL, NULL }, + { rcCalFunctionRoll, msgRollLeft, _imageRollLeft, &RadioComponentController::_inputStickMin, NULL, NULL }, + { rcCalFunctionPitch, msgPitchUp, _imagePitchUp, &RadioComponentController::_inputStickDetect, NULL, NULL }, + { rcCalFunctionPitch, msgPitchDown, _imagePitchDown, &RadioComponentController::_inputStickMin, NULL, NULL }, + { rcCalFunctionPitch, msgPitchCenter, _imageHome, &RadioComponentController::_inputCenterWait, NULL, NULL }, + { rcCalFunctionMax, msgSwitchMinMax, _imageSwitchMinMax, &RadioComponentController::_inputSwitchMinMax, &RadioComponentController::_advanceState, NULL }, + { rcCalFunctionMax, msgComplete, _imageThrottleDown, NULL, &RadioComponentController::_writeCalibration, NULL }, + }; + + bool badStep = false; + if (step < 0) { + badStep = true; + } + if (_px4Vehicle()) { + if (step >= (int)(sizeof(rgStateMachinePX4) / sizeof(rgStateMachinePX4[0]))) { + badStep = true; + } + } else { + if (step >= (int)(sizeof(rgStateMachineAPM) / sizeof(rgStateMachineAPM[0]))) { + badStep = true; + } + } + if (badStep) { + qWarning() << "Bad step value" << step; + step = 0; + } + + const stateMachineEntry* stateMachine = _px4Vehicle() ? rgStateMachinePX4 : rgStateMachineAPM; - return &rgStateMachine[step]; + return &stateMachine[step]; } void RadioComponentController::_advanceState(void) @@ -187,7 +240,7 @@ void RadioComponentController::_setupCurrentState(void) _setHelpImage(state->image); - _stickDetectChannel = _chanMax; + _stickDetectChannel = _chanMax(); _stickDetectSettleStarted = false; _rcCalSaveCurrentValues(); @@ -196,53 +249,52 @@ void RadioComponentController::_setupCurrentState(void) _skipButton->setEnabled(state->skipFn != NULL); } -/// @brief This routine is called whenever a raw value for an RC channel changes. It will call the input -/// function as specified by the state machine. -/// @param chan RC channel on which signal is coming from (0-based) -/// @param fval Current value for channel -void RadioComponentController::_remoteControlChannelRawChanged(int chan, float fval) +/// Connected to Vehicle::rcChannelsChanged signal +void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]) { - if (chan >= 0 && chan <= _chanMax) { - // We always update raw values - _rcRawValue[chan] = fval; - emit channelRCValueChanged(chan, _rcRawValue[chan]); - - // Signal attitude rc values to Qml if mapped - if (_rgChannelInfo[chan].function != rcCalFunctionMax) { - switch (_rgChannelInfo[chan].function) { + int maxChannel = std::min(channelCount, _chanMax()); + + for (int channel=0; channel (int)_chanCount) { - _chanCount = chan + 1; - emit channelCountChanged(_chanCount); - } - } - - if (_currentStep != -1) { - const stateMachineEntry* state = _getStateMachineEntry(_currentStep); - Q_ASSERT(state); - if (state->rcInputFn) { - (this->*state->rcInputFn)(state->function, chan, fval); + + if (_currentStep == -1) { + if (_chanCount != channelCount) { + _chanCount = channelCount; + emit channelCountChanged(_chanCount); + } + } else { + const stateMachineEntry* state = _getStateMachineEntry(_currentStep); + Q_ASSERT(state); + if (state->rcInputFn) { + (this->*state->rcInputFn)(state->function, channel, channelValue); + } } } } @@ -346,14 +398,14 @@ bool RadioComponentController::_stickSettleComplete(int value) void RadioComponentController::_inputStickDetect(enum rcCalFunctions function, int channel, int value) { - qCDebug(RadioComponentControllerLog) << "_inputStickDetect function:channel:value" << _rgFunctionInfo[function].parameterName << channel << value; + qCDebug(RadioComponentControllerLog) << "_inputStickDetect function:channel:value" << _functionInfo()[function].parameterName << channel << value; // If this channel is already used in a mapping we can't use it again if (_rgChannelInfo[channel].function != rcCalFunctionMax) { return; } - if (_stickDetectChannel == _chanMax) { + if (_stickDetectChannel == _chanMax()) { // We have not detected enough movement on a channel yet if (abs(_rcValueSave[channel] - value) > _rcCalMoveDelta) { @@ -400,7 +452,7 @@ void RadioComponentController::_inputStickMin(enum rcCalFunctions function, int return; } - if (_stickDetectChannel == _chanMax) { + if (_stickDetectChannel == _chanMax()) { // Setup up to detect stick being pegged to extreme position if (_rgChannelInfo[channel].reversed) { if (value > _rcCalPWMCenterPoint + _rcCalMoveDelta) { @@ -447,7 +499,7 @@ void RadioComponentController::_inputCenterWait(enum rcCalFunctions function, in return; } - if (_stickDetectChannel == _chanMax) { + if (_stickDetectChannel == _chanMax()) { // Sticks have not yet moved close enough to center if (abs(_rcCalPWMCenterPoint - value) < _rcCalRoughCenterDelta) { @@ -499,7 +551,7 @@ void RadioComponentController::_skipFlaps(void) _rgChannelInfo[i].function = rcCalFunctionMax; } } - _rgFunctionChannelMapping[RadioComponentController::rcCalFunctionFlaps] = _chanMax; + _rgFunctionChannelMapping[RadioComponentController::rcCalFunctionFlaps] = _chanMax(); // Skip over flap steps _currentStep += 2; @@ -510,7 +562,7 @@ void RadioComponentController::_saveFlapsDown(void) { int channel = _rgFunctionChannelMapping[rcCalFunctionFlaps]; - if (channel == _chanMax) { + if (channel == _chanMax()) { // Channel not yet mapped, still waiting for switch to move if (_unitTestMode) { emit nextButtonMessageBoxDisplayed(); @@ -552,7 +604,7 @@ void RadioComponentController::_inputFlapsUp(enum rcCalFunctions function, int c return; } - if (_stickDetectChannel == _chanMax) { + if (_stickDetectChannel == _chanMax()) { // Setup up to detect stick being pegged to extreme position if (_rgChannelInfo[channel].reversed) { if (value > _rcCalPWMCenterPoint + _rcCalMoveDelta) { @@ -624,7 +676,7 @@ void RadioComponentController::_inputFlapsDetect(enum rcCalFunctions function, i void RadioComponentController::_resetInternalCalibrationValues(void) { // Set all raw channels to not reversed and center point values - for (int i=0; i<_chanMax; i++) { + for (int i=0; i<_chanMax(); i++) { struct ChannelInfo* info = &_rgChannelInfo[i]; info->function = rcCalFunctionMax; info->reversed = false; @@ -635,33 +687,35 @@ void RadioComponentController::_resetInternalCalibrationValues(void) // Initialize attitude function mapping to function channel not set for (size_t i=0; irawValue().toInt(&ok); - Q_ASSERT(ok); - - // Parameter: 1-based channel, 0=not mapped - // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped - - if (switchChannel != 0) { - qCDebug(RadioComponentControllerLog) << "Reserving 0-based switch channel" << switchChannel - 1; - _rgFunctionChannelMapping[curFunction] = switchChannel - 1; - _rgChannelInfo[switchChannel - 1].function = curFunction; + static const rcCalFunctions rgFlightModeFunctions[] = { + rcCalFunctionModeSwitch, + rcCalFunctionPosCtlSwitch, + rcCalFunctionLoiterSwitch, + rcCalFunctionReturnSwitch }; + static const size_t crgFlightModeFunctions = sizeof(rgFlightModeFunctions) / sizeof(rgFlightModeFunctions[0]); + + for (size_t i=0; i < crgFlightModeFunctions; i++) { + QVariant value; + enum rcCalFunctions curFunction = rgFlightModeFunctions[i]; + + bool ok; + int switchChannel = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[curFunction].parameterName)->rawValue().toInt(&ok); + Q_ASSERT(ok); + + // Parameter: 1-based channel, 0=not mapped + // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped + + if (switchChannel != 0) { + qCDebug(RadioComponentControllerLog) << "Reserving 0-based switch channel" << switchChannel - 1; + _rgFunctionChannelMapping[curFunction] = switchChannel - 1; + _rgChannelInfo[switchChannel - 1].function = curFunction; + } } } @@ -673,13 +727,13 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void) { // Initialize all function mappings to not set - for (int i=0; i<_chanMax; i++) { + for (int i=0; i<_chanMax(); i++) { struct ChannelInfo* info = &_rgChannelInfo[i]; info->function = rcCalFunctionMax; } for (size_t i=0; ircTrim = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1))->rawValue().toInt(&convertOk); @@ -712,12 +766,15 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void) for (int i=0; irawValue().toInt(&convertOk); - Q_ASSERT(convertOk); - - if (paramChannel != 0) { - _rgFunctionChannelMapping[i] = paramChannel - 1; - _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i; + const char* paramName = _functionInfo()[i].parameterName; + if (paramName) { + paramChannel = getParameterFact(FactSystem::defaultComponentId, paramName)->rawValue().toInt(&convertOk); + Q_ASSERT(convertOk); + + if (paramChannel != 0) { + _rgFunctionChannelMapping[i] = paramChannel - 1; + _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i; + } } } @@ -732,7 +789,7 @@ void RadioComponentController::spektrumBindMode(int mode) /// @brief Validates the current settings against the calibration rules resetting values as necessary. void RadioComponentController::_validateCalibration(void) { - for (int chan = 0; chan<_chanMax; chan++) { + for (int chan = 0; chan<_chanMax(); chan++) { struct ChannelInfo* info = &_rgChannelInfo[chan]; if (chan < _chanCount) { @@ -790,7 +847,7 @@ void RadioComponentController::_writeCalibration(void) QString revTpl("RC%1_REV"); // Note that the rc parameters are all float, so you must cast to float in order to get the right QVariant - for (int chan = 0; chan<_chanMax; chan++) { + for (int chan = 0; chan<_chanMax(); chan++) { struct ChannelInfo* info = &_rgChannelInfo[chan]; int oneBasedChannel = chan + 1; @@ -801,25 +858,40 @@ void RadioComponentController::_writeCalibration(void) } // Write function mapping parameters + bool functionMappingChanged = false; for (size_t i=0; isetRawValue(paramChannel); + const char* paramName = _functionInfo()[i].parameterName; + if (paramName) { + Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName); + + if (paramFact->rawValue().toInt() != paramChannel) { + functionMappingChanged = true; + getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName)->setRawValue(paramChannel); + } + } } - // If the RC_CHAN_COUNT parameter is available write the channel count - if (parameterExists(FactSystem::defaultComponentId, "RC_CHAN_CNT")) { - getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->setRawValue(_chanCount); + if (_px4Vehicle()) { + // If the RC_CHAN_COUNT parameter is available write the channel count + if (parameterExists(FactSystem::defaultComponentId, "RC_CHAN_CNT")) { + getParameterFact(FactSystem::defaultComponentId, "RC_CHAN_CNT")->setRawValue(_chanCount); + } } _stopCalibration(); _setInternalCalibrationValuesFromParameters(); + + if (_vehicle->apmFirmware() && functionMappingChanged) { + emit functionMappingChangedAPMReboot(); + } } /// @brief Starts the calibration process @@ -865,7 +937,7 @@ void RadioComponentController::_stopCalibration(void) void RadioComponentController::_rcCalSaveCurrentValues(void) { qCDebug(RadioComponentControllerLog) << "_rcCalSaveCurrentValues"; - for (int i = 0; i < _chanMax; i++) { + for (int i = 0; i < _chanMax(); i++) { _rcValueSave[i] = _rcRawValue[i]; } } @@ -936,7 +1008,7 @@ int RadioComponentController::channelCount(void) int RadioComponentController::rollChannelRCValue(void) { - if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) { return _rcRawValue[rcCalFunctionRoll]; } else { return 1500; @@ -945,7 +1017,7 @@ int RadioComponentController::rollChannelRCValue(void) int RadioComponentController::pitchChannelRCValue(void) { - if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) { return _rcRawValue[rcCalFunctionPitch]; } else { return 1500; @@ -954,7 +1026,7 @@ int RadioComponentController::pitchChannelRCValue(void) int RadioComponentController::yawChannelRCValue(void) { - if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) { return _rcRawValue[rcCalFunctionYaw]; } else { return 1500; @@ -963,7 +1035,7 @@ int RadioComponentController::yawChannelRCValue(void) int RadioComponentController::throttleChannelRCValue(void) { - if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) { return _rcRawValue[rcCalFunctionThrottle]; } else { return 1500; @@ -972,27 +1044,27 @@ int RadioComponentController::throttleChannelRCValue(void) bool RadioComponentController::rollChannelMapped(void) { - return _rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax; + return _rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax(); } bool RadioComponentController::pitchChannelMapped(void) { - return _rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax; + return _rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax(); } bool RadioComponentController::yawChannelMapped(void) { - return _rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax; + return _rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax(); } bool RadioComponentController::throttleChannelMapped(void) { - return _rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax; + return _rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax(); } bool RadioComponentController::rollChannelReversed(void) { - if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionRoll] != _chanMax()) { return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionRoll]].reversed; } else { return false; @@ -1001,7 +1073,7 @@ bool RadioComponentController::rollChannelReversed(void) bool RadioComponentController::pitchChannelReversed(void) { - if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionPitch] != _chanMax()) { return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionPitch]].reversed; } else { return false; @@ -1010,7 +1082,7 @@ bool RadioComponentController::pitchChannelReversed(void) bool RadioComponentController::yawChannelReversed(void) { - if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionYaw] != _chanMax()) { return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionYaw]].reversed; } else { return false; @@ -1019,7 +1091,7 @@ bool RadioComponentController::yawChannelReversed(void) bool RadioComponentController::throttleChannelReversed(void) { - if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax) { + if (_rgFunctionChannelMapping[rcCalFunctionThrottle] != _chanMax()) { return _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionThrottle]].reversed; } else { return false; @@ -1054,3 +1126,18 @@ void RadioComponentController::copyTrims(void) { _uas->startCalibration(UASInterface::StartCalibrationCopyTrims); } + +bool RadioComponentController::_px4Vehicle(void) const +{ + return _vehicle->firmwareType() == MAV_AUTOPILOT_PX4; +} + +const struct RadioComponentController::FunctionInfo* RadioComponentController::_functionInfo(void) const +{ + return _px4Vehicle() ? _rgFunctionInfoPX4 : _rgFunctionInfoAPM; +} + +int RadioComponentController::_chanMax(void) const +{ + return _px4Vehicle() ? _chanMaxPX4 : _chanMaxAPM; +} diff --git a/src/AutoPilotPlugins/PX4/RadioComponentController.h b/src/AutoPilotPlugins/Common/RadioComponentController.h similarity index 91% rename from src/AutoPilotPlugins/PX4/RadioComponentController.h rename to src/AutoPilotPlugins/Common/RadioComponentController.h index 6c48a96fe..20a1f3ef6 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponentController.h +++ b/src/AutoPilotPlugins/Common/RadioComponentController.h @@ -140,9 +140,12 @@ signals: // @brief Signalled when in unit test mode and a message box should be displayed by the next button void nextButtonMessageBoxDisplayed(void); + // Signaled to QML to indicator reboot is required + void functionMappingChangedAPMReboot(void); + private slots: - void _remoteControlChannelRawChanged(int chan, float val); - + void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]); + private: /// @brief These identify the various controls functions. They are also used as indices into the _rgFunctioInfo /// array. @@ -209,7 +212,9 @@ private: int _currentStep; ///< Current step of state machine - const struct stateMachineEntry* _getStateMachineEntry(int step); + const struct stateMachineEntry* _getStateMachineEntry(int step) const; + const struct FunctionInfo* _functionInfo(void) const; + bool _px4Vehicle(void) const; void _advanceState(void); void _setupCurrentState(void); @@ -251,6 +256,8 @@ private: void _storeSettings(void); void _signalAllAttiudeValueChanges(void); + + int _chanMax(void) const; // @brief Called by unit test code to set the mode to unit testing void _setUnitTestMode(void){ _unitTestMode = true; } @@ -279,16 +286,20 @@ private: static const int _updateInterval; ///< Interval for ui update timer - static const struct FunctionInfo _rgFunctionInfo[rcCalFunctionMax]; ///< Information associated with each function. + static const struct FunctionInfo _rgFunctionInfoAPM[rcCalFunctionMax]; ///< Information associated with each function, PX4 firmware + static const struct FunctionInfo _rgFunctionInfoPX4[rcCalFunctionMax]; ///< Information associated with each function, APM firmware + int _rgFunctionChannelMapping[rcCalFunctionMax]; ///< Maps from rcCalFunctions to channel index. _chanMax indicates channel not set for this function. static const int _attitudeControls = 5; int _chanCount; ///< Number of actual rc channels available - static const int _chanMax = 18; ///< Maximum number of supported rc channels - static const int _chanMinimum = 5; ///< Minimum numner of channels required to run PX4 + static const int _chanMaxPX4 = 18; ///< Maximum number of supported rc channels, PX4 Firmware + static const int _chanMaxAPM = 14; ///< Maximum number of supported rc channels, APM firmware + static const int _chanMaxAny = 18; ///< Maximum number of support rc channels by this implementation + static const int _chanMinimum = 5; ///< Minimum numner of channels required to run - struct ChannelInfo _rgChannelInfo[_chanMax]; ///< Information associated with each rc channel + struct ChannelInfo _rgChannelInfo[_chanMaxAny]; ///< Information associated with each rc channel enum rcCalStates _rcCalState; ///< Current calibration state int _rcCalStateCurrentChannel; ///< Current channel being worked on in rcCalStateIdentify and rcCalStateDetectInversion @@ -306,9 +317,9 @@ private: static const int _rcCalSettleDelta; static const int _rcCalMinDelta; - int _rcValueSave[_chanMax]; ///< Saved values prior to detecting channel movement + int _rcValueSave[_chanMaxAny]; ///< Saved values prior to detecting channel movement - int _rcRawValue[_chanMax]; ///< Current set of raw channel values + int _rcRawValue[_chanMaxAny]; ///< Current set of raw channel values int _stickDetectChannel; int _stickDetectInitialValue; diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 014350998..930cda13e 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -99,7 +99,7 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) _airframeComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); - _radioComponent = new RadioComponent(_vehicle, this); + _radioComponent = new PX4RadioComponent(_vehicle, this); Q_CHECK_PTR(_radioComponent); _radioComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h index e23ac9fcd..ebc559fdd 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h @@ -27,7 +27,7 @@ #include "AutoPilotPlugin.h" #include "PX4AirframeLoader.h" #include "AirframeComponent.h" -#include "RadioComponent.h" +#include "PX4RadioComponent.h" #include "FlightModesComponent.h" #include "SensorsComponent.h" #include "SafetyComponent.h" @@ -53,7 +53,7 @@ public: // These methods should only be used by objects within the plugin AirframeComponent* airframeComponent(void) { return _airframeComponent; } - RadioComponent* radioComponent(void) { return _radioComponent; } + PX4RadioComponent* radioComponent(void) { return _radioComponent; } FlightModesComponent* flightModesComponent(void) { return _flightModesComponent; } SensorsComponent* sensorsComponent(void) { return _sensorsComponent; } SafetyComponent* safetyComponent(void) { return _safetyComponent; } @@ -67,7 +67,7 @@ private: PX4AirframeLoader* _airframeFacts; QVariantList _components; AirframeComponent* _airframeComponent; - RadioComponent* _radioComponent; + PX4RadioComponent* _radioComponent; FlightModesComponent* _flightModesComponent; SensorsComponent* _sensorsComponent; SafetyComponent* _safetyComponent; diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.cc b/src/AutoPilotPlugins/PX4/PX4RadioComponent.cc similarity index 77% rename from src/AutoPilotPlugins/PX4/RadioComponent.cc rename to src/AutoPilotPlugins/PX4/PX4RadioComponent.cc index fead1e094..4e34322f0 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.cc +++ b/src/AutoPilotPlugins/PX4/PX4RadioComponent.cc @@ -21,42 +21,38 @@ ======================================================================*/ -/// @file -/// @author Don Gagne - -#include "RadioComponent.h" -#include "QGCQmlWidgetHolder.h" +#include "PX4RadioComponent.h" #include "PX4AutoPilotPlugin.h" -RadioComponent::RadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : +PX4RadioComponent::PX4RadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) : PX4Component(vehicle, autopilot, parent), _name(tr("Radio")) { } -QString RadioComponent::name(void) const +QString PX4RadioComponent::name(void) const { return _name; } -QString RadioComponent::description(void) const +QString PX4RadioComponent::description(void) const { return tr("The Radio Component is used to setup which channels on your RC Transmitter you will use for each vehicle control such as Roll, Pitch, Yaw and Throttle. " "It also allows you to assign switches and dials to the various flight modes. " "Prior to flight you must also calibrate the extents for all of your channels."); } -QString RadioComponent::iconResource(void) const +QString PX4RadioComponent::iconResource(void) const { return "/qmlimages/RadioComponentIcon.png"; } -bool RadioComponent::requiresSetup(void) const +bool PX4RadioComponent::requiresSetup(void) const { return _autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->rawValue().toInt() == 1 ? false : true; } -bool RadioComponent::setupComplete(void) const +bool PX4RadioComponent::setupComplete(void) const { if (_autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->rawValue().toInt() != 1) { // The best we can do to detect the need for a radio calibration is look for attitude @@ -73,7 +69,7 @@ bool RadioComponent::setupComplete(void) const return true; } -QString RadioComponent::setupStateDescription(void) const +QString PX4RadioComponent::setupStateDescription(void) const { const char* stateDescription; @@ -85,7 +81,7 @@ QString RadioComponent::setupStateDescription(void) const return QString(stateDescription); } -QStringList RadioComponent::setupCompleteChangedTriggerList(void) const +QStringList PX4RadioComponent::setupCompleteChangedTriggerList(void) const { QStringList triggers; @@ -94,7 +90,7 @@ QStringList RadioComponent::setupCompleteChangedTriggerList(void) const return triggers; } -QStringList RadioComponent::paramFilterList(void) const +QStringList PX4RadioComponent::paramFilterList(void) const { QStringList list; @@ -103,21 +99,20 @@ QStringList RadioComponent::paramFilterList(void) const return list; } -QUrl RadioComponent::setupSource(void) const +QUrl PX4RadioComponent::setupSource(void) const { return QUrl::fromUserInput("qrc:/qml/RadioComponent.qml"); } -QUrl RadioComponent::summaryQmlSource(void) const +QUrl PX4RadioComponent::summaryQmlSource(void) const { - return QUrl::fromUserInput("qrc:/qml/RadioComponentSummary.qml"); + return QUrl::fromUserInput("qrc:/qml/PX4RadioComponentSummary.qml"); } -QString RadioComponent::prerequisiteSetup(void) const +QString PX4RadioComponent::prerequisiteSetup(void) const { if (_autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->rawValue().toInt() != 1) { PX4AutoPilotPlugin* plugin = dynamic_cast(_autopilot); - Q_ASSERT(plugin); if (!plugin->airframeComponent()->setupComplete()) { return plugin->airframeComponent()->name(); diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.h b/src/AutoPilotPlugins/PX4/PX4RadioComponent.h similarity index 82% rename from src/AutoPilotPlugins/PX4/RadioComponent.h rename to src/AutoPilotPlugins/PX4/PX4RadioComponent.h index 15f718140..3ffc86cc0 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.h +++ b/src/AutoPilotPlugins/PX4/PX4RadioComponent.h @@ -21,22 +21,17 @@ ======================================================================*/ -#ifndef RADIOCOMPONENT_H -#define RADIOCOMPONENT_H +#ifndef PX4RadioComponent_H +#define PX4RadioComponent_H #include "PX4Component.h" -/// @file -/// @brief The Radio VehicleComponent is used to calibrate the trasmitter and assign function mapping -/// to channels. -/// @author Don Gagne - -class RadioComponent : public PX4Component +class PX4RadioComponent : public PX4Component { Q_OBJECT public: - RadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); + PX4RadioComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); // Virtuals from PX4Component virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/PX4/RadioComponentSummary.qml b/src/AutoPilotPlugins/PX4/PX4RadioComponentSummary.qml similarity index 100% rename from src/AutoPilotPlugins/PX4/RadioComponentSummary.qml rename to src/AutoPilotPlugins/PX4/PX4RadioComponentSummary.qml diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 26d878790..ff1b1a841 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -171,6 +171,11 @@ public: // Called to signal app shutdown. Disconnects all links while turning off auto-connect. void shutdown(void); +#ifdef QT_DEBUG + // Only used by unit test tp restart after a shutdown + void restart(void) { setConnectionsAllowed(); } +#endif + /// @return true: specified link is an autoconnect link bool isAutoconnectLink(LinkInterface* link); diff --git a/src/qgcunittest/PX4RCCalibrationTest.cc b/src/qgcunittest/RadioConfigTest.cc similarity index 68% rename from src/qgcunittest/PX4RCCalibrationTest.cc rename to src/qgcunittest/RadioConfigTest.cc index 73dd2f671..6d10c93d1 100644 --- a/src/qgcunittest/PX4RCCalibrationTest.cc +++ b/src/qgcunittest/RadioConfigTest.cc @@ -21,7 +21,7 @@ ======================================================================*/ -#include "PX4RCCalibrationTest.h" +#include "RadioConfigTest.h" #include "RadioComponentController.h" #include "MultiVehicleManager.h" #include "QGCApplication.h" @@ -67,7 +67,7 @@ const int RadioConfigTest::_testMinValue = RadioComponentController::_rcCalPWMDe const int RadioConfigTest::_testMaxValue = RadioComponentController::_rcCalPWMDefaultMaxValue - 10; const int RadioConfigTest::_testCenterValue = RadioConfigTest::_testMinValue + ((RadioConfigTest::_testMaxValue - RadioConfigTest::_testMinValue) / 2); -const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSettings[RadioConfigTest::_availableChannels] = { +const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSettingsPX4[RadioComponentController::_chanMaxPX4] = { // Function Min Max # Reversed // Channel 0 : Not mapped to function, Simulate invalid Min/Max @@ -104,9 +104,9 @@ const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSetting { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, }; -// Note the: 1500/*RadioComponentController::_rcCalPWMCenterPoint*/ entires. For some reason I couldn't get the compiler to do the +// Note the: 1500/*RadioComponentController::_rcCalPWMCenterPoint*/ entries. For some reason I couldn't get the compiler to do the // right thing with the constant. So I just hacked inthe real value instead of fighting with it any longer. -const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSettingsValidate[RadioComponentController::_chanMax] = { +const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSettingsValidatePX4[RadioComponentController::_chanMaxPX4] = { // Function Min Value Max Value Trim Value Reversed // Channels 0: not mapped and should be set to defaults @@ -138,6 +138,62 @@ const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSetting { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, }; +const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSettingsAPM[RadioComponentController::_chanMaxAPM] = { + // Function Min Max # Reversed + + // Channel 0 : Not mapped to function, Simulate invalid Min/Max + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + + // Channels 1-4: Mapped to attitude control function + { RadioComponentController::rcCalFunctionRoll, _testMinValue, _testMaxValue, 0, true }, + { RadioComponentController::rcCalFunctionPitch, _testMinValue, _testMaxValue, 0, false }, + { RadioComponentController::rcCalFunctionYaw, _testMinValue, _testMaxValue, 0, true }, + { RadioComponentController::rcCalFunctionThrottle, _testMinValue, _testMaxValue, 0, false }, + + // Channels 5-11: Not mapped to function, Simulate invalid Min/Max, since available channel Min/Max is still shown. + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testCenterValue, 0, false }, + + // Channel 12 : Not mapped to function, Simulate invalid Min, valid Max + { RadioComponentController::rcCalFunctionMax, _testCenterValue, _testMaxValue, 0, false }, + + // Channel 13 : Not mapped to function, Simulate valid Min, invalid Max + { RadioComponentController::rcCalFunctionMax, _testMinValue, _testCenterValue, 0, false }, +}; + +// Note the: 1500/*RadioComponentController::_rcCalPWMCenterPoint*/ entries. For some reason I couldn't get the compiler to do the +// right thing with the constant. So I just hacked inthe real value instead of fighting with it any longer. +const struct RadioConfigTest::ChannelSettings RadioConfigTest::_rgChannelSettingsValidateAPM[RadioComponentController::_chanMaxAPM] = { + // Function Min Value Max Value Trim Value Reversed + + // Channels 0: not mapped and should be set to defaults + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + + // Channels 1-4: Mapped to attitude control function + { RadioComponentController::rcCalFunctionRoll, _testMinValue, _testMaxValue, _testCenterValue, true }, + { RadioComponentController::rcCalFunctionPitch, _testMinValue, _testMaxValue, _testCenterValue, false }, + { RadioComponentController::rcCalFunctionYaw, _testMinValue, _testMaxValue, _testCenterValue, true }, + { RadioComponentController::rcCalFunctionThrottle, _testMinValue, _testMaxValue, _testMinValue, false }, + + // Channels 5-11: not mapped and should be set to defaults + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + + // Channels 12-13 are not mapped and should be set to defaults + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, + { RadioComponentController::rcCalFunctionMax, RadioComponentController::_rcCalPWMDefaultMinValue, RadioComponentController::_rcCalPWMDefaultMaxValue, 1500/*RadioComponentController::_rcCalPWMCenterPoint*/, false }, +}; + RadioConfigTest::RadioConfigTest(void) : _calWidget(NULL), _controller(NULL) @@ -145,11 +201,9 @@ RadioConfigTest::RadioConfigTest(void) : } -void RadioConfigTest::init(void) +void RadioConfigTest::_init(MAV_AUTOPILOT firmwareType) { - UnitTest::init(); - - _connectMockLink(); + _connectMockLink(firmwareType); _autopilot = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->autopilotPlugin(); Q_ASSERT(_autopilot); @@ -186,43 +240,6 @@ void RadioConfigTest::cleanup(void) UnitTest::cleanup(); } -/// @brief Test for correct behavior in determining minimum numbers of channels for flight. -void RadioConfigTest::_minRCChannels_test(void) -{ - // Next button won't be enabled until we see the minimum number of channels. - for (int chan=0; chanemitRemoteControlChannelRawChanged(chan, (float)RadioComponentController::_rcCalPWMCenterPoint); - - // We use _chanCount internally so we should validate it - QCOMPARE(_controller->_chanCount, chan+1); - - // Validate Next button state - _controller->nextButtonClicked(); - bool signalFound = _multiSpyNextButtonMessageBox->waitForSignalByIndex(0, 200); - if (chan == RadioComponentController::_chanMinimum - 1) { - // Last channel should trigger Calibration available - QCOMPARE(signalFound, false); - QCOMPARE(_controller->_currentStep, 0); - } else { - // Still less than the minimum channels. Next button should show message box. Calibration should not start. - QCOMPARE(signalFound, true); - QCOMPARE(_controller->_currentStep, -1); - } - _multiSpyNextButtonMessageBox->clearAllSignals(); - - // The following test code no longer works since view update doesn't happens until parameters are received. - // Leaving code here because RC Cal could be restructured to handle this case at some point. -#if 0 - // Only available channels should have visible widget. A ui update cycle needs to have passed so we wait a little. - QTest::qWait(RadioComponentController::_updateInterval * 2); - for (int chanWidget=0; chanWidgetobjectName() << chanWidget << chan; - QCOMPARE(_rgValueWidget[chanWidget]->isVisible(), !!(chanWidget <= chan)); - } -#endif - } -} - void RadioConfigTest::_beginCalibration(void) { CHK_BUTTONS(nextButtonMask | cancelButtonMask); @@ -261,13 +278,13 @@ void RadioConfigTest::_stickMoveAutoStep(const char* functionStr, enum RadioComp int channel = _rgFunctionChannelMap[function]; int saveStep = _controller->_currentStep; - bool reversed = _rgChannelSettings[channel].reversed; + bool reversed = _channelSettings()[channel].reversed; if (!identifyStep && direction != moveToCenter) { // We have already identified the function channel mapping. Move other channels around to make sure there is no impact. int otherChannel = channel + 1; - if (otherChannel >= RadioComponentController::_chanMax) { + if (otherChannel >= _chanMax()) { otherChannel = 0; } @@ -306,9 +323,9 @@ void RadioConfigTest::_switchMinMaxStep(void) _mockLink->emitRemoteControlChannelRawChanged(0, (float)(RadioComponentController::_rcCalPWMValidMaxValue - 1)); // Send min/max values switch channels - for (int chan=0; chan<_availableChannels; chan++) { - _mockLink->emitRemoteControlChannelRawChanged(chan, _rgChannelSettings[chan].rcMin); - _mockLink->emitRemoteControlChannelRawChanged(chan, _rgChannelSettings[chan].rcMax); + for (int chan=0; chan<_chanMax(); chan++) { + _mockLink->emitRemoteControlChannelRawChanged(chan, _channelSettings()[chan].rcMin); + _mockLink->emitRemoteControlChannelRawChanged(chan, _channelSettings()[chan].rcMax); } _channelHomePosition(); @@ -325,7 +342,7 @@ void RadioConfigTest::_flapsDetectStep(void) qCDebug(RadioConfigTestLog) << "_flapsDetectStep channel" << channel; // Test code can't handle reversed flaps channel - Q_ASSERT(!_rgChannelSettings[channel].reversed); + Q_ASSERT(!_channelSettings()[channel].reversed); CHK_BUTTONS(nextButtonMask | cancelButtonMask | skipButtonMask); @@ -367,8 +384,10 @@ void RadioConfigTest::_switchSelectAutoStep(const char* functionStr, RadioCompon QCOMPARE(_controller->_currentStep, saveStep + 1); } -void RadioConfigTest::_fullCalibration_test(void) +void RadioConfigTest::_fullCalibrationWorker(MAV_AUTOPILOT firmwareType) { + _init(firmwareType); + // IMPORTANT NOTE: We used channels 1-5 for attitude mapping in the test below. // MockLink.params file cannot have flight mode switches mapped to those channels. // If it does it will cause errors since the stick will not be detetected where @@ -380,16 +399,17 @@ void RadioConfigTest::_fullCalibration_test(void) bool found = false; // If we are mapping this function during cal set it into _rgFunctionChannelMap - for (int channel=0; channelgetParameterFact(FactSystem::defaultComponentId, switchParam)->rawValue().toInt() != channel + 1); + if (_px4Vehicle()) { + // Make sure this function isn't being use for a switch + QStringList switchList; + switchList << "RC_MAP_MODE_SW" << "RC_MAP_LOITER_SW" << "RC_MAP_RETURN_SW" << "RC_MAP_POSCTL_SW" << "RC_MAP_ACRO_SW"; + + foreach (QString switchParam, switchList) { + Q_ASSERT(_autopilot->getParameterFact(FactSystem::defaultComponentId, switchParam)->rawValue().toInt() != channel + 1); + } } _rgFunctionChannelMap[function] = channel; @@ -401,12 +421,17 @@ void RadioConfigTest::_fullCalibration_test(void) // If we aren't mapping this function during calibration, set it to the previous setting if (!found) { - _rgFunctionChannelMap[function] = _autopilot->getParameterFact(FactSystem::defaultComponentId, RadioComponentController::_rgFunctionInfo[function].parameterName)->rawValue().toInt(); - qCDebug(RadioConfigTestLog) << "Assigning switch" << function << _rgFunctionChannelMap[function]; - if (_rgFunctionChannelMap[function] == 0) { - _rgFunctionChannelMap[function] = -1; // -1 signals no mapping + const char* paramName = _functionInfo()[function].parameterName; + if (paramName) { + _rgFunctionChannelMap[function] = _autopilot->getParameterFact(FactSystem::defaultComponentId, paramName)->rawValue().toInt(); + qCDebug(RadioConfigTestLog) << "Assigning switch" << function << _rgFunctionChannelMap[function]; + if (_rgFunctionChannelMap[function] == 0) { + _rgFunctionChannelMap[function] = -1; // -1 signals no mapping + } else { + _rgFunctionChannelMap[function]--; // parameter is 1-based, _rgFunctionChannelMap is not + } } else { - _rgFunctionChannelMap[function]--; // parameter is 1-based, _rgFunctionChannelMap is not + _rgFunctionChannelMap[function] = -1; // -1 signals no mapping } } } @@ -424,21 +449,34 @@ void RadioConfigTest::_fullCalibration_test(void) _stickMoveAutoStep("Pitch", RadioComponentController::rcCalFunctionPitch, moveToMin, false /* not identify step */); _stickMoveAutoStep("Pitch", RadioComponentController::rcCalFunctionPitch, moveToCenter, false /* not identify step */); _switchMinMaxStep(); - _flapsDetectStep(); - _stickMoveAutoStep("Flaps", RadioComponentController::rcCalFunctionFlaps, moveToMin, false /* not identify step */); - _switchSelectAutoStep("Aux1", RadioComponentController::rcCalFunctionAux1); - _switchSelectAutoStep("Aux2", RadioComponentController::rcCalFunctionAux2); + if (firmwareType == MAV_AUTOPILOT_PX4) { + _flapsDetectStep(); + _stickMoveAutoStep("Flaps", RadioComponentController::rcCalFunctionFlaps, moveToMin, false /* not identify step */); + _switchSelectAutoStep("Aux1", RadioComponentController::rcCalFunctionAux1); + _switchSelectAutoStep("Aux2", RadioComponentController::rcCalFunctionAux2); + } // One more click and the parameters should get saved _controller->nextButtonClicked(); _validateParameters(); } +void RadioConfigTest::_fullCalibration_px4_test(void) +{ + _fullCalibrationWorker(MAV_AUTOPILOT_PX4); +} + + +void RadioConfigTest::_fullCalibration_apm_test(void) +{ + _fullCalibrationWorker(MAV_AUTOPILOT_ARDUPILOTMEGA); +} + /// @brief Sets rc input to Throttle down home position. Centers all other channels. void RadioConfigTest::_channelHomePosition(void) { // Initialize available channels to center point. - for (int i=0; i<_availableChannels; i++) { + for (int i=0; i<_chanMax(); i++) { _mockLink->emitRemoteControlChannelRawChanged(i, (float)RadioComponentController::_rcCalPWMCenterPoint); } @@ -465,20 +503,23 @@ void RadioConfigTest::_validateParameters(void) expectedParameterValue = chanIndex + 1; // 1-based parameter value } - qCDebug(RadioConfigTestLog) << "Validate" << chanFunction << _autopilot->getParameterFact(FactSystem::defaultComponentId, RadioComponentController::_rgFunctionInfo[chanFunction].parameterName)->rawValue().toInt(); - QCOMPARE(_autopilot->getParameterFact(FactSystem::defaultComponentId, RadioComponentController::_rgFunctionInfo[chanFunction].parameterName)->rawValue().toInt(), expectedParameterValue); + const char* paramName = _functionInfo()[chanFunction].parameterName; + if (paramName) { + qCDebug(RadioConfigTestLog) << "Validate" << chanFunction << _autopilot->getParameterFact(FactSystem::defaultComponentId, paramName)->rawValue().toInt(); + QCOMPARE(_autopilot->getParameterFact(FactSystem::defaultComponentId, paramName)->rawValue().toInt(), expectedParameterValue); + } } // Validate the channel settings. Note the channels are 1-based in parameter names. - for (int chan = 0; changetParameterFact(FactSystem::defaultComponentId, minTpl.arg(oneBasedChannel))->rawValue().toInt(&convertOk); QCOMPARE(convertOk, true); @@ -507,7 +548,35 @@ void RadioConfigTest::_validateParameters(void) } else { expectedValue = _rgFunctionChannelMap[chanFunction] + 1; // 1-based } - // qCDebug(RadioConfigTestLog) << chanFunction << expectedValue << mapParamsSet[RadioComponentController::_rgFunctionInfo[chanFunction].parameterName].toInt(); - QCOMPARE(_autopilot->getParameterFact(FactSystem::defaultComponentId, RadioComponentController::_rgFunctionInfo[chanFunction].parameterName)->rawValue().toInt(), expectedValue); + const char* paramName = _functionInfo()[chanFunction].parameterName; + if (paramName) { + // qCDebug(RadioConfigTestLog) << chanFunction << expectedValue << mapParamsSet[paramName].toInt(); + QCOMPARE(_autopilot->getParameterFact(FactSystem::defaultComponentId, paramName)->rawValue().toInt(), expectedValue); + } } } + +bool RadioConfigTest::_px4Vehicle(void) const +{ + return _vehicle->firmwareType() == MAV_AUTOPILOT_PX4; +} + +const struct RadioConfigTest::ChannelSettings* RadioConfigTest::_channelSettings(void) const +{ + return _px4Vehicle() ? _rgChannelSettingsPX4 : _rgChannelSettingsAPM; +} + +const struct RadioConfigTest::ChannelSettings* RadioConfigTest::_channelSettingsValidate(void) const +{ + return _px4Vehicle() ? _rgChannelSettingsValidatePX4 : _rgChannelSettingsValidateAPM; +} + +const struct RadioComponentController::FunctionInfo* RadioConfigTest::_functionInfo(void) const +{ + return _px4Vehicle() ? RadioComponentController::_rgFunctionInfoPX4 : RadioComponentController::_rgFunctionInfoAPM; +} + +int RadioConfigTest::_chanMax(void) const +{ + return _px4Vehicle() ? RadioComponentController::_chanMaxPX4 : RadioComponentController::_chanMaxAPM; +} diff --git a/src/qgcunittest/PX4RCCalibrationTest.h b/src/qgcunittest/RadioConfigTest.h similarity index 80% rename from src/qgcunittest/PX4RCCalibrationTest.h rename to src/qgcunittest/RadioConfigTest.h index 86517bbe1..879b88fcd 100644 --- a/src/qgcunittest/PX4RCCalibrationTest.h +++ b/src/qgcunittest/RadioConfigTest.h @@ -47,12 +47,11 @@ public: RadioConfigTest(void); private slots: - void init(void); void cleanup(void); - void _minRCChannels_test(void); - void _fullCalibration_test(void); - + void _fullCalibration_px4_test(void); + void _fullCalibration_apm_test(void); + private: /// @brief Modes to run worker functions enum TestMode { @@ -67,15 +66,6 @@ private: moveToCenter, }; - void _channelHomePosition(void); - void _minRCChannels(void); - void _beginCalibration(void); - void _stickMoveWaitForSettle(int channel, int value); - void _stickMoveAutoStep(const char* functionStr, enum RadioComponentController::rcCalFunctions function, enum MoveToDirection direction, bool identifyStep); - void _switchMinMaxStep(void); - void _flapsDetectStep(void); - void _switchSelectAutoStep(const char* functionStr, RadioComponentController::rcCalFunctions function); - enum { validateMinMaxMask = 1 << 0, validateTrimsMask = 1 << 1, @@ -85,12 +75,28 @@ private: }; struct ChannelSettings { - int function; + int function; int rcMin; int rcMax; int rcTrim; int reversed; }; + + void _init(MAV_AUTOPILOT firmwareType); + void _channelHomePosition(void); + void _minRCChannels(void); + void _beginCalibration(void); + void _stickMoveWaitForSettle(int channel, int value); + void _stickMoveAutoStep(const char* functionStr, enum RadioComponentController::rcCalFunctions function, enum MoveToDirection direction, bool identifyStep); + void _switchMinMaxStep(void); + void _flapsDetectStep(void); + void _switchSelectAutoStep(const char* functionStr, RadioComponentController::rcCalFunctions function); + bool _px4Vehicle(void) const; + const struct RadioComponentController::FunctionInfo* _functionInfo(void) const; + const struct ChannelSettings* _channelSettings(void) const; + const struct ChannelSettings* _channelSettingsValidate(void) const; + void _fullCalibrationWorker(MAV_AUTOPILOT firmwareType); + int _chanMax(void) const; void _validateParameters(void); @@ -113,12 +119,13 @@ private: static const int _testMaxValue; static const int _testCenterValue; - static const int _availableChannels = 18; ///< Simulate 18 channel RC Transmitter static const int _stickSettleWait; - static const struct ChannelSettings _rgChannelSettings[_availableChannels]; - static const struct ChannelSettings _rgChannelSettingsValidate[RadioComponentController::_chanMax]; - + static const struct ChannelSettings _rgChannelSettingsPX4[RadioComponentController::_chanMaxPX4]; + static const struct ChannelSettings _rgChannelSettingsAPM[RadioComponentController::_chanMaxAPM]; + static const struct ChannelSettings _rgChannelSettingsValidatePX4[RadioComponentController::_chanMaxPX4]; + static const struct ChannelSettings _rgChannelSettingsValidateAPM[RadioComponentController::_chanMaxAPM]; + int _rgFunctionChannelMap[RadioComponentController::rcCalFunctionMax]; RadioComponentController* _controller; -- 2.22.0