diff --git a/QGCApplication.pro b/QGCApplication.pro
index e3de2ee522f3ece14e0005b9b158d6841cb00d79..4fccc3bb648ad8ef38f4aa3f3c51943385642fd6 100644
--- a/QGCApplication.pro
+++ b/QGCApplication.pro
@@ -142,6 +142,7 @@ INCLUDEPATH += \
src/comm \
src/FlightDisplay \
src/input \
+ src/Joystick \
src/lib/qmapcontrol \
src/QmlControls \
src/uas \
@@ -214,9 +215,6 @@ FORMS += \
!MobileBuild {
FORMS += \
- src/ui/JoystickButton.ui \
- src/ui/JoystickAxis.ui \
- src/ui/JoystickWidget.ui \
src/ui/QGCHilConfiguration.ui \
src/ui/QGCHilFlightGearConfiguration.ui \
src/ui/QGCHilJSBSimConfiguration.ui \
@@ -242,6 +240,8 @@ HEADERS += \
src/FlightDisplay/FlightDisplayView.h \
src/GAudioOutput.h \
src/HomePositionManager.h \
+ src/Joystick/Joystick.h \
+ src/Joystick/JoystickManager.h \
src/LogCompressor.h \
src/MG.h \
src/QGC.h \
@@ -352,15 +352,12 @@ HEADERS += \
src/comm/QGCHilLink.h \
src/comm/QGCJSBSimLink.h \
src/comm/QGCXPlaneLink.h \
- src/input/JoystickInput.h \
src/ui/CameraView.h \
- src/ui/JoystickAxis.h \
- src/ui/JoystickButton.h \
- src/ui/JoystickWidget.h \
src/ui/QGCHilConfiguration.h \
src/ui/QGCHilFlightGearConfiguration.h \
src/ui/QGCHilJSBSimConfiguration.h \
src/ui/QGCHilXPlaneConfiguration.h \
+ src/VehicleSetup/JoystickConfigController.h \
}
SOURCES += \
@@ -379,6 +376,8 @@ SOURCES += \
src/FlightDisplay/FlightDisplayView.cc \
src/GAudioOutput.cc \
src/HomePositionManager.cc \
+ src/Joystick/Joystick.cc \
+ src/Joystick/JoystickManager.cc \
src/LogCompressor.cc \
src/main.cc \
src/QGC.cc \
@@ -482,15 +481,12 @@ SOURCES += \
src/comm/QGCFlightGearLink.cc \
src/comm/QGCJSBSimLink.cc \
src/comm/QGCXPlaneLink.cc \
- src/input/JoystickInput.cc \
src/ui/CameraView.cc \
- src/ui/JoystickAxis.cc \
- src/ui/JoystickButton.cc \
- src/ui/JoystickWidget.cc \
src/ui/QGCHilConfiguration.cc \
src/ui/QGCHilFlightGearConfiguration.cc \
src/ui/QGCHilJSBSimConfiguration.cc \
src/ui/QGCHilXPlaneConfiguration.cc \
+ src/VehicleSetup/JoystickConfigController.cc \
}
#
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 36df1d2193acb75ea1abdeb5b04afc63cad728b3..1dce5b3d04a51587b70f3e416a305445c18980a7 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -99,15 +99,16 @@
src/QmlControls/MissionItemIndexLabel.qml
src/QmlControls/MissionItemSummary.qml
- src/QmlControls/ScreenToolsFontQuery.qml
- src/ViewWidgets/ParameterEditorWidget.qml
- src/ViewWidgets/CustomCommandWidget.qml
+
src/VehicleSetup/SetupView.qml
- src/VehicleSetup/SetupViewButtonsConnected.qml
- src/VehicleSetup/SetupViewButtonsDisconnected.qml
src/VehicleSetup/VehicleSummary.qml
src/VehicleSetup/FirmwareUpgrade.qml
+ src/VehicleSetup/JoystickConfig.qml
src/VehicleSetup/SetupParameterEditor.qml
+
+ src/QmlControls/ScreenToolsFontQuery.qml
+ src/ViewWidgets/ParameterEditorWidget.qml
+ src/ViewWidgets/CustomCommandWidget.qml
src/AutoPilotPlugins/PX4/SafetyComponent.qml
src/AutoPilotPlugins/PX4/RadioComponent.qml
src/AutoPilotPlugins/PX4/PowerComponent.qml
@@ -222,6 +223,7 @@
resources/calibration/accel_up.png
resources/calibration/accel_left.png
+
resources/calibration/mode1/radioCenter.png
resources/calibration/mode1/radioHome.png
@@ -235,6 +237,7 @@
resources/calibration/mode1/radioThrottleDown.png
resources/calibration/mode1/radioSwitchMinMax.png
+
resources/calibration/mode2/radioCenter.png
resources/calibration/mode2/radioHome.png
@@ -248,6 +251,19 @@
resources/calibration/mode2/radioThrottleDown.png
resources/calibration/mode2/radioSwitchMinMax.png
+
+
+ resources/calibration/joystick/joystickCenter.png
+ resources/calibration/joystick/joystickRollLeft.png
+ resources/calibration/joystick/joystickRollRight.png
+ resources/calibration/joystick/joystickPitchUp.png
+ resources/calibration/joystick/joystickPitchDown.png
+ resources/calibration/joystick/joystickYawLeft.png
+ resources/calibration/joystick/joystickYawRight.png
+ resources/calibration/joystick/joystickThrottleUp.png
+ resources/calibration/joystick/joystickThrottleDown.png
+
+
resources/styles/style-dark.css
resources/styles/style-light.css
diff --git a/resources/calibration/joystick/joystickCenter.png b/resources/calibration/joystick/joystickCenter.png
new file mode 100644
index 0000000000000000000000000000000000000000..556321ec4b1c3142dc40acaf8068b4fb57e923a5
Binary files /dev/null and b/resources/calibration/joystick/joystickCenter.png differ
diff --git a/resources/calibration/joystick/joystickPitchDown.png b/resources/calibration/joystick/joystickPitchDown.png
new file mode 100644
index 0000000000000000000000000000000000000000..6e895b1a5a50dac35d977dd496a830cd26b1a5d8
Binary files /dev/null and b/resources/calibration/joystick/joystickPitchDown.png differ
diff --git a/resources/calibration/joystick/joystickPitchUp.png b/resources/calibration/joystick/joystickPitchUp.png
new file mode 100644
index 0000000000000000000000000000000000000000..ad9d7c03fa5dc49d5889eedf2551f900381c11fc
Binary files /dev/null and b/resources/calibration/joystick/joystickPitchUp.png differ
diff --git a/resources/calibration/joystick/joystickRollLeft.png b/resources/calibration/joystick/joystickRollLeft.png
new file mode 100644
index 0000000000000000000000000000000000000000..fddc7dfb4217051af0ca1b51897f82ef7cd2c810
Binary files /dev/null and b/resources/calibration/joystick/joystickRollLeft.png differ
diff --git a/resources/calibration/joystick/joystickRollRight.png b/resources/calibration/joystick/joystickRollRight.png
new file mode 100644
index 0000000000000000000000000000000000000000..9556eed1e7e62df8c0ffe5e0206ad03db6c6f943
Binary files /dev/null and b/resources/calibration/joystick/joystickRollRight.png differ
diff --git a/resources/calibration/joystick/joystickThrottleDown.png b/resources/calibration/joystick/joystickThrottleDown.png
new file mode 100644
index 0000000000000000000000000000000000000000..7436477b0acd29f42758d77df5978cb955785d6a
Binary files /dev/null and b/resources/calibration/joystick/joystickThrottleDown.png differ
diff --git a/resources/calibration/joystick/joystickThrottleUp.png b/resources/calibration/joystick/joystickThrottleUp.png
new file mode 100644
index 0000000000000000000000000000000000000000..223199991fc90272b1fae1ba6f28799b31681ca2
Binary files /dev/null and b/resources/calibration/joystick/joystickThrottleUp.png differ
diff --git a/resources/calibration/joystick/joystickYawLeft.png b/resources/calibration/joystick/joystickYawLeft.png
new file mode 100644
index 0000000000000000000000000000000000000000..788a09c0b8c504db6f6c1068dd408cf8f39d7554
Binary files /dev/null and b/resources/calibration/joystick/joystickYawLeft.png differ
diff --git a/resources/calibration/joystick/joystickYawRight.png b/resources/calibration/joystick/joystickYawRight.png
new file mode 100644
index 0000000000000000000000000000000000000000..381108825e10adacbbc729dd9ea1ea22b8665f87
Binary files /dev/null and b/resources/calibration/joystick/joystickYawRight.png differ
diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.qml b/src/AutoPilotPlugins/PX4/RadioComponent.qml
index 1f9e233f36155a97ca06198efc11ea50e778b112..cacafa4da8b5311cb391d4efc203065e26163055 100644
--- a/src/AutoPilotPlugins/PX4/RadioComponent.qml
+++ b/src/AutoPilotPlugins/PX4/RadioComponent.qml
@@ -50,6 +50,9 @@ QGCView {
function updateChannelCount()
{
if (controllerAndViewReady) {
+ if (joystickManager.activeJoystick && joystickManager.activeJoystick.enabled) {
+ 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
@@ -128,6 +131,14 @@ QGCView {
}
}
+ Component {
+ id: joystickEnabledDialogComponent
+
+ QGCViewMessage {
+ message: "Radio Config is disabled since you have a Joystick enabled."
+ }
+ }
+
Component {
id: spektrumBindDialogComponent
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index ffa423d56d7136f0a7e001588ded423d863e9886..d718fc8e7c00aa794a40fad579d113404b5ed906 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -76,6 +76,12 @@ public:
/// @param[out] custom_mode Custom mode for SET_MODE mavlink message
virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) = 0;
+ /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
+ /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
+ /// The remainder can be assigned to Vehicle actions.
+ /// @return -1: reserver all buttons, >0 number of buttons to reserve
+ virtual int manualControlReservedButtonCount(void) = 0;
+
protected:
FirmwarePlugin(QObject* parent = NULL) : QGCSingleton(parent) { }
};
diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc
index 997e3f9b269e1710147bb860dd0fb637eaa12279..cb47d987290d059ccf5b7cb9984b8b493ff5d149 100644
--- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc
@@ -89,3 +89,10 @@ bool GenericFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* ba
return false;
}
+
+int GenericFirmwarePlugin::manualControlReservedButtonCount(void)
+{
+ // We don't know whether the firmware is going to used any of these buttons.
+ // So reserve them all.
+ return -1;
+}
diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h
index c0fcf061000e1d130f817598db31eee27c8824dd..109e14ba4da4815495a99da5bb9072d7b19cad66 100644
--- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h
+++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h
@@ -43,6 +43,7 @@ public:
virtual QStringList flightModes(void) { return QStringList(); }
virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
+ virtual int manualControlReservedButtonCount(void);
private:
/// All access to singleton is through AutoPilotPluginManager::instance
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index b3ff047378648ca156b761d87200740fd365dbca..f8d3aa966afcb8573b60fa11918f90461b700620 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -176,3 +176,8 @@ bool PX4FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_m
return found;
}
+
+int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
+{
+ return 8; // 8 buttons reserved for rc switch simulation
+}
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
index d6c710cc27a7eaf8c98aeaec30be06f786374364..ea3314895bf70cc39297e1bf5b042a4bfc149d92 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
@@ -43,6 +43,7 @@ public:
virtual QStringList flightModes(void);
virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
+ virtual int manualControlReservedButtonCount(void);
private:
/// All access to singleton is through AutoPilotPluginManager::instance
diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc
new file mode 100644
index 0000000000000000000000000000000000000000..22a7361858e9cd259ddd1821160be0e16257c585
--- /dev/null
+++ b/src/Joystick/Joystick.cc
@@ -0,0 +1,506 @@
+/*=====================================================================
+
+ 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 "Joystick.h"
+#include "QGC.h"
+#include "MultiVehicleManager.h"
+#include "AutoPilotPlugin.h"
+#include "UAS.h"
+
+#include
+
+#ifdef Q_OS_MAC
+ #include
+#else
+ #include
+#endif
+
+QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog")
+
+const char* Joystick::_settingsGroup = "Joysticks";
+const char* Joystick::_calibratedSettingsKey = "Calibrated";
+const char* Joystick::_buttonActionSettingsKey = "ButtonAction%1";
+const char* Joystick::_throttleModeSettingsKey = "ThrottleMode";
+const char* Joystick::_enabledSettingsKey = "Enabled";
+
+const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
+ "RollAxis",
+ "PitchAxis",
+ "YawAxis",
+ "ThrottleAxis"
+};
+
+Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlIndex)
+ : _sdlIndex(sdlIndex)
+ , _exitThread(false)
+ , _name(name)
+ , _enabled(false)
+ , _calibrated(false)
+ , _calibrating(false)
+ , _axisCount(axisCount)
+ , _buttonCount(buttonCount)
+ , _lastButtonBits(0)
+ , _throttleMode(ThrottleModeCenterZero)
+{
+ for (int i=0; i<_cAxes; i++) {
+ _rgAxisValues[i] = 0;
+ }
+ for (int i=0; i<_cButtons; i++) {
+ _rgButtonValues[i] = false;
+ _rgButtonActions[i] = -1;
+ }
+
+ _loadSettings();
+}
+
+Joystick::~Joystick()
+{
+
+}
+
+void Joystick::_loadSettings(void)
+{
+ QSettings settings;
+
+ settings.beginGroup(_settingsGroup);
+ settings.beginGroup(_name);
+
+ bool badSettings = false;
+ bool convertOk;
+
+ qCDebug(JoystickLog) << "_loadSettings " << _name;
+
+ _calibrated = settings.value(_calibratedSettingsKey, false).toBool();
+ _enabled = settings.value(_enabledSettingsKey, false).toBool();
+
+ _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk);
+ badSettings |= !convertOk;
+
+ qCDebug(JoystickLog) << "_loadSettings calibrated:enabled:throttlemode:badsettings" << _calibrated << _enabled << _throttleMode << badSettings;
+
+ QString minTpl ("Axis%1Min");
+ QString maxTpl ("Axis%1Max");
+ QString trimTpl ("Axis%1Trim");
+ QString revTpl ("Axis%1Rev");
+
+ for (int axis=0; axis<_cAxes; axis++) {
+ Calibration_t* calibration = &_rgCalibration[axis];
+
+ calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk);
+ badSettings |= !convertOk;
+
+ calibration->min = settings.value(minTpl.arg(axis), -32768).toInt(&convertOk);
+ badSettings |= !convertOk;
+
+ calibration->max = settings.value(maxTpl.arg(axis), 32768).toInt(&convertOk);
+ badSettings |= !convertOk;
+
+ calibration->reversed = settings.value(revTpl.arg(axis), false).toBool();
+
+ qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << badSettings;
+ }
+
+ for (int function=0; functioncenter);
+ settings.setValue(minTpl.arg(axis), calibration->min);
+ settings.setValue(maxTpl.arg(axis), calibration->max);
+ settings.setValue(revTpl.arg(axis), calibration->reversed);
+
+ qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed"
+ << _name
+ << axis
+ << calibration->min
+ << calibration->max
+ << calibration->center
+ << calibration->reversed;
+ }
+
+ for (int function=0; function calibration.center) {
+ axisBasis = 1.0f;
+ valueNormalized = value - calibration.center;
+ axisLength = calibration.max - calibration.center;
+ } else {
+ axisBasis = -1.0f;
+ valueNormalized = calibration.center - value;
+ axisLength = calibration.center - calibration.min;
+ }
+
+ float axisPercent = valueNormalized / axisLength;
+
+ float correctedValue = axisBasis * axisPercent;
+
+ if (calibration.reversed) {
+ correctedValue *= -1.0f;
+ }
+
+#if 0
+ qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:basis:normalized:length"
+ << correctedValue
+ << value
+ << calibration.min
+ << calibration.max
+ << calibration.center
+ << calibration.center
+ << axisBasis
+ << valueNormalized
+ << axisLength;
+#endif
+
+ return correctedValue;
+}
+
+
+void Joystick::run(void)
+{
+ SDL_Joystick* sdlJoystick = SDL_JoystickOpen(_sdlIndex);
+ Vehicle * activeVehicle = MultiVehicleManager::instance()->activeVehicle();
+
+ if (!sdlJoystick) {
+ qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError();
+ return;
+ }
+
+ while (!_exitThread) {
+ SDL_JoystickUpdate();
+
+ // Update axes
+ for (int axisIndex=0; axisIndex<_axisCount; axisIndex++) {
+ int newAxisValue = SDL_JoystickGetAxis(sdlJoystick, axisIndex);
+ // Calibration code requires signal to be emitted even if value hasn't changed
+ _rgAxisValues[axisIndex] = newAxisValue;
+ emit rawAxisValueChanged(axisIndex, newAxisValue);
+ }
+
+ // Update buttons
+ for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) {
+ bool newButtonValue = !!SDL_JoystickGetButton(sdlJoystick, buttonIndex);
+ if (newButtonValue != _rgButtonValues[buttonIndex]) {
+ _rgButtonValues[buttonIndex] = newButtonValue;
+ emit rawButtonPressedChanged(buttonIndex, newButtonValue);
+ }
+ }
+
+ if (_calibrated && _enabled && !_calibrating) {
+ int axis = _rgFunctionAxis[rollFunction];
+ float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
+
+ axis = _rgFunctionAxis[pitchFunction];
+ float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
+
+ axis = _rgFunctionAxis[yawFunction];
+ float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
+
+ axis = _rgFunctionAxis[throttleFunction];
+ float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
+
+ roll = std::max(-1.0f, std::min(roll, 1.0f));
+ pitch = std::max(-1.0f, std::min(pitch, 1.0f));
+ yaw = std::max(-1.0f, std::min(yaw, 1.0f));
+ throttle = std::max(-1.0f, std::min(throttle, 1.0f));
+
+ // Adjust throttle to 0:1 range
+ if (_throttleMode == ThrottleModeCenterZero) {
+ throttle = std::max(0.0f, throttle);
+ throttle = (throttle * 2.0f) - 1.0f;
+ } else {
+ throttle = (throttle + 1.0f) / 2.0f;
+ }
+
+ // Set up button pressed information
+
+ // We only send the buttons the firmwware has reserved
+ int reservedButtonCount = activeVehicle->manualControlReservedButtonCount();
+ if (reservedButtonCount == -1) {
+ reservedButtonCount = _buttonCount;
+ }
+
+ quint16 newButtonBits = 0; // New set of button which are down
+ quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal
+
+ for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) {
+ quint16 buttonBit = 1 << buttonIndex;
+
+ if (_rgButtonValues[buttonIndex]) {
+ // Button pressed down, just record it
+ newButtonBits |= buttonBit;
+ } else {
+ if (_lastButtonBits & buttonBit) {
+ // Button was down last time through, but is now up which indicates a button press
+ qCDebug(JoystickLog) << "button triggered" << buttonIndex;
+
+
+ if (buttonIndex >= reservedButtonCount) {
+ // Button is above firmware reserved set
+ int buttonAction =_rgButtonActions[buttonIndex];
+ if (buttonAction != -1) {
+ qCDebug(JoystickLog) << "buttonActionTriggered" << buttonAction;
+ emit buttonActionTriggered(buttonAction);
+ }
+ } else {
+ // Button is within firmware reserved set
+ // Record the button press for manualControl signal
+ buttonPressedBits |= buttonBit;
+ qCDebug(JoystickLog) << "button press recorded for manualControl" << buttonIndex;
+ }
+ }
+ }
+ }
+
+ _lastButtonBits = newButtonBits;
+
+ emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, activeVehicle->joystickMode());
+ }
+
+ // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms)
+ QGC::SLEEP::msleep(40);
+ }
+
+ SDL_JoystickClose(sdlJoystick);
+}
+
+void Joystick::startPolling(void)
+{
+ if (enabled()) {
+ UAS* uas = MultiVehicleManager::instance()->activeVehicle()->uas();
+
+ connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
+ connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
+ }
+
+ _exitThread = false;
+ start();
+}
+
+void Joystick::stopPolling(void)
+{
+ UAS* uas = MultiVehicleManager::instance()->activeVehicle()->uas();
+
+ disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
+ disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
+
+ _exitThread = true;
+}
+
+void Joystick::setCalibration(int axis, Calibration_t& calibration)
+{
+ if (axis < 0 || axis > _cAxes) {
+ qCWarning(JoystickLog) << "Invalid axis index" << axis;
+ return;
+ }
+
+ _calibrated = true;
+ _rgCalibration[axis] = calibration;
+ _saveSettings();
+ emit calibratedChanged(_calibrated);
+}
+
+Joystick::Calibration_t Joystick::getCalibration(int axis)
+{
+ if (axis < 0 || axis > _cAxes) {
+ qCWarning(JoystickLog) << "Invalid axis index" << axis;
+ }
+
+ return _rgCalibration[axis];
+}
+
+void Joystick::setFunctionAxis(AxisFunction_t function, int axis)
+{
+ if (axis < 0 || axis > _cAxes) {
+ qCWarning(JoystickLog) << "Invalid axis index" << axis;
+ return;
+ }
+
+ _calibrated = true;
+ _rgFunctionAxis[function] = axis;
+ _saveSettings();
+ emit calibratedChanged(_calibrated);
+}
+
+int Joystick::getFunctionAxis(AxisFunction_t function)
+{
+ if (function < 0 || function >= maxFunction) {
+ qCWarning(JoystickLog) << "Invalid function" << function;
+ }
+
+ return _rgFunctionAxis[function];
+}
+
+QStringList Joystick::actions(void)
+{
+ QStringList list;
+
+ foreach(QAction* action, MultiVehicleManager::instance()->activeVehicle()->uas()->getActions()) {
+ list += action->text();
+ }
+
+ return list;
+}
+
+void Joystick::setButtonAction(int button, int action)
+{
+ if (button < 0 || button > _cButtons) {
+ qCWarning(JoystickLog) << "Invalid button index" << button;
+ return;
+ }
+
+ _rgButtonActions[button] = action;
+ _saveSettings();
+ emit buttonActionsChanged(buttonActions());
+}
+
+int Joystick::getButtonAction(int button)
+{
+ if (button < 0 || button > _cButtons) {
+ qCWarning(JoystickLog) << "Invalid button index" << button;
+ }
+
+ return _rgButtonActions[button];
+}
+
+QVariantList Joystick::buttonActions(void)
+{
+ QVariantList list;
+
+ for (int button=0; button<_buttonCount; button++) {
+ list += QVariant::fromValue(_rgButtonActions[button]);
+ }
+
+ return list;
+}
+
+int Joystick::throttleMode(void)
+{
+ return _throttleMode;
+}
+
+void Joystick::setThrottleMode(int mode)
+{
+ if (mode < 0 || mode >= ThrottleModeMax) {
+ qCWarning(JoystickLog) << "Invalid throttle mode" << mode;
+ return;
+ }
+
+ _throttleMode = (ThrottleMode_t)mode;
+ _saveSettings();
+ emit throttleModeChanged(_throttleMode);
+}
+
+bool Joystick::enabled(void)
+{
+ Fact* fact = MultiVehicleManager::instance()->activeVehicle()->autopilotPlugin()->getParameterFact(FactSystem::defaultComponentId, "COM_RC_IN_MODE");
+ if (!fact) {
+ qCWarning(JoystickLog) << "Missing COM_RC_IN_MODE parameter";
+ return false;
+ }
+
+ return _enabled && _calibrated && (fact->value().toInt() == 2);
+}
+
+void Joystick::setEnabled(bool enabled)
+{
+ if (!_calibrated) {
+ return;
+ }
+
+ Fact* fact = MultiVehicleManager::instance()->activeVehicle()->autopilotPlugin()->getParameterFact(FactSystem::defaultComponentId, "COM_RC_IN_MODE");
+ if (!fact) {
+ qCWarning(JoystickLog) << "Missing COM_RC_IN_MODE parameter";
+ return;
+ }
+
+ _enabled = enabled;
+ fact->setValue(enabled ? 2 : 0);
+
+ _saveSettings();
+ emit enabledChanged(_enabled);
+
+ if (_enabled) {
+ startPolling();
+ } else {
+ stopPolling();
+ }
+}
diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h
new file mode 100644
index 0000000000000000000000000000000000000000..a2622b117b4e7ea9bc911867034304bb523b4989
--- /dev/null
+++ b/src/Joystick/Joystick.h
@@ -0,0 +1,167 @@
+/*=====================================================================
+
+ 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 Joystick_H
+#define Joystick_H
+
+#include
+#include
+
+#include "QGCLoggingCategory.h"
+#include "Vehicle.h"
+
+Q_DECLARE_LOGGING_CATEGORY(JoystickLog)
+
+class Joystick : public QThread
+{
+ Q_OBJECT
+
+public:
+ Joystick(const QString& name, int axisCount, int buttonCount, int sdlIndex);
+ ~Joystick();
+
+ typedef struct {
+ int min;
+ int max;
+ int center;
+ bool reversed;
+ } Calibration_t;
+
+ typedef enum {
+ rollFunction,
+ pitchFunction,
+ yawFunction,
+ throttleFunction,
+ maxFunction
+ } AxisFunction_t;
+
+ typedef enum {
+ ThrottleModeCenterZero,
+ ThrottleModeDownZero,
+ ThrottleModeMax
+ } ThrottleMode_t;
+
+ Q_PROPERTY(QString name READ name CONSTANT)
+
+ Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged)
+ Q_PROPERTY(bool enabled READ enabled WRITE setEnabled NOTIFY enabledChanged)
+
+ Q_PROPERTY(int buttonCount MEMBER _buttonCount CONSTANT)
+ Q_PROPERTY(int axisCount MEMBER _axisCount CONSTANT)
+
+ Q_PROPERTY(QStringList actions READ actions CONSTANT)
+
+ Q_PROPERTY(QVariantList buttonActions READ buttonActions NOTIFY buttonActionsChanged)
+ Q_INVOKABLE void setButtonAction(int button, int action);
+ Q_INVOKABLE int getButtonAction(int button);
+
+ Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged)
+
+ /// Start the polling thread which will in turn emit joystick signals
+ void startPolling(void);
+ void stopPolling(void);
+
+ void setCalibration(int axis, Calibration_t& calibration);
+ Calibration_t getCalibration(int axis);
+
+ void setFunctionAxis(AxisFunction_t function, int axis);
+ int getFunctionAxis(AxisFunction_t function);
+
+ QStringList actions(void);
+ QVariantList buttonActions(void);
+
+ QString name(void) { return _name; }
+
+ int throttleMode(void);
+ void setThrottleMode(int mode);
+
+ bool enabled(void);
+ void setEnabled(bool enabled);
+
+ bool calibrating(void) { return _calibrating; }
+ void setCalibrating(bool calibrating) { _calibrating = calibrating; }
+
+signals:
+ void calibratedChanged(bool calibrated);
+
+ // The raw signals are only meant for use by calibration
+ void rawAxisValueChanged(int index, int value);
+ void rawButtonPressedChanged(int index, int pressed);
+
+ void buttonActionsChanged(QVariantList actions);
+
+ void throttleModeChanged(int mode);
+
+ void enabledChanged(bool enabled);
+
+ /// Signal containing new joystick information
+ /// @param roll Range is -1:1, negative meaning roll left, positive meaning roll right
+ /// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up
+ /// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right
+ /// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle
+ /// @param mode See Vehicle::JoystickMode_t enum
+ void manualControl(float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode);
+
+ void buttonActionTriggered(int action);
+
+private:
+ void _saveSettings(void);
+ void _loadSettings(void);
+ float _adjustRange(int value, Calibration_t calibration);
+
+ // Override from QThread
+ virtual void run(void);
+
+private:
+ int _sdlIndex; ///< Index for SDL_JoystickOpen
+
+ bool _exitThread; ///< true: signal thread to exit
+
+ QString _name;
+ bool _enabled;
+ bool _calibrated;
+ bool _calibrating;
+ int _axisCount;
+ int _buttonCount;
+
+ static const int _cAxes = 4;
+ int _rgAxisValues[_cAxes];
+ Calibration_t _rgCalibration[_cAxes];
+ int _rgFunctionAxis[maxFunction];
+ static const char* _rgFunctionSettingsKey[maxFunction];
+
+ static const int _cButtons = 12;
+ int _rgButtonValues[_cButtons];
+ int _rgButtonActions[_cButtons];
+ quint16 _lastButtonBits;
+
+ ThrottleMode_t _throttleMode;
+
+ static const char* _settingsGroup;
+ static const char* _calibratedSettingsKey;
+ static const char* _buttonActionSettingsKey;
+ static const char* _throttleModeSettingsKey;
+ static const char* _enabledSettingsKey;
+};
+
+#endif
diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc
new file mode 100644
index 0000000000000000000000000000000000000000..e8c28570f45b7f5d9b936004917b5edfc9241b9f
--- /dev/null
+++ b/src/Joystick/JoystickManager.cc
@@ -0,0 +1,158 @@
+/*=====================================================================
+
+ 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 "JoystickManager.h"
+
+#include
+
+#ifdef Q_OS_MAC
+ #include
+#else
+ #include
+#endif
+
+QGC_LOGGING_CATEGORY(JoystickManagerLog, "JoystickManagerLog")
+
+IMPLEMENT_QGC_SINGLETON(JoystickManager, JoystickManager)
+
+const char * JoystickManager::_settingsGroup = "JoystickManager";
+const char * JoystickManager::_settingsKeyActiveJoystick = "ActiveJoystick";
+
+JoystickManager::JoystickManager(QObject* parent)
+ : QGCSingleton(parent)
+ , _activeJoystick(NULL)
+{
+ QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+
+ if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
+ qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError();
+ return;
+ }
+
+ // Load available joysticks
+
+ qCDebug(JoystickManagerLog) << "Available joysticks";
+
+ for (int i=0; iname();
+ }
+
+ setActiveJoystick(_name2JoystickMap.value(name, _name2JoystickMap.first()));
+ settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
+}
+
+Joystick* JoystickManager::activeJoystick(void)
+{
+ return _activeJoystick;
+}
+
+void JoystickManager::setActiveJoystick(Joystick* joystick)
+{
+ QSettings settings;
+
+ if (!_name2JoystickMap.contains(joystick->name())) {
+ qCWarning(JoystickManagerLog) << "Set active not in map" << joystick->name();
+ return;
+ }
+
+ if (_activeJoystick) {
+ _activeJoystick->stopPolling();
+ }
+
+ _activeJoystick = joystick;
+
+ settings.beginGroup(_settingsGroup);
+ settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
+
+ emit activeJoystickChanged(_activeJoystick);
+ emit activeJoystickNameChanged(_activeJoystick->name());
+}
+
+QVariantList JoystickManager::joysticks(void)
+{
+ QVariantList list;
+
+ foreach (QString name, _name2JoystickMap.keys()) {
+ list += QVariant::fromValue(_name2JoystickMap[name]);
+ }
+
+ return list;
+}
+
+QStringList JoystickManager::joystickNames(void)
+{
+ return _name2JoystickMap.keys();
+}
+
+QString JoystickManager::activeJoystickName(void)
+{
+ return _activeJoystick ? _activeJoystick->name() : QString();
+}
+
+void JoystickManager::setActiveJoystickName(const QString& name)
+{
+ if (!_name2JoystickMap.contains(name)) {
+ qCWarning(JoystickManagerLog) << "Set active not in map" << name;
+ return;
+ }
+
+ setActiveJoystick(_name2JoystickMap[name]);
+}
diff --git a/src/Joystick/JoystickManager.h b/src/Joystick/JoystickManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5c9fc96ff2bdca8819019abbc9127b91c04ee9a
--- /dev/null
+++ b/src/Joystick/JoystickManager.h
@@ -0,0 +1,81 @@
+/*=====================================================================
+
+ 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 JoystickManager_H
+#define JoystickManager_H
+
+#include "QGCSingleton.h"
+#include "QGCLoggingCategory.h"
+#include "Joystick.h"
+
+#include
+
+Q_DECLARE_LOGGING_CATEGORY(JoystickManagerLog)
+
+class JoystickManager : public QGCSingleton
+{
+ Q_OBJECT
+
+ DECLARE_QGC_SINGLETON(JoystickManager, JoystickManager)
+
+public:
+ /// List of available joysticks
+ Q_PROPERTY(QVariantList joysticks READ joysticks CONSTANT)
+ Q_PROPERTY(QStringList joystickNames READ joystickNames CONSTANT)
+
+ /// Active joystick
+ Q_PROPERTY(Joystick* activeJoystick READ activeJoystick WRITE setActiveJoystick NOTIFY activeJoystickChanged)
+ Q_PROPERTY(QString activeJoystickName READ activeJoystickName WRITE setActiveJoystickName NOTIFY activeJoystickNameChanged)
+
+ QVariantList joysticks();
+ QStringList joystickNames(void);
+
+ Joystick* activeJoystick(void);
+ void setActiveJoystick(Joystick* joystick);
+
+ QString activeJoystickName(void);
+ void setActiveJoystickName(const QString& name);
+
+signals:
+ void activeJoystickChanged(Joystick* joystick);
+ void activeJoystickNameChanged(const QString& name);
+
+private slots:
+
+private:
+ /// All access to singleton is through JoystickManager::instance
+ JoystickManager(QObject* parent = NULL);
+ ~JoystickManager();
+
+ void _setActiveJoystickFromSettings(void);
+
+private:
+ Joystick* _activeJoystick;
+
+ QMap _name2JoystickMap;
+
+ static const char * _settingsGroup;
+ static const char * _settingsKeyActiveJoystick;
+};
+
+#endif
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index 48615b08811eb8d5cfe90b1c326c4f5e63158070..45e04e734edbd0bd556c43f8208ced3b0cac3dcd 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -89,6 +89,8 @@ G_END_DECLS
#include "PX4/PX4FirmwarePlugin.h"
#include "Vehicle.h"
#include "MavlinkQmlSingleton.h"
+#include "JoystickManager.h"
+#include "JoystickConfigController.h"
#ifdef QGC_RTLAB_ENABLED
#include "OpalLink.h"
@@ -327,16 +329,19 @@ void QGCApplication::_initCommon(void)
qmlRegisterUncreatableType("QGroundControl.AutoPilotPlugin", 1, 0, "AutoPilotPlugin", "Can only reference, cannot create");
qmlRegisterUncreatableType("QGroundControl.AutoPilotPlugin", 1, 0, "VehicleComponent", "Can only reference, cannot create");
qmlRegisterUncreatableType("QGroundControl.Vehicle", 1, 0, "Vehicle", "Can only reference, cannot create");
+ qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only");
+ qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "ViewWidgetController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "ParameterEditorController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "FlightModesComponentController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "AirframeComponentController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "SensorsComponentController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "PowerComponentController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "RadioComponentController");
- qmlRegisterType("QGroundControl.Controllers", 1, 0, "ScreenToolsController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ViewWidgetController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ParameterEditorController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "FlightModesComponentController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "AirframeComponentController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "SensorsComponentController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "PowerComponentController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "RadioComponentController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ScreenToolsController");
+ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "JoystickConfigController");
#ifndef __mobile__
qmlRegisterType("QGroundControl.Controllers", 1, 0, "FirmwareUpgradeController");
@@ -576,6 +581,11 @@ void QGCApplication::_createSingletons(void)
Q_UNUSED(multiVehicleManager);
Q_ASSERT(multiVehicleManager);
+ // No dependencies
+ JoystickManager* joystickManager = JoystickManager::_createSingleton();
+ Q_UNUSED(joystickManager);
+ Q_ASSERT(joystickManager);
+
// No dependencies
GAudioOutput* audio = GAudioOutput::_createSingleton();
Q_UNUSED(audio);
@@ -636,6 +646,7 @@ void QGCApplication::_destroySingletons(void)
HomePositionManager::_deleteSingleton();
LinkManager::_deleteSingleton();
GAudioOutput::_deleteSingleton();
+ JoystickManager::_deleteSingleton();
MultiVehicleManager::_deleteSingleton();
FirmwarePluginManager::_deleteSingleton();
GenericFirmwarePlugin::_deleteSingleton();
diff --git a/src/QGCQuickWidget.cc b/src/QGCQuickWidget.cc
index 4b21422fd3032413fb32bf9d24d6acddfe28b343..351d48177fb715306477cfd1109e15c583824cb2 100644
--- a/src/QGCQuickWidget.cc
+++ b/src/QGCQuickWidget.cc
@@ -25,6 +25,7 @@
#include "AutoPilotPluginManager.h"
#include "QGCMessageBox.h"
#include "MultiVehicleManager.h"
+#include "JoystickManager.h"
#include
#include
@@ -40,6 +41,7 @@ QGCQuickWidget::QGCQuickWidget(QWidget* parent) :
{
rootContext()->engine()->addImportPath("qrc:/qml");
rootContext()->setContextProperty("multiVehicleManager", MultiVehicleManager::instance());
+ rootContext()->setContextProperty("joystickManager", JoystickManager::instance());
}
void QGCQuickWidget::setAutoPilot(AutoPilotPlugin* autoPilot)
diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc
index 7bb1da915f2e8226f06b6715aa132aaea5c7e0df..cdb54dfe06f7df3b89944efe7a7c44e920f81ffc 100644
--- a/src/Vehicle/MultiVehicleManager.cc
+++ b/src/Vehicle/MultiVehicleManager.cc
@@ -26,6 +26,9 @@
#include "MultiVehicleManager.h"
#include "AutoPilotPlugin.h"
+#include "JoystickManager.h"
+#include "MAVLinkProtocol.h"
+#include "UAS.h"
IMPLEMENT_QGC_SINGLETON(MultiVehicleManager, MultiVehicleManager)
@@ -107,6 +110,15 @@ void MultiVehicleManager::_deleteVehiclePhase1(void)
qWarning() << "Vehicle not found in map!";
}
+ // Disconnect the vehicle from the uas
+ vehicle->uas()->clearVehicle();
+
+ // Disconnect joystick
+ Joystick* joystick = JoystickManager::instance()->activeJoystick();
+ if (joystick) {
+ joystick->stopPolling();
+ }
+
// First we must signal that a vehicle is no longer available.
_activeVehicleAvailable = false;
_parameterReadyVehicleAvailable = false;
@@ -150,6 +162,12 @@ void MultiVehicleManager::setActiveVehicle(Vehicle* vehicle)
{
if (vehicle != _activeVehicle) {
if (_activeVehicle) {
+ // Disconnect joystick
+ Joystick* joystick = JoystickManager::instance()->activeJoystick();
+ if (joystick) {
+ joystick->stopPolling();
+ }
+
// The sequence of signals is very important in order to not leave Qml elements connected
// to a non-existent vehicle.
@@ -195,6 +213,12 @@ void MultiVehicleManager::_autopilotPluginReadyChanged(bool pluginReady)
}
if (autopilot->vehicle() == _activeVehicle) {
+ // Connect joystick
+ Joystick* joystick = JoystickManager::instance()->activeJoystick();
+ if (joystick && joystick->enabled()) {
+ joystick->startPolling();
+ }
+
_parameterReadyVehicleAvailable = pluginReady;
emit parameterReadyVehicleAvailableChanged(pluginReady);
}
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 69e4ad94a0c1ca196fe7fc352a90a141a2731a05..cb62bdda3a1c04669858fc4a2a7bf315356d078d 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -28,6 +28,7 @@
#include "FirmwarePlugin.h"
#include "AutoPilotPluginManager.h"
#include "UASMessageHandler.h"
+#include "UAS.h"
QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")
@@ -35,9 +36,15 @@ QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")
#define DEFAULT_LAT 38.965767f
#define DEFAULT_LON -120.083923f
+const char* Vehicle::_settingsGroup = "Vehicle%1"; // %1 replace with mavlink system id
+const char* Vehicle::_joystickModeSettingsKey = "JoystickMode";
+
Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
: _id(vehicleId)
, _firmwareType(firmwareType)
+ , _firmwarePlugin(NULL)
+ , _autopilotPlugin(NULL)
+ , _joystickMode(JoystickModeRC)
, _uas(NULL)
, _mav(NULL)
, _currentMessageCount(0)
@@ -74,6 +81,8 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
, _wpm(NULL)
, _updateCount(0)
{
+ _loadSettings();
+
_addLink(link);
connect(MAVLinkProtocol::instance(), &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived);
@@ -809,3 +818,57 @@ void Vehicle::resetMessages()
emit messageTypeChanged();
}
}
+
+int Vehicle::manualControlReservedButtonCount(void)
+{
+ return _firmwarePlugin->manualControlReservedButtonCount();
+}
+
+void Vehicle::_loadSettings(void)
+{
+ QSettings settings;
+
+ settings.beginGroup(QString(_settingsGroup).arg(_id));
+
+ bool convertOk;
+
+ _joystickMode = (JoystickMode_t)settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk);
+ if (!convertOk) {
+ _joystickMode = JoystickModeRC;
+ }
+}
+
+void Vehicle::_saveSettings(void)
+{
+ QSettings settings;
+
+ settings.beginGroup(QString(_settingsGroup).arg(_id));
+
+ settings.setValue(_joystickModeSettingsKey, _joystickMode);
+}
+
+int Vehicle::joystickMode(void)
+{
+ return _joystickMode;
+}
+
+void Vehicle::setJoystickMode(int mode)
+{
+ if (mode < 0 || mode >= JoystickModeMax) {
+ qCWarning(VehicleLog) << "Invalid joystick mode" << mode;
+ return;
+ }
+
+ _joystickMode = (JoystickMode_t)mode;
+ _saveSettings();
+ emit joystickModeChanged(mode);
+}
+
+QStringList Vehicle::joystickModes(void)
+{
+ QStringList list;
+
+ list << "Simulate RC" << "Attitude" << "Position" << "Force" << "Velocity";
+
+ return list;
+}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 37bcb9d81712b08dfcf1a0ef33a7d0db58a611c7..fc0271acdda4bf46c67ed40df014da17a0f9b58f 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -29,13 +29,17 @@
#include
#include
+#include
#include "LinkInterface.h"
#include "QGCMAVLink.h"
-#include "UAS.h"
+#include "MissionItem.h"
+class UAS;
+class UASInterface;
class FirmwarePlugin;
class AutoPilotPlugin;
+class UASWaypointManager;
Q_DECLARE_LOGGING_CATEGORY(VehicleLog)
@@ -91,6 +95,30 @@ public:
//-- MissionItem management
Q_PROPERTY(QQmlListProperty missionItems READ missionItems NOTIFY missionItemsChanged)
+ /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
+ /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
+ /// The remainder can be assigned to Vehicle actions.
+ /// @return -1: reserver all buttons, >0 number of buttons to reserve
+ Q_PROPERTY(int manualControlReservedButtonCount READ manualControlReservedButtonCount CONSTANT)
+
+ typedef enum {
+ JoystickModeRC, ///< Joystick emulates am RC Transmitter
+ JoystickModeAttitude,
+ JoystickModePosition,
+ JoystickModeForce,
+ JoystickModeVelocity,
+ JoystickModeMax
+ } JoystickMode_t;
+
+ /// The joystick mode associated with this vehicle. Joystick modes are stored keyed by mavlink system id.
+ Q_PROPERTY(int joystickMode READ joystickMode WRITE setJoystickMode NOTIFY joystickModeChanged)
+ int joystickMode(void);
+ void setJoystickMode(int mode);
+
+ /// List of joystick mode names
+ Q_PROPERTY(QStringList joystickModes READ joystickModes CONSTANT)
+ QStringList joystickModes(void);
+
// Property accesors
int id(void) { return _id; }
MAV_AUTOPILOT firmwareType(void) { return _firmwareType; }
@@ -106,6 +134,8 @@ public:
QList links(void);
+ int manualControlReservedButtonCount(void);
+
typedef enum {
MessageNone,
MessageNormal,
@@ -171,6 +201,7 @@ public slots:
signals:
void allLinksDisconnected(void);
void coordinateChanged(QGeoCoordinate coordinate);
+ void joystickModeChanged(int mode);
/// Used internally to move sendMessage call to main thread
void _sendMessageOnThread(mavlink_message_t message);
@@ -241,6 +272,9 @@ private slots:
private:
bool _containsLink(LinkInterface* link);
void _addLink(LinkInterface* link);
+ void _loadSettings(void);
+ void _saveSettings(void);
+
bool _isAirplane ();
void _addChange (int id);
float _oneDecimal (float value);
@@ -257,6 +291,8 @@ private:
/// This way Link deletion works correctly.
QList _links;
+ JoystickMode_t _joystickMode;
+
UAS* _uas;
QGeoCoordinate _geoCoordinate;
@@ -302,5 +338,8 @@ private:
UASWaypointManager* _wpm;
int _updateCount;
QList_waypoints;
+
+ static const char* _settingsGroup;
+ static const char* _joystickModeSettingsKey;
};
#endif
diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml
new file mode 100644
index 0000000000000000000000000000000000000000..f10491f1f0296a6c5d03a4e89cba6f948b0e77a8
--- /dev/null
+++ b/src/VehicleSetup/JoystickConfig.qml
@@ -0,0 +1,652 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2015 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 .
+
+ ======================================================================*/
+
+import QtQuick 2.2
+import QtQuick.Controls 1.2
+import QtQuick.Dialogs 1.2
+
+import QGroundControl.Palette 1.0
+import QGroundControl.Controls 1.0
+import QGroundControl.ScreenTools 1.0
+import QGroundControl.Controllers 1.0
+
+/// Joystick Config
+QGCView {
+ id: rootQGCView
+ viewPanel: panel
+
+ QGCPalette { id: qgcPal; colorGroupEnabled: panel.enabled }
+
+ readonly property string dialogTitle: "Joystick Config"
+ readonly property real labelToMonitorMargin: defaultTextWidth * 3
+ property bool controllerCompleted: false
+ property bool controllerAndViewReady: false
+
+ property var _activeVehicle: multiVehicleManager.activeVehicle
+ property var _activeJoystick: joystickManager.activeJoystick
+
+ function updateAxisCount()
+ {
+ if (controllerAndViewReady) {
+ if (controller.axisCount < controller.minAxisCount) {
+ showDialog(axisCountDialogComponent, dialogTitle, 50, 0)
+ } else {
+ hideDialog()
+ }
+ }
+ }
+
+ JoystickConfigController {
+ id: controller
+ factPanel: panel
+ statusText: statusText
+ cancelButton: cancelButton
+ nextButton: nextButton
+ skipButton: skipButton
+
+ onAxisCountChanged: updateAxisCount()
+
+ Component.onCompleted: {
+ controllerCompleted = true
+ if (rootQGCView.completedSignalled) {
+ controllerAndViewReady = true
+ controller.start()
+ updateAxisCount()
+ }
+ }
+ }
+
+ onCompleted: {
+ if (controllerCompleted) {
+ controllerAndViewReady = true
+ controller.start()
+ updateAxisCount()
+ }
+ }
+
+ QGCViewPanel {
+ id: panel
+ anchors.fill: parent
+
+ Component {
+ id: axisCountDialogComponent
+
+ QGCViewMessage {
+ message: controller.axisCount == 0 ? "No joystick axes deteced." : controller.minAxisCount + " joystick axes or more are needed to fly."
+ }
+ }
+
+ // Live axis monitor control component
+ Component {
+ id: axisMonitorDisplayComponent
+
+ Item {
+ property int axisValue: 0
+
+
+ property int __lastAxisValue: 0
+ readonly property int __axisValueMaxJitter: 100
+ property color __barColor: qgcPal.windowShade
+
+ // Bar
+ Rectangle {
+ id: bar
+ anchors.verticalCenter: parent.verticalCenter
+ width: parent.width
+ height: parent.height / 2
+ color: __barColor
+ }
+
+ // Center point
+ Rectangle {
+ anchors.horizontalCenter: parent.horizontalCenter
+ width: defaultTextWidth / 2
+ height: parent.height
+ color: qgcPal.window
+ }
+
+ // Indicator
+ Rectangle {
+ anchors.verticalCenter: parent.verticalCenter
+ width: parent.height * 0.75
+ height: width
+ x: (reversed ? (parent.width - _indicatorPosition) : _indicatorPosition) - (width / 2)
+ radius: width / 2
+ color: qgcPal.text
+ visible: mapped
+
+ property real _percentAxisValue: ((axisValue + 32768.0) / (32768.0 * 2))
+ property real _indicatorPosition: parent.width * _percentAxisValue
+ }
+
+ QGCLabel {
+ anchors.fill: parent
+ horizontalAlignment: Text.AlignHCenter
+ verticalAlignment: Text.AlignVCenter
+ text: "Not Mapped"
+ visible: !mapped
+ }
+
+ ColorAnimation {
+ id: barAnimation
+ target: bar
+ property: "color"
+ from: "yellow"
+ to: __barColor
+ duration: 1500
+ }
+
+/*
+ // Axis value debugger
+ QGCLabel {
+ anchors.fill: parent
+ text: axisValue
+ }
+*/
+ }
+ } // Component - axisMonitorDisplayComponent
+
+ // Main view Qml starts here
+
+ QGCLabel {
+ id: header
+ font.pixelSize: ScreenTools.largeFontPixelSize
+ text: "JOYSTICK CONFIG"
+ }
+
+ Item {
+ id: spacer
+ anchors.top: header.bottom
+ width: parent.width
+ height: 10
+ }
+
+ // Left side column
+ Column {
+ id: leftColumn
+ anchors.rightMargin: ScreenTools.defaultFontPixelWidth
+ anchors.top: spacer.bottom
+ anchors.left: parent.left
+ anchors.right: rightColumn.left
+ spacing: 10
+
+ // Attitude Controls
+ Column {
+ width: parent.width
+ spacing: 5
+
+ QGCLabel { text: "Attitude Controls" }
+
+ Item {
+ width: parent.width
+ height: defaultTextHeight * 2
+
+ QGCLabel {
+ id: rollLabel
+ width: defaultTextWidth * 10
+ text: "Roll"
+ }
+
+ Loader {
+ id: rollLoader
+ anchors.left: rollLabel.right
+ anchors.right: parent.right
+ height: rootQGCView.defaultTextHeight
+ width: 100
+ sourceComponent: axisMonitorDisplayComponent
+
+ property real defaultTextWidth: rootQGCView.defaultTextWidth
+ property bool mapped: controller.rollAxisMapped
+ property bool reversed: controller.rollAxisReversed
+ }
+
+ Connections {
+ target: controller
+
+ onRollAxisValueChanged: rollLoader.item.axisValue = value
+ }
+ }
+
+ Item {
+ width: parent.width
+ height: defaultTextHeight * 2
+
+ QGCLabel {
+ id: pitchLabel
+ width: defaultTextWidth * 10
+ text: "Pitch"
+ }
+
+ Loader {
+ id: pitchLoader
+ anchors.left: pitchLabel.right
+ anchors.right: parent.right
+ height: rootQGCView.defaultTextHeight
+ width: 100
+ sourceComponent: axisMonitorDisplayComponent
+
+ property real defaultTextWidth: rootQGCView.defaultTextWidth
+ property bool mapped: controller.pitchAxisMapped
+ property bool reversed: controller.pitchAxisReversed
+ }
+
+ Connections {
+ target: controller
+
+ onPitchAxisValueChanged: pitchLoader.item.axisValue = value
+ }
+ }
+
+ Item {
+ width: parent.width
+ height: defaultTextHeight * 2
+
+ QGCLabel {
+ id: yawLabel
+ width: defaultTextWidth * 10
+ text: "Yaw"
+ }
+
+ Loader {
+ id: yawLoader
+ anchors.left: yawLabel.right
+ anchors.right: parent.right
+ height: rootQGCView.defaultTextHeight
+ width: 100
+ sourceComponent: axisMonitorDisplayComponent
+
+ property real defaultTextWidth: rootQGCView.defaultTextWidth
+ property bool mapped: controller.yawAxisMapped
+ property bool reversed: controller.yawAxisReversed
+ }
+
+ Connections {
+ target: controller
+
+ onYawAxisValueChanged: yawLoader.item.axisValue = value
+ }
+ }
+
+ Item {
+ width: parent.width
+ height: defaultTextHeight * 2
+
+ QGCLabel {
+ id: throttleLabel
+ width: defaultTextWidth * 10
+ text: "Throttle"
+ }
+
+ Loader {
+ id: throttleLoader
+ anchors.left: throttleLabel.right
+ anchors.right: parent.right
+ height: rootQGCView.defaultTextHeight
+ width: 100
+ sourceComponent: axisMonitorDisplayComponent
+
+ property real defaultTextWidth: rootQGCView.defaultTextWidth
+ property bool mapped: controller.throttleAxisMapped
+ property bool reversed: controller.throttleAxisReversed
+ }
+
+ Connections {
+ target: controller
+
+ onThrottleAxisValueChanged: throttleLoader.item.axisValue = value
+ }
+ }
+ } // Column - Attitude Control labels
+
+ // Command Buttons
+ Row {
+ spacing: 10
+
+ QGCButton {
+ id: skipButton
+ text: "Skip"
+
+ onClicked: controller.skipButtonClicked()
+ }
+
+ QGCButton {
+ id: cancelButton
+ text: "Cancel"
+
+ onClicked: controller.cancelButtonClicked()
+ }
+
+ QGCButton {
+ id: nextButton
+ primary: true
+ text: "Calibrate"
+
+ onClicked: controller.nextButtonClicked()
+ }
+ } // Row - Buttons
+
+ // Status Text
+ QGCLabel {
+ id: statusText
+ width: parent.width
+ wrapMode: Text.WordWrap
+ }
+
+ Rectangle {
+ width: parent.width
+ height: 1
+ border.color: qgcPal.text
+ border.width: 1
+ }
+
+ // Settings
+ Row {
+ width: parent.width
+ spacing: ScreenTools.defaultFontPixelWidth
+
+ // Left column settings
+ Column {
+ width: parent.width / 2
+ spacing: ScreenTools.defaultFontPixelHeight
+
+ QGCLabel { text: "Additional Joystick settings:" }
+
+ Column {
+ width: parent.width
+ spacing: ScreenTools.defaultFontPixelHeight
+
+ Row {
+ width: parent.width
+ spacing: ScreenTools.defaultFontPixelWidth
+
+ QGCLabel {
+ id: activeJoystickLabel
+ anchors.baseline: joystickCombo.baseline
+ text: "Active joystick:"
+ }
+
+ QGCComboBox {
+ id: joystickCombo
+ width: parent.width - activeJoystickLabel.width - parent.spacing
+ model: joystickManager.joystickNames
+
+ onActivated: _activeJoystick.setActiveJoystickName(textAt(index))
+ }
+ }
+
+ QGCCheckBox {
+ enabled: calibrated
+ text: calibrated ? "Enable joystick input, disable RC input" : "Enable joystick input (Calibrate First)"
+ checked: _activeJoystick.enabled
+
+ property bool calibrated: _activeJoystick.calibrated
+
+ onClicked: _activeJoystick.enabled = checked
+ }
+
+ Column {
+ spacing: ScreenTools.defaultFontPixelHeight / 3
+
+ ExclusiveGroup { id: throttleModeExclusiveGroup }
+
+ QGCRadioButton {
+ exclusiveGroup: throttleModeExclusiveGroup
+ text: "Center stick is zero throttle"
+ checked: _activeJoystick.throttleMode == 0
+
+ onClicked: _activeJoystick.throttleMode = 0
+ }
+
+ QGCRadioButton {
+ exclusiveGroup: throttleModeExclusiveGroup
+ text: "Full down stick is zero throttle"
+ checked: _activeJoystick.throttleMode == 1
+
+ onClicked: _activeJoystick.throttleMode = 1
+ }
+ }
+
+ QGCCheckBox {
+ id: advancedSettings
+ checked: _activeVehicle.joystickMode != 0
+ text: "Advanced settings (careful!)"
+
+ onClicked: {
+ if (!checked) {
+ _activeVehicle.joystickMode = 0
+ }
+ }
+ }
+
+ Row {
+ width: parent.width
+ spacing: ScreenTools.defaultFontPixelWidth
+ visible: advancedSettings.checked
+
+ QGCLabel {
+ id: joystickModeLabel
+ anchors.baseline: joystickModeCombo.baseline
+ text: "Joystick mode:"
+ }
+
+ QGCComboBox {
+ id: joystickModeCombo
+ currentIndex: _activeVehicle.joystickMode
+ width: ScreenTools.defaultFontPixelWidth * 20
+ model: _activeVehicle.joystickModes
+
+ onActivated: _activeVehicle.joystickMode = index
+ }
+ }
+ }
+ } // Column - left column
+
+ // Right column settings
+ Column {
+ width: parent.width / 2
+ spacing: ScreenTools.defaultFontPixelHeight
+
+ Connections {
+ target: _activeJoystick
+
+ onRawButtonPressedChanged: {
+ if (buttonActionRepeater.itemAt(index)) {
+ buttonActionRepeater.itemAt(index).pressed = pressed
+ }
+ }
+ }
+
+ QGCLabel { text: "Button actions:" }
+
+ Column {
+ width: parent.width
+ spacing: ScreenTools.defaultFontPixelHeight / 3
+
+ QGCLabel {
+ visible: _activeVehicle.manualControlReservedButtonCount != 0
+ text: "Buttons 0-" + reservedButtonCount + " reserved for firmware use"
+
+ property int reservedButtonCount: _activeVehicle.manualControlReservedButtonCount == -1 ? _activeJoystick.buttonCount : _activeVehicle.manualControlReservedButtonCount
+ }
+
+ Repeater {
+ id: buttonActionRepeater
+ model: _activeJoystick.buttonCount
+
+ Row {
+ spacing: ScreenTools.defaultFontPixelWidth
+ visible: _activeVehicle.manualControlReservedButtonCount == -1 ? false : modelData >= _activeVehicle.manualControlReservedButtonCount
+
+ property bool pressed
+
+ QGCCheckBox {
+ anchors.verticalCenter: parent.verticalCenter
+ checked: _activeJoystick.buttonActions[modelData] != -1
+
+ onClicked: _activeJoystick.setButtonAction(modelData, checked ? buttonActionCombo.currentIndex : -1)
+ }
+
+ Rectangle {
+ anchors.verticalCenter: parent.verticalCenter
+ width: ScreenTools.defaultFontPixelHeight * 1.5
+ height: width
+ border.width: 1
+ border.color: qgcPal.text
+ color: pressed ? qgcPal.buttonHighlight : qgcPal.button
+
+
+ QGCLabel {
+ anchors.fill: parent
+ color: pressed ? qgcPal.buttonHighlightText : qgcPal.buttonText
+ horizontalAlignment: Text.AlignHCenter
+ verticalAlignment: Text.AlignVCenter
+ text: modelData
+ }
+ }
+
+ QGCComboBox {
+ id: buttonActionCombo
+ width: ScreenTools.defaultFontPixelWidth * 20
+ model: _activeJoystick.actions
+ currentIndex: _activeJoystick.buttonActions[modelData]
+
+ onActivated: _activeJoystick.setButtonAction(modelData, index)
+ }
+ }
+ } // Repeater
+ } // Column
+ } // Column - right setting column
+ } // Row - Settings
+ } // Column - Left Main Column
+
+ // Right side column
+ Column {
+ id: rightColumn
+ anchors.top: parent.top
+ anchors.right: parent.right
+ width: defaultTextWidth * 35
+ spacing: 10
+
+ Image {
+ //width: parent.width
+ height: defaultTextHeight * 15
+ fillMode: Image.PreserveAspectFit
+ smooth: true
+ source: controller.imageHelp
+ }
+
+ // Axis monitor
+ Column {
+ width: parent.width
+ spacing: 5
+
+ QGCLabel { text: "Axis Monitor" }
+
+ Connections {
+ target: controller
+
+ onAxisValueChanged: {
+ if (axisMonitorRepeater.itemAt(axis)) {
+ axisMonitorRepeater.itemAt(axis).loader.item.axisValue = value
+ }
+ }
+ }
+
+ Repeater {
+ id: axisMonitorRepeater
+ model: controller.axisCount
+ width: parent.width
+
+ Row {
+ spacing: 5
+
+ // Need this to get to loader from Connections above
+ property Item loader: theLoader
+
+ QGCLabel {
+ id: axisLabel
+ text: modelData
+ }
+
+ Loader {
+ id: theLoader
+ anchors.verticalCenter: axisLabel.verticalCenter
+ height: rootQGCView.defaultTextHeight
+ width: 200
+ sourceComponent: axisMonitorDisplayComponent
+
+ property real defaultTextWidth: rootQGCView.defaultTextWidth
+ property bool mapped: true
+ readonly property bool reversed: false
+ }
+ }
+ }
+ } // Column - Axis Monitor
+
+ // Button monitor
+ Column {
+ width: parent.width
+ spacing: ScreenTools.defaultFontPixelHeight
+
+ QGCLabel { text: "Button Monitor" }
+
+ Connections {
+ target: _activeJoystick
+
+ onRawButtonPressedChanged: {
+ if (buttonMonitorRepeater.itemAt(index)) {
+ buttonMonitorRepeater.itemAt(index).pressed = pressed
+ }
+ }
+ }
+
+ Row {
+ spacing: -1
+
+ Repeater {
+ id: buttonMonitorRepeater
+ model: _activeJoystick.buttonCount
+
+ Rectangle {
+ width: ScreenTools.defaultFontPixelHeight * 1.5
+ height: width
+ border.width: 1
+ border.color: qgcPal.text
+ color: pressed ? qgcPal.buttonHighlight : qgcPal.button
+
+ property bool pressed
+
+ QGCLabel {
+ anchors.fill: parent
+ color: pressed ? qgcPal.buttonHighlightText : qgcPal.buttonText
+ horizontalAlignment: Text.AlignHCenter
+ verticalAlignment: Text.AlignVCenter
+ text: modelData
+ }
+ }
+ } // Repeater
+ } // Row
+ } // Column - Axis Monitor
+ } // Column - Right Column
+ } // QGCViewPanel
+}
diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc
new file mode 100644
index 0000000000000000000000000000000000000000..a031c2af22693ef095d1ab0efe0791bdfbc1dea0
--- /dev/null
+++ b/src/VehicleSetup/JoystickConfigController.cc
@@ -0,0 +1,739 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009, 2015 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 "JoystickConfigController.h"
+#include "JoystickManager.h"
+#include "QGCMessageBox.h"
+
+#include
+
+QGC_LOGGING_CATEGORY(JoystickConfigControllerLog, "JoystickConfigControllerLog")
+
+const int JoystickConfigController::_updateInterval = 150; ///< Interval for timer which updates radio channel widgets
+const int JoystickConfigController::_calCenterPoint = 0;
+const int JoystickConfigController::_calValidMinValue = -32768; ///< Largest valid minimum axis value
+const int JoystickConfigController::_calValidMaxValue = 32768; ///< Smallest valid maximum axis value
+const int JoystickConfigController::_calDefaultMinValue = -32768; ///< Default value for Min if not set
+const int JoystickConfigController::_calDefaultMaxValue = 32768; ///< Default value for Max if not set
+const int JoystickConfigController::_calRoughCenterDelta = 500; ///< Delta around center point which is considered to be roughly centered
+const int JoystickConfigController::_calMoveDelta = 32768/2; ///< Amount of delta past center which is considered stick movement
+const int JoystickConfigController::_calSettleDelta = 100; ///< Amount of delta which is considered no stick movement
+const int JoystickConfigController::_calMinDelta = 1000; ///< Amount of delta allowed around min value to consider channel at min
+
+const int JoystickConfigController::_stickDetectSettleMSecs = 500;
+
+const char* JoystickConfigController::_imageFilePrefix = "calibration/";
+const char* JoystickConfigController::_imageFileMode2Dir = "joystick/";
+const char* JoystickConfigController::_imageCenter = "joystickCenter.png";
+const char* JoystickConfigController::_imageThrottleUp = "joystickThrottleUp.png";
+const char* JoystickConfigController::_imageThrottleDown = "joystickThrottleDown.png";
+const char* JoystickConfigController::_imageYawLeft = "joystickYawLeft.png";
+const char* JoystickConfigController::_imageYawRight = "joystickYawRight.png";
+const char* JoystickConfigController::_imageRollLeft = "joystickRollLeft.png";
+const char* JoystickConfigController::_imageRollRight = "joystickRollRight.png";
+const char* JoystickConfigController::_imagePitchUp = "joystickPitchUp.png";
+const char* JoystickConfigController::_imagePitchDown = "joystickPitchDown.png";
+
+const char* JoystickConfigController::_settingsGroup = "Joysticks";
+
+JoystickConfigController::JoystickConfigController(void) :
+ _currentStep(-1),
+ _axisCount(0),
+ _calState(calStateAxisWait),
+ _statusText(NULL),
+ _cancelButton(NULL),
+ _nextButton(NULL),
+ _skipButton(NULL)
+{
+ // FIXME: Needs to handle active joystick change
+ connect(JoystickManager::instance()->activeJoystick(), &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
+ JoystickManager::instance()->activeJoystick()->startPolling();
+ _loadSettings();
+
+ _resetInternalCalibrationValues();
+}
+
+void JoystickConfigController::start(void)
+{
+ _stopCalibration();
+ _setInternalCalibrationValuesFromSettings();
+}
+
+JoystickConfigController::~JoystickConfigController()
+{
+ _storeSettings();
+}
+
+/// @brief Returns the state machine entry for the specified state.
+const JoystickConfigController::stateMachineEntry* JoystickConfigController::_getStateMachineEntry(int step)
+{
+ static const char* msgBegin = "Allow all sticks to center as shown in diagram.\n\nClick 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 hold 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* msgComplete = "All settings have been captured. Click Next to Save.";
+
+ static const stateMachineEntry rgStateMachine[] = {
+ //Function
+ { Joystick::maxFunction, msgBegin, _imageCenter, &JoystickConfigController::_inputCenterWaitBegin, &JoystickConfigController::_saveAllTrims, NULL },
+ { Joystick::throttleFunction, msgThrottleUp, _imageThrottleUp, &JoystickConfigController::_inputStickDetect, NULL, NULL },
+ { Joystick::throttleFunction, msgThrottleDown, _imageThrottleDown, &JoystickConfigController::_inputStickMin, NULL, NULL },
+ { Joystick::yawFunction, msgYawRight, _imageYawRight, &JoystickConfigController::_inputStickDetect, NULL, NULL },
+ { Joystick::yawFunction, msgYawLeft, _imageYawLeft, &JoystickConfigController::_inputStickMin, NULL, NULL },
+ { Joystick::rollFunction, msgRollRight, _imageRollRight, &JoystickConfigController::_inputStickDetect, NULL, NULL },
+ { Joystick::rollFunction, msgRollLeft, _imageRollLeft, &JoystickConfigController::_inputStickMin, NULL, NULL },
+ { Joystick::pitchFunction, msgPitchUp, _imagePitchUp, &JoystickConfigController::_inputStickDetect, NULL, NULL },
+ { Joystick::pitchFunction, msgPitchDown, _imagePitchDown, &JoystickConfigController::_inputStickMin, NULL, NULL },
+ { Joystick::pitchFunction, msgPitchCenter, _imageCenter, &JoystickConfigController::_inputCenterWait, NULL, NULL },
+ { Joystick::maxFunction, msgComplete, _imageCenter, NULL, &JoystickConfigController::_writeCalibration, NULL },
+ };
+
+ Q_ASSERT(step >=0 && step < (int)(sizeof(rgStateMachine) / sizeof(rgStateMachine[0])));
+
+ return &rgStateMachine[step];
+}
+
+void JoystickConfigController::_advanceState(void)
+{
+ _currentStep++;
+ _setupCurrentState();
+}
+
+
+/// @brief Sets up the state machine according to the current step from _currentStep.
+void JoystickConfigController::_setupCurrentState(void)
+{
+ const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
+
+ _statusText->setProperty("text", state->instructions);
+
+ _setHelpImage(state->image);
+
+ _stickDetectAxis = _axisMax;
+ _stickDetectSettleStarted = false;
+
+ _calSaveCurrentValues();
+
+ _nextButton->setEnabled(state->nextFn != NULL);
+ _skipButton->setEnabled(state->skipFn != NULL);
+}
+
+void JoystickConfigController::_axisValueChanged(int axis, int value)
+{
+ if (axis >= 0 && axis <= _axisMax) {
+ // We always update raw values
+ _axisRawValue[axis] = value;
+ emit axisValueChanged(axis, _axisRawValue[axis]);
+
+ // Signal attitude axis values to Qml if mapped
+ if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
+ switch (_rgAxisInfo[axis].function) {
+ case Joystick::rollFunction:
+ emit rollAxisValueChanged(_axisRawValue[axis]);
+ break;
+ case Joystick::pitchFunction:
+ emit pitchAxisValueChanged(_axisRawValue[axis]);
+ break;
+ case Joystick::yawFunction:
+ emit yawAxisValueChanged(_axisRawValue[axis]);
+ break;
+ case Joystick::throttleFunction:
+ emit throttleAxisValueChanged(_axisRawValue[axis]);
+ break;
+ default:
+ break;
+ }
+ }
+
+ //qCDebug(JoystickConfigControllerLog) << "Raw value" << axis << value;
+
+ if (_currentStep == -1) {
+ // Track the axis count by keeping track of how many axes we see
+ if (axis + 1 > (int)_axisCount) {
+ _axisCount = axis + 1;
+ emit axisCountChanged(_axisCount);
+ }
+ }
+
+ if (_currentStep != -1) {
+ const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
+ Q_ASSERT(state);
+ if (state->rcInputFn) {
+ (this->*state->rcInputFn)(state->function, axis, value);
+ }
+ }
+ }
+}
+
+void JoystickConfigController::nextButtonClicked(void)
+{
+ if (_currentStep == -1) {
+ // Need to have enough channels
+ if (_axisCount < _axisMinimum) {
+ QGCMessageBox::warning(tr("Joystick"), tr("Detected %1 joystick axes. To operate PX4, you need at least %2 axes.").arg(_axisCount).arg(_axisMinimum));
+ return;
+ }
+ _startCalibration();
+ } else {
+ const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
+ Q_ASSERT(state);
+ Q_ASSERT(state->nextFn);
+ (this->*state->nextFn)();
+ }
+}
+
+void JoystickConfigController::skipButtonClicked(void)
+{
+ Q_ASSERT(_currentStep != -1);
+
+ const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
+ Q_ASSERT(state);
+ Q_ASSERT(state->skipFn);
+ (this->*state->skipFn)();
+}
+
+void JoystickConfigController::cancelButtonClicked(void)
+{
+ _stopCalibration();
+}
+
+void JoystickConfigController::_saveAllTrims(void)
+{
+ // We save all trims as the first step. At this point no axes are mapped but it should still
+ // allow us to get good trims for the roll/pitch/yaw/throttle even though we don't know which
+ // axiss they are yet. AS we continue through the process the other axes will get their
+ // trims reset to correct values.
+
+ for (int i=0; i<_axisCount; i++) {
+ qCDebug(JoystickConfigControllerLog) << "_saveAllTrims trim" << _axisRawValue[i];
+ _rgAxisInfo[i].axisTrim = _axisRawValue[i];
+ }
+ _advanceState();
+}
+
+/// @brief Waits for the sticks to be centered, enabling Next when done.
+void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t function, int axis, int value)
+{
+ Q_UNUSED(function);
+ Q_UNUSED(axis);
+ Q_UNUSED(value);
+
+ // FIXME: Doesn't wait for center
+
+ _nextButton->setEnabled(true);
+}
+
+bool JoystickConfigController::_stickSettleComplete(int value)
+{
+ // We are waiting for the stick to settle out to a max position
+
+ if (abs(_stickDetectValue - value) > _calSettleDelta) {
+ // Stick is moving too much to consider stopped
+
+ qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, _stickDetectValue:value" << _stickDetectValue << value;
+
+ _stickDetectValue = value;
+ _stickDetectSettleStarted = false;
+ } else {
+ // Stick is still positioned within the specified small range
+
+ if (_stickDetectSettleStarted) {
+ // We have already started waiting
+
+ if (_stickDetectSettleElapsed.elapsed() > _stickDetectSettleMSecs) {
+ // Stick has stayed positioned in one place long enough, detection is complete.
+ return true;
+ }
+ } else {
+ // Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs
+
+ qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, _stickDetectValue:value" << _stickDetectValue << value;
+
+ _stickDetectSettleStarted = true;
+ _stickDetectSettleElapsed.start();
+ }
+ }
+
+ return false;
+}
+
+void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t function, int axis, int value)
+{
+ qCDebug(JoystickConfigControllerLog) << "_inputStickDetect function:axis:value" << function << axis << value;
+
+ // If this axis is already used in a mapping we can't use it again
+ if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
+ return;
+ }
+
+ if (_stickDetectAxis == _axisMax) {
+ // We have not detected enough movement on a axis yet
+
+ if (abs(_axisValueSave[axis] - value) > _calMoveDelta) {
+ // Stick has moved far enough to consider it as being selected for the function
+
+ qCDebug(JoystickConfigControllerLog) << "_inputStickDetect starting settle wait, function:axis:value" << function << axis << value;
+
+ // Setup up to detect stick being pegged to min or max value
+ _stickDetectAxis = axis;
+ _stickDetectInitialValue = value;
+ _stickDetectValue = value;
+ }
+ } else if (axis == _stickDetectAxis) {
+ if (_stickSettleComplete(value)) {
+ AxisInfo* info = &_rgAxisInfo[axis];
+
+ qCDebug(JoystickConfigControllerLog) << "_inputStickDetect settle complete, function:axis:value" << function << axis << value;
+
+ // Stick detection is complete. Stick should be at max position.
+ // Map the axis to the function
+ _rgFunctionAxisMapping[function] = axis;
+ info->function = function;
+
+ // Axis should be at max value, if it is below initial set point the the axis is reversed.
+ info->reversed = value < _axisValueSave[axis];
+ qCDebug(JoystickConfigControllerLog) << "_inputStickDetect reversed:value:_axisValueSave" << info->reversed << value << _axisValueSave[axis];
+
+ if (info->reversed) {
+ _rgAxisInfo[axis].axisMin = value;
+ } else {
+ _rgAxisInfo[axis].axisMax = value;
+ }
+
+ _signalAllAttiudeValueChanges();
+
+ _advanceState();
+ }
+ }
+}
+
+void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, int axis, int value)
+{
+ // We only care about the axis mapped to the function we are working on
+ if (_rgFunctionAxisMapping[function] != axis) {
+ return;
+ }
+
+ if (_stickDetectAxis == _axisMax) {
+ // Setup up to detect stick being pegged to extreme position
+ if (_rgAxisInfo[axis].reversed) {
+ if (value > _calCenterPoint + _calMoveDelta) {
+ _stickDetectAxis = axis;
+ _stickDetectInitialValue = value;
+ _stickDetectValue = value;
+ }
+ } else {
+ if (value < _calCenterPoint - _calMoveDelta) {
+ _stickDetectAxis = axis;
+ _stickDetectInitialValue = value;
+ _stickDetectValue = value;
+ }
+ }
+ } else {
+ // We are waiting for the selected axis to settle out
+
+ if (_stickSettleComplete(value)) {
+ AxisInfo* info = &_rgAxisInfo[axis];
+
+ // Stick detection is complete. Stick should be at min position.
+ if (info->reversed) {
+ _rgAxisInfo[axis].axisMax = value;
+ } else {
+ _rgAxisInfo[axis].axisMin = value;
+ }
+
+ // Check if this is throttle and set trim accordingly
+ if (function == Joystick::throttleFunction) {
+ _rgAxisInfo[axis].axisTrim = value;
+ }
+ // XXX to support configs which can reverse they need to check a reverse
+ // flag here and not do this.
+
+ _advanceState();
+ }
+ }
+}
+
+void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t function, int axis, int value)
+{
+ // We only care about the axis mapped to the function we are working on
+ if (_rgFunctionAxisMapping[function] != axis) {
+ return;
+ }
+
+ if (_stickDetectAxis == _axisMax) {
+ // Sticks have not yet moved close enough to center
+
+ if (abs(_calCenterPoint - value) < _calRoughCenterDelta) {
+ // Stick has moved close enough to center that we can start waiting for it to settle
+ _stickDetectAxis = axis;
+ _stickDetectInitialValue = value;
+ _stickDetectValue = value;
+ }
+ } else {
+ if (_stickSettleComplete(value)) {
+ _advanceState();
+ }
+ }
+}
+
+/// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence.
+void JoystickConfigController::_resetInternalCalibrationValues(void)
+{
+ // Set all raw axiss to not reversed and center point values
+ for (size_t i=0; i<_axisMax; i++) {
+ struct AxisInfo* info = &_rgAxisInfo[i];
+ info->function = Joystick::maxFunction;
+ info->reversed = false;
+ info->axisMin = JoystickConfigController::_calCenterPoint;
+ info->axisMax = JoystickConfigController::_calCenterPoint;
+ info->axisTrim = JoystickConfigController::_calCenterPoint;
+ }
+
+ // Initialize attitude function mapping to function axis not set
+ for (size_t i=0; iactiveJoystick();
+
+ // Initialize all function mappings to not set
+
+ for (size_t i=0; i<_axisMax; i++) {
+ struct AxisInfo* info = &_rgAxisInfo[i];
+ info->function = Joystick::maxFunction;
+ }
+
+ for (size_t i=0; igetCalibration(axis);
+ info->axisTrim = calibration.center;
+ info->axisMin = calibration.min;
+ info->axisMax = calibration.max;
+ info->reversed = calibration.reversed;
+
+ qCDebug(JoystickConfigControllerLog) << "Read settings axis:min:max:trim:reversed" << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed;
+ }
+
+ for (int function=0; functiongetFunctionAxis((Joystick::AxisFunction_t)function);
+
+ _rgFunctionAxisMapping[function] = paramAxis;
+ _rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
+ }
+
+ _signalAllAttiudeValueChanges();
+}
+
+/// @brief Validates the current settings against the calibration rules resetting values as necessary.
+void JoystickConfigController::_validateCalibration(void)
+{
+ for (int chan = 0; chan<_axisMax; chan++) {
+ struct AxisInfo* info = &_rgAxisInfo[chan];
+
+ if (chan < _axisCount) {
+ // Validate Min/Max values. Although the axis appears as available we still may
+ // not have good min/max/trim values for it. Set to defaults if needed.
+ if (info->axisMin > _calValidMinValue || info->axisMax < _calValidMaxValue) {
+ qCDebug(JoystickConfigControllerLog) << "_validateCalibration resetting axis" << chan;
+ info->axisMin = _calDefaultMinValue;
+ info->axisMax = _calDefaultMaxValue;
+ info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
+ } else {
+ switch (_rgAxisInfo[chan].function) {
+ case Joystick::throttleFunction:
+ case Joystick::yawFunction:
+ case Joystick::rollFunction:
+ case Joystick::pitchFunction:
+ // Make sure trim is within min/max
+ if (info->axisTrim < info->axisMin) {
+ info->axisTrim = info->axisMin;
+ } else if (info->axisTrim > info->axisMax) {
+ info->axisTrim = info->axisMax;
+ }
+ break;
+ default:
+ // Non-attitude control axiss have calculated trim
+ info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
+ break;
+ }
+
+ }
+ } else {
+ // Unavailable axiss are set to defaults
+ qCDebug(JoystickConfigControllerLog) << "_validateCalibration resetting unavailable axis" << chan;
+ info->axisMin = _calDefaultMinValue;
+ info->axisMax = _calDefaultMaxValue;
+ info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
+ info->reversed = false;
+ }
+ }
+}
+
+
+/// @brief Saves the rc calibration values to the board parameters.
+void JoystickConfigController::_writeCalibration(void)
+{
+ Joystick* joystick = JoystickManager::instance()->activeJoystick();
+
+ _validateCalibration();
+
+ for (int axis=0; axis<_axisMax; axis++) {
+ Joystick::Calibration_t calibration;
+
+ struct AxisInfo* info = &_rgAxisInfo[axis];
+
+ calibration.center = info->axisTrim;
+ calibration.min = info->axisMin;
+ calibration.max = info->axisMax;
+ calibration.reversed = info->reversed;
+
+ joystick->setCalibration(axis, calibration);
+ }
+
+ // Write function mapping parameters
+ for (int function=0; functionsetFunctionAxis((Joystick::AxisFunction_t)function, _rgFunctionAxisMapping[function]);
+ }
+
+ _stopCalibration();
+ _setInternalCalibrationValuesFromSettings();
+}
+
+/// @brief Starts the calibration process
+void JoystickConfigController::_startCalibration(void)
+{
+ Q_ASSERT(_axisCount >= _axisMinimum);
+
+ _resetInternalCalibrationValues();
+
+ _nextButton->setProperty("text", "Next");
+ _cancelButton->setEnabled(true);
+
+ _currentStep = 0;
+ _setupCurrentState();
+
+ JoystickManager::instance()->activeJoystick()->setCalibrating(true);
+}
+
+/// @brief Cancels the calibration process, setting things back to initial state.
+void JoystickConfigController::_stopCalibration(void)
+{
+ _currentStep = -1;
+
+ _setInternalCalibrationValuesFromSettings();
+
+ _statusText->setProperty("text", "");
+
+ _nextButton->setProperty("text", "Calibrate");
+ _nextButton->setEnabled(true);
+ _cancelButton->setEnabled(false);
+ _skipButton->setEnabled(false);
+
+ _setHelpImage(_imageCenter);
+
+ JoystickManager::instance()->activeJoystick()->setCalibrating(false);
+}
+
+/// @brief Saves the current axis values, so that we can detect when the use moves an input.
+void JoystickConfigController::_calSaveCurrentValues(void)
+{
+ qCDebug(JoystickConfigControllerLog) << "_calSaveCurrentValues";
+ for (unsigned i = 0; i < _axisMax; i++) {
+ _axisValueSave[i] = _axisRawValue[i];
+ }
+}
+
+/// @brief Set up the Save state of calibration.
+void JoystickConfigController::_calSave(void)
+{
+ _calState = calStateSave;
+
+ _statusText->setProperty("text",
+ "The current calibration settings are now displayed for each axis on screen.\n\n"
+ "Click the Next button to upload calibration to board. Click Cancel if you don't want to save these values.");
+
+ _nextButton->setEnabled(true);
+ _skipButton->setEnabled(false);
+ _cancelButton->setEnabled(true);
+
+ // This updates the internal values according to the validation rules. Then _updateView will tick and update ui
+ // such that the settings that will be written our are displayed.
+ _validateCalibration();
+}
+
+void JoystickConfigController::_loadSettings(void)
+{
+ QSettings settings;
+
+ settings.beginGroup(_settingsGroup);
+}
+
+void JoystickConfigController::_storeSettings(void)
+{
+ QSettings settings;
+
+ settings.beginGroup(_settingsGroup);
+}
+
+void JoystickConfigController::_setHelpImage(const char* imageFile)
+{
+ QString file = _imageFilePrefix;
+
+ file += _imageFileMode2Dir;
+ file += imageFile;
+
+ qCDebug(JoystickConfigControllerLog) << "_setHelpImage" << file;
+
+ _imageHelp = file;
+ emit imageHelpChanged(file);
+}
+
+int JoystickConfigController::axisCount(void)
+{
+ return _axisCount;
+}
+
+int JoystickConfigController::rollAxisValue(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisMax) {
+ return _axisRawValue[Joystick::rollFunction];
+ } else {
+ return 1500;
+ }
+}
+
+int JoystickConfigController::pitchAxisValue(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisMax) {
+ return _axisRawValue[Joystick::pitchFunction];
+ } else {
+ return 1500;
+ }
+}
+
+int JoystickConfigController::yawAxisValue(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisMax) {
+ return _axisRawValue[Joystick::yawFunction];
+ } else {
+ return 1500;
+ }
+}
+
+int JoystickConfigController::throttleAxisValue(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisMax) {
+ return _axisRawValue[Joystick::throttleFunction];
+ } else {
+ return 1500;
+ }
+}
+
+bool JoystickConfigController::rollAxisMapped(void)
+{
+ return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisMax;
+}
+
+bool JoystickConfigController::pitchAxisMapped(void)
+{
+ return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisMax;
+}
+
+bool JoystickConfigController::yawAxisMapped(void)
+{
+ return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisMax;
+}
+
+bool JoystickConfigController::throttleAxisMapped(void)
+{
+ return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisMax;
+}
+
+bool JoystickConfigController::rollAxisReversed(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisMax) {
+ return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].reversed;
+ } else {
+ return false;
+ }
+}
+
+bool JoystickConfigController::pitchAxisReversed(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisMax) {
+ return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].reversed;
+ } else {
+ return false;
+ }
+}
+
+bool JoystickConfigController::yawAxisReversed(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisMax) {
+ return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].reversed;
+ } else {
+ return false;
+ }
+}
+
+bool JoystickConfigController::throttleAxisReversed(void)
+{
+ if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisMax) {
+ return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].reversed;
+ } else {
+ return false;
+ }
+}
+
+void JoystickConfigController::_signalAllAttiudeValueChanges(void)
+{
+ emit rollAxisMappedChanged(rollAxisMapped());
+ emit pitchAxisMappedChanged(pitchAxisMapped());
+ emit yawAxisMappedChanged(yawAxisMapped());
+ emit throttleAxisMappedChanged(throttleAxisMapped());
+
+ emit rollAxisReversedChanged(rollAxisReversed());
+ emit pitchAxisReversedChanged(pitchAxisReversed());
+ emit yawAxisReversedChanged(yawAxisReversed());
+ emit throttleAxisReversedChanged(throttleAxisReversed());
+}
diff --git a/src/VehicleSetup/JoystickConfigController.h b/src/VehicleSetup/JoystickConfigController.h
new file mode 100644
index 0000000000000000000000000000000000000000..caeacb26e2e4293f894bf8ea0d52bbaf52ea4c05
--- /dev/null
+++ b/src/VehicleSetup/JoystickConfigController.h
@@ -0,0 +1,265 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009, 2015 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 .
+
+ ======================================================================*/
+
+
+/// @file
+/// @brief Radio Config Qml Controller
+/// @author Don Gagne
+
+#include "FactPanelController.h"
+#include "QGCLoggingCategory.h"
+#include "Joystick.h"
+
+Q_DECLARE_LOGGING_CATEGORY(JoystickConfigControllerLog)
+
+class RadioConfigest;
+
+namespace Ui {
+ class JoystickConfigController;
+}
+
+
+class JoystickConfigController : public FactPanelController
+{
+ Q_OBJECT
+
+ friend class RadioConfigTest; ///< This allows our unit test to access internal information needed.
+
+public:
+ JoystickConfigController(void);
+ ~JoystickConfigController();
+
+ Q_PROPERTY(int minAxisCount MEMBER _axisMinimum CONSTANT)
+ Q_PROPERTY(int axisCount READ axisCount NOTIFY axisCountChanged)
+
+ Q_PROPERTY(QQuickItem* statusText MEMBER _statusText)
+ Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
+ Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
+ Q_PROPERTY(QQuickItem* skipButton MEMBER _skipButton)
+
+ Q_PROPERTY(bool rollAxisMapped READ rollAxisMapped NOTIFY rollAxisMappedChanged)
+ Q_PROPERTY(bool pitchAxisMapped READ pitchAxisMapped NOTIFY pitchAxisMappedChanged)
+ Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged)
+ Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged)
+
+ Q_PROPERTY(int rollAxisValue READ rollAxisValue NOTIFY rollAxisValueChanged)
+ Q_PROPERTY(int pitchAxisValue READ pitchAxisValue NOTIFY pitchAxisValueChanged)
+ Q_PROPERTY(int yawAxisValue READ yawAxisValue NOTIFY yawAxisValueChanged)
+ Q_PROPERTY(int throttleAxisValue READ throttleAxisValue NOTIFY throttleAxisValueChanged)
+
+ Q_PROPERTY(int rollAxisReversed READ rollAxisReversed NOTIFY rollAxisReversedChanged)
+ Q_PROPERTY(int pitchAxisReversed READ pitchAxisReversed NOTIFY pitchAxisReversedChanged)
+ Q_PROPERTY(int yawAxisReversed READ yawAxisReversed NOTIFY yawAxisReversedChanged)
+ Q_PROPERTY(int throttleAxisReversed READ throttleAxisReversed NOTIFY throttleAxisReversedChanged)
+
+ Q_PROPERTY(QString imageHelp MEMBER _imageHelp NOTIFY imageHelpChanged)
+
+ Q_INVOKABLE void cancelButtonClicked(void);
+ Q_INVOKABLE void skipButtonClicked(void);
+ Q_INVOKABLE void nextButtonClicked(void);
+ Q_INVOKABLE void start(void);
+
+ int rollAxisValue(void);
+ int pitchAxisValue(void);
+ int yawAxisValue(void);
+ int throttleAxisValue(void);
+
+ bool rollAxisMapped(void);
+ bool pitchAxisMapped(void);
+ bool yawAxisMapped(void);
+ bool throttleAxisMapped(void);
+
+ bool rollAxisReversed(void);
+ bool pitchAxisReversed(void);
+ bool yawAxisReversed(void);
+ bool throttleAxisReversed(void);
+
+ int axisCount(void);
+
+signals:
+ void axisCountChanged(int axisCount);
+ void axisValueChanged(int axis, int value);
+
+ void rollAxisMappedChanged(bool mapped);
+ void pitchAxisMappedChanged(bool mapped);
+ void yawAxisMappedChanged(bool mapped);
+ void throttleAxisMappedChanged(bool mapped);
+
+ void rollAxisValueChanged(int value);
+ void pitchAxisValueChanged(int value);
+ void yawAxisValueChanged(int value);
+ void throttleAxisValueChanged(int value);
+
+ void rollAxisReversedChanged(bool reversed);
+ void pitchAxisReversedChanged(bool reversed);
+ void yawAxisReversedChanged(bool reversed);
+ void throttleAxisReversedChanged(bool reversed);
+
+ void imageHelpChanged(QString source);
+
+ // @brief Signalled when in unit test mode and a message box should be displayed by the next button
+ void nextButtonMessageBoxDisplayed(void);
+
+private slots:
+ void _axisValueChanged(int axis, int value);
+
+private:
+ /// @brief The states of the calibration state machine.
+ enum calStates {
+ calStateAxisWait,
+ calStateBegin,
+ calStateIdentify,
+ calStateMinMax,
+ calStateCenterThrottle,
+ calStateDetectInversion,
+ calStateTrims,
+ calStateSave
+ };
+
+ typedef void (JoystickConfigController::*inputFn)(Joystick::AxisFunction_t function, int axis, int value);
+ typedef void (JoystickConfigController::*buttonFn)(void);
+ struct stateMachineEntry {
+ Joystick::AxisFunction_t function;
+ const char* instructions;
+ const char* image;
+ inputFn rcInputFn;
+ buttonFn nextFn;
+ buttonFn skipFn;
+ };
+
+ /// @brief A set of information associated with a radio axis.
+ struct AxisInfo {
+ Joystick::AxisFunction_t function; ///< Function mapped to this axis, Joystick::maxFunction for none
+ bool reversed; ///< true: axis is reverse, false: not reversed
+ int axisMin; ///< Minimum axis value
+ int axisMax; ///< Maximum axis value
+ int axisTrim; ///< Trim position
+ };
+
+ int _currentStep; ///< Current step of state machine
+
+ const struct stateMachineEntry* _getStateMachineEntry(int step);
+
+ void _advanceState(void);
+ void _setupCurrentState(void);
+
+ void _inputCenterWaitBegin (Joystick::AxisFunction_t function, int axis, int value);
+ void _inputStickDetect (Joystick::AxisFunction_t function, int axis, int value);
+ void _inputStickMin (Joystick::AxisFunction_t function, int axis, int value);
+ void _inputCenterWait (Joystick::AxisFunction_t function, int axis, int value);
+
+ void _switchDetect(Joystick::AxisFunction_t function, int axis, int value, bool moveToNextStep);
+
+ void _saveFlapsDown(void);
+ void _skipFlaps(void);
+ void _saveAllTrims(void);
+
+ bool _stickSettleComplete(int value);
+
+ void _validateCalibration(void);
+ void _writeCalibration(void);
+ void _resetInternalCalibrationValues(void);
+ void _setInternalCalibrationValuesFromSettings(void);
+
+ void _startCalibration(void);
+ void _stopCalibration(void);
+ void _calSave(void);
+
+ void _calSaveCurrentValues(void);
+
+ void _setHelpImage(const char* imageFile);
+
+ void _loadSettings(void);
+ void _storeSettings(void);
+
+ void _signalAllAttiudeValueChanges(void);
+
+ // Member variables
+
+ static const char* _imageFileMode2Dir;
+ static const char* _imageFilePrefix;
+ static const char* _imageCenter;
+ static const char* _imageThrottleUp;
+ static const char* _imageThrottleDown;
+ static const char* _imageYawLeft;
+ static const char* _imageYawRight;
+ static const char* _imageRollLeft;
+ static const char* _imageRollRight;
+ static const char* _imagePitchUp;
+ static const char* _imagePitchDown;
+
+ static const char* _settingsGroup;
+
+ static const int _updateInterval; ///< Interval for ui update timer
+
+ int _rgFunctionAxisMapping[Joystick::maxFunction]; ///< Maps from joystick function to axis index. _axisMax indicates axis not set for this function.
+
+ static const int _attitudeControls = 5;
+
+ int _axisCount; ///< Number of actual joystick axes available
+ static const int _axisMax = 4; ///< Maximum number of supported joystick axes
+ static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4
+
+ struct AxisInfo _rgAxisInfo[_axisMax]; ///< Information associated with each axis
+
+ enum calStates _calState; ///< Current calibration state
+ int _calStateCurrentAxis; ///< Current axis being worked on in calStateIdentify and calStateDetectInversion
+ bool _calStateAxisComplete; ///< Work associated with current axis is complete
+ int _calStateIdentifyOldMapping; ///< Previous mapping for axis being currently identified
+ int _calStateReverseOldMapping; ///< Previous mapping for axis being currently used to detect inversion
+
+ static const int _calCenterPoint;
+ static const int _calValidMinValue;
+ static const int _calValidMaxValue;
+ static const int _calDefaultMinValue;
+ static const int _calDefaultMaxValue;
+ static const int _calRoughCenterDelta;
+ static const int _calMoveDelta;
+ static const int _calSettleDelta;
+ static const int _calMinDelta;
+
+ int _axisValueSave[_axisMax]; ///< Saved values prior to detecting axis movement
+
+ int _axisRawValue[_axisMax]; ///< Current set of raw axis values
+
+ int _stickDetectAxis;
+ int _stickDetectInitialValue;
+ int _stickDetectValue;
+ bool _stickDetectSettleStarted;
+ QTime _stickDetectSettleElapsed;
+ static const int _stickDetectSettleMSecs;
+
+ QQuickItem* _statusText;
+ QQuickItem* _cancelButton;
+ QQuickItem* _nextButton;
+ QQuickItem* _skipButton;
+
+ QString _imageHelp;
+};
+
+#endif // JoystickConfigController_H
diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml
index 59ffb31723855e8b6dc61edc2cbe8cad3934ac5b..05c6ad64f7e19c07214c8cf074efdb6506c19014 100644
--- a/src/VehicleSetup/SetupView.qml
+++ b/src/VehicleSetup/SetupView.qml
@@ -72,6 +72,16 @@ Rectangle {
}
}
+ function showJoystickPanel()
+ {
+ if (multiVehicleManager.activeVehicleAvailable && multiVehicleManager.activeVehicle.autopilot.armed) {
+ messagePanelText = armedVehicleText
+ panelLoader.sourceComponent = messagePanelComponent
+ } else {
+ panelLoader.source = "JoystickConfig.qml";
+ }
+ }
+
function showParametersPanel()
{
panelLoader.source = "SetupParameterEditor.qml";
@@ -170,6 +180,18 @@ Rectangle {
onClicked: showFirmwarePanel()
}
+ SubMenuButton {
+ id: joystickButton
+ width: buttonWidth
+ setupIndicator: true
+ setupComplete: joystickManager.activeJoystick.calibrated
+ exclusiveGroup: setupButtonGroup
+ visible: multiVehicleManager.parameterReadyVehicleAvailable && joystickManager.joysticks.length != 0
+ text: "JOYSTICK"
+
+ onClicked: showJoystickPanel()
+ }
+
Repeater {
model: multiVehicleManager.parameterReadyVehicleAvailable ? multiVehicleManager.activeVehicle.autopilot.vehicleComponents : 0
diff --git a/src/VehicleSetup/SetupViewButtonsConnected.qml b/src/VehicleSetup/SetupViewButtonsConnected.qml
deleted file mode 100644
index df5514e4c861f905795904b40a2a53deade80dd4..0000000000000000000000000000000000000000
--- a/src/VehicleSetup/SetupViewButtonsConnected.qml
+++ /dev/null
@@ -1,62 +0,0 @@
-import QtQuick 2.3
-import QtQuick.Controls 1.2
-import QtQuick.Controls.Styles 1.2
-import QtGraphicalEffects 1.0
-
-import QGroundControl.FactSystem 1.0
-import QGroundControl.Palette 1.0
-import QGroundControl.Controls 1.0
-
-Rectangle {
- id: topLevel
-
- QGCPalette { id: palette; colorGroupEnabled: true }
- color: palette.window
-
- ExclusiveGroup { id: setupButtonGroup }
-
- Column {
- anchors.fill: parent
-
- SubMenuButton {
- id: summaryButton; objectName: "summaryButton"
- width: parent.width
- text: "SUMMARY"
- imageResource: "VehicleSummaryIcon.png"
- setupIndicator: false
- exclusiveGroup: setupButtonGroup
- onClicked: controller.summaryButtonClicked()
- }
-
- SubMenuButton {
- id: firmwareButton; objectName: "firmwareButton"
- width: parent.width
- text: "FIRMWARE"
- imageResource: "FirmwareUpgradeIcon.png"
- setupIndicator: false
- exclusiveGroup: setupButtonGroup
- onClicked: controller.firmwareButtonClicked()
- }
-
- Repeater {
- model: autopilot ? autopilot.vehicleComponents : 0
-
- SubMenuButton {
- width: parent.width
- text: modelData.name.toUpperCase()
- imageResource: modelData.iconResource
- setupComplete: modelData.setupComplete
- exclusiveGroup: setupButtonGroup
- onClicked: controller.setupButtonClicked(modelData)
- }
- }
-
- SubMenuButton {
- width: parent.width
- text: "PARAMETERS"
- setupIndicator: false
- exclusiveGroup: setupButtonGroup
- onClicked: controller.parametersButtonClicked()
- }
- }
-}
diff --git a/src/VehicleSetup/SetupViewButtonsDisconnected.qml b/src/VehicleSetup/SetupViewButtonsDisconnected.qml
deleted file mode 100644
index e5634ccbf77750a9da7356ca800a9ecea74c3a08..0000000000000000000000000000000000000000
--- a/src/VehicleSetup/SetupViewButtonsDisconnected.qml
+++ /dev/null
@@ -1,40 +0,0 @@
-import QtQuick 2.3
-import QtQuick.Controls 1.2
-import QtQuick.Controls.Styles 1.2
-import QtGraphicalEffects 1.0
-
-import QGroundControl.FactSystem 1.0
-import QGroundControl.Palette 1.0
-import QGroundControl.Controls 1.0
-
-Rectangle {
- id: topLevel
-
- QGCPalette { id: palette; colorGroupEnabled: true }
- color: palette.window
-
- ExclusiveGroup { id: setupButtonGroup }
-
- Column {
- anchors.fill: parent
-
- SubMenuButton {
- id: firmwareButton; objectName: "firmwareButton"
- width: parent.width
- text: "FIRMWARE"
- imageResource: "FirmwareUpgradeIcon.png"
- setupIndicator: false
- exclusiveGroup: setupButtonGroup
- onClicked: controller.firmwareButtonClicked()
- }
-
- Item { width: parent.width; height: 10 } // spacer
-
- QGCLabel {
- width: parent.width
- text: "Full setup options are only available when connected to vehicle and full parameter list has completed downloading."
- wrapMode: Text.WordWrap
- horizontalAlignment: Text.AlignHCenter
- }
- }
-}
diff --git a/src/ViewWidgets/CustomCommandWidgetController.cc b/src/ViewWidgets/CustomCommandWidgetController.cc
index 538709d3713a1cca98f3c74b04f3cb849b44de5e..80c482cff01a11262ed9d596c6f6d7e36b70a341 100644
--- a/src/ViewWidgets/CustomCommandWidgetController.cc
+++ b/src/ViewWidgets/CustomCommandWidgetController.cc
@@ -25,6 +25,7 @@
#include "MultiVehicleManager.h"
#include "QGCMAVLink.h"
#include "QGCFileDialog.h"
+#include "UAS.h"
#include
#include
diff --git a/src/ViewWidgets/ViewWidgetController.cc b/src/ViewWidgets/ViewWidgetController.cc
index 71295f244f179a520d5d812f6db028e0dba3047c..5a39032d669b9e9e4f353e4b5d739719533d6172 100644
--- a/src/ViewWidgets/ViewWidgetController.cc
+++ b/src/ViewWidgets/ViewWidgetController.cc
@@ -23,6 +23,7 @@
#include "ViewWidgetController.h"
#include "MultiVehicleManager.h"
+#include "UAS.h"
ViewWidgetController::ViewWidgetController(void) :
_autopilot(NULL),
diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc
deleted file mode 100644
index ad950d4297bb813028aedda0211a26241a596c51..0000000000000000000000000000000000000000
--- a/src/input/JoystickInput.cc
+++ /dev/null
@@ -1,764 +0,0 @@
-/*=====================================================================
-======================================================================*/
-
-/**
- * @file
- * @brief Joystick interface
- *
- * @author Lorenz Meier
- * @author Andreas Romer
- * @author Julian Oes
-#include
-#include "UAS.h"
-#include "MultiVehicleManager.h"
-#include "QGC.h"
-#include
-#include
-#include
-#include
-
-using namespace std;
-
-/**
- * The coordinate frame of the joystick axis is the aeronautical frame like shown on this image:
- * @image html http://pixhawk.ethz.ch/wiki/_media/standards/body-frame.png Aeronautical frame
- */
-JoystickInput::JoystickInput() :
- sdlJoystickMin(-32768.0f),
- sdlJoystickMax(32767.0f),
- isEnabled(false),
- isCalibrating(false),
- done(false),
- uas(NULL),
- autopilotType(0),
- systemType(0),
- uasCanReverse(false),
- rollAxis(-1),
- pitchAxis(-1),
- yawAxis(-1),
- throttleAxis(-1),
- joystickName(""),
- joystickID(-1),
- joystickNumButtons(0)
-{
-
- // Make sure we initialize with the correct UAS.
- _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle());
-
- // Start this thread. This allows the Joystick Settings window to work correctly even w/o any UASes connected.
- start();
-}
-
-JoystickInput::~JoystickInput()
-{
- storeGeneralSettings();
- storeJoystickSettings();
- done = true;
-}
-
-void JoystickInput::loadGeneralSettings()
-{
- // Load defaults from settings
- QSettings settings;
-
- // Deal with settings specific to the JoystickInput
- settings.beginGroup("JOYSTICK_INPUT");
- isEnabled = settings.value("ENABLED", false).toBool();
- joystickName = settings.value("JOYSTICK_NAME", "").toString();
- mode = (JOYSTICK_MODE)settings.value("JOYSTICK_MODE", JOYSTICK_MODE_ATTITUDE).toInt();
- settings.endGroup();
-}
-
-/**
- * @brief Restores settings for the current joystick from saved settings file.
- * Assumes that both joystickName & joystickNumAxes are correct.
- */
-void JoystickInput::loadJoystickSettings()
-{
- // Load defaults from settings
- QSettings settings;
-
- // Now for the current joystick
- settings.beginGroup(joystickName);
- rollAxis = (settings.value("ROLL_AXIS_MAPPING", -1).toInt());
- pitchAxis = (settings.value("PITCH_AXIS_MAPPING", -1).toInt());
- yawAxis = (settings.value("YAW_AXIS_MAPPING", -1).toInt());
- throttleAxis = (settings.value("THROTTLE_AXIS_MAPPING", -1).toInt());
-
- // Clear out and then restore the (AUTOPILOT, SYSTEM) mapping for joystick settings
- joystickSettings.clear();
- int autopilots = settings.beginReadArray("AUTOPILOTS");
- for (int i = 0; i < autopilots; i++)
- {
- settings.setArrayIndex(i);
- int autopilotType = settings.value("AUTOPILOT_TYPE", 0).toInt();
- int systems = settings.beginReadArray("SYSTEMS");
- for (int j = 0; j < systems; j++)
- {
- settings.setArrayIndex(j);
- int systemType = settings.value("SYSTEM_TYPE", 0).toInt();
-
- // Now that both the autopilot and system type are available, update some references.
- QMap* joystickAxesInverted = &joystickSettings[autopilotType][systemType].axesInverted;
- QMap* joystickAxesLimited = &joystickSettings[autopilotType][systemType].axesLimited;
- QMap* joystickAxesMinRange = &joystickSettings[autopilotType][systemType].axesMaxRange;
- QMap* joystickAxesMaxRange = &joystickSettings[autopilotType][systemType].axesMinRange;
- QMap* joystickButtonActions = &joystickSettings[autopilotType][systemType].buttonActions;
-
- // Read back the joystickAxesInverted QList one element at a time.
- int axesStored = settings.beginReadArray("AXES_INVERTED");
- for (int k = 0; k < axesStored; k++)
- {
- settings.setArrayIndex(k);
- int index = settings.value("INDEX", 0).toInt();
- bool inverted = settings.value("INVERTED", false).toBool();
- joystickAxesInverted->insert(index, inverted);
- }
- settings.endArray();
-
- // Read back the joystickAxesLimited QList one element at a time.
- axesStored = settings.beginReadArray("AXES_LIMITED");
- for (int k = 0; k < axesStored; k++)
- {
- settings.setArrayIndex(k);
- int index = settings.value("INDEX", 0).toInt();
- bool limited = settings.value("LIMITED", false).toBool();
- joystickAxesLimited->insert(index, limited);
- }
- settings.endArray();
-
- // Read back the joystickAxesMinRange QList one element at a time.
- axesStored = settings.beginReadArray("AXES_MIN_RANGE");
- for (int k = 0; k < axesStored; k++)
- {
- settings.setArrayIndex(k);
- int index = settings.value("INDEX", 0).toInt();
- float min = settings.value("MIN_RANGE", false).toFloat();
- joystickAxesMinRange->insert(index, min);
- }
- settings.endArray();
-
- // Read back the joystickAxesMaxRange QList one element at a time.
- axesStored = settings.beginReadArray("AXES_MAX_RANGE");
- for (int k = 0; k < axesStored; k++)
- {
- settings.setArrayIndex(k);
- int index = settings.value("INDEX", 0).toInt();
- float max = settings.value("MAX_RANGE", false).toFloat();
- joystickAxesMaxRange->insert(index, max);
- }
- settings.endArray();
-
- // Read back the button->action mapping.
- int buttonsStored = settings.beginReadArray("BUTTONS_ACTIONS");
- for (int k = 0; k < buttonsStored; k++)
- {
- settings.setArrayIndex(k);
- int index = settings.value("INDEX", 0).toInt();
- int action = settings.value("ACTION", 0).toInt();
- joystickButtonActions->insert(index, action);
- }
- settings.endArray();
- }
- settings.endArray();
- }
- settings.endArray();
-
- settings.endGroup();
-
- emit joystickSettingsChanged();
-}
-
-void JoystickInput::storeGeneralSettings() const
-{
- QSettings settings;
- settings.beginGroup("JOYSTICK_INPUT");
- settings.setValue("ENABLED", isEnabled);
- settings.setValue("JOYSTICK_NAME", joystickName);
- settings.setValue("JOYSTICK_MODE", mode);
- settings.endGroup();
-}
-
-void JoystickInput::storeJoystickSettings() const
-{
- // Store settings
- QSettings settings;
- settings.beginGroup(joystickName);
- settings.setValue("ROLL_AXIS_MAPPING", rollAxis);
- settings.setValue("PITCH_AXIS_MAPPING", pitchAxis);
- settings.setValue("YAW_AXIS_MAPPING", yawAxis);
- settings.setValue("THROTTLE_AXIS_MAPPING", throttleAxis);
- settings.beginWriteArray("AUTOPILOTS");
- QMapIterator > i(joystickSettings);
- int autopilotIndex = 0;
- while (i.hasNext())
- {
- i.next();
- settings.setArrayIndex(autopilotIndex++);
-
- int autopilotType = i.key();
- settings.setValue("AUTOPILOT_TYPE", autopilotType);
-
- settings.beginWriteArray("SYSTEMS");
- QMapIterator j(i.value());
- int systemIndex = 0;
- while (j.hasNext())
- {
- j.next();
- settings.setArrayIndex(systemIndex++);
-
- int systemType = j.key();
- settings.setValue("SYSTEM_TYPE", systemType);
-
- // Now that both the autopilot and system type are available, update some references.
- QMapIterator joystickAxesInverted(joystickSettings[autopilotType][systemType].axesInverted);
- QMapIterator joystickAxesLimited(joystickSettings[autopilotType][systemType].axesLimited);
- QMapIterator joystickAxesMinRange(joystickSettings[autopilotType][systemType].axesMaxRange);
- QMapIterator joystickAxesMaxRange(joystickSettings[autopilotType][systemType].axesMinRange);
- QMapIterator joystickButtonActions(joystickSettings[autopilotType][systemType].buttonActions);
-
- settings.beginWriteArray("AXES_INVERTED");
- int k = 0;
- while (joystickAxesInverted.hasNext())
- {
- joystickAxesInverted.next();
- int inverted = joystickAxesInverted.value();
- // Only save this axes' inversion status if it's not the default
- if (inverted)
- {
- settings.setArrayIndex(k++);
- int index = joystickAxesInverted.key();
- settings.setValue("INDEX", index);
- settings.setValue("INVERTED", inverted);
- }
- }
- settings.endArray();
-
- settings.beginWriteArray("AXES_MIN_RANGE");
- k = 0;
- while (joystickAxesMinRange.hasNext())
- {
- joystickAxesMinRange.next();
- float min = joystickAxesMinRange.value();
- settings.setArrayIndex(k++);
- int index = joystickAxesMinRange.key();
- settings.setValue("INDEX", index);
- settings.setValue("MIN_RANGE", min);
- }
- settings.endArray();
-
- settings.beginWriteArray("AXES_MAX_RANGE");
- k = 0;
- while (joystickAxesMaxRange.hasNext())
- {
- joystickAxesMaxRange.next();
- float max = joystickAxesMaxRange.value();
- settings.setArrayIndex(k++);
- int index = joystickAxesMaxRange.key();
- settings.setValue("INDEX", index);
- settings.setValue("MAX_RANGE", max);
- }
- settings.endArray();
-
- settings.beginWriteArray("AXES_LIMITED");
- k = 0;
- while (joystickAxesLimited.hasNext())
- {
- joystickAxesLimited.next();
- int limited = joystickAxesLimited.value();
- if (limited)
- {
- settings.setArrayIndex(k++);
- int index = joystickAxesLimited.key();
- settings.setValue("INDEX", index);
- settings.setValue("LIMITED", limited);
- }
- }
- settings.endArray();
-
- settings.beginWriteArray("BUTTONS_ACTIONS");
- k = 0;
- while (joystickButtonActions.hasNext())
- {
- joystickButtonActions.next();
- int action = joystickButtonActions.value();
- if (action != -1)
- {
- settings.setArrayIndex(k++);
- int index = joystickButtonActions.key();
- settings.setValue("INDEX", index);
- settings.setValue("ACTION", action);
- }
- }
- settings.endArray();
- }
- settings.endArray(); // SYSTEMS
- }
- settings.endArray(); // AUTOPILOTS
- settings.endGroup();
-}
-
-void JoystickInput::_activeVehicleChanged(Vehicle* vehicle)
-{
- if (this->uas)
- {
- disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), uas, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8)));
- disconnect(this, SIGNAL(actionTriggered(int)), uas, SLOT(triggerAction(int)));
- uasCanReverse = false;
- uas = NULL;
- }
-
- // Save any settings for the last UAS
- if (joystickID > -1)
- {
- storeJoystickSettings();
- }
-
- if (vehicle)
- {
- uas = vehicle->uas();
- connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16,quint8)), uas, SLOT(setExternalControlSetpoint(float,float,float,float,qint8,qint8,quint16,quint8)));
- qDebug() << "connected joystick";
- connect(this, SIGNAL(actionTriggered(int)), uas, SLOT(triggerAction(int)));
- uasCanReverse = uas->systemCanReverse();
-
- // Update the joystick settings for a new UAS.
- autopilotType = uas->getAutopilotType();
- systemType = uas->getSystemType();
- }
-
- // Make sure any UI elements know we've updated the UAS. The signal is re-emitted here so that UI elements know to
- // update their UAS-specific UI.
- emit activeVehicleChanged(vehicle);
-
- // Load any joystick-specific settings now that the UAS has changed.
- if (joystickID > -1)
- {
- loadJoystickSettings();
- }
-}
-
-void JoystickInput::setEnabled(bool enabled)
-{
- this->isEnabled = enabled;
- storeJoystickSettings();
-}
-
-void JoystickInput::init()
-{
- // Initialize SDL Joystick support and detect number of joysticks.
- if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
- printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
- }
-
- // Enumerate joysticks and select one
- numJoysticks = SDL_NumJoysticks();
-
- // If no joysticks are connected, there's nothing we can do, so just keep
- // going back to sleep every second unless the program quits.
- while (!numJoysticks && !done)
- {
- QGC::SLEEP::sleep(1);
- }
- if (done)
- {
- return;
- }
-
- // Now that we've detected a joystick, load in the joystick-agnostic settings.
- loadGeneralSettings();
-
- // Enumerate all found devices
- qDebug() << QString("%1 Input devices found:").arg(numJoysticks);
- int activeJoystick = 0;
- for(int i=0; i < numJoysticks; i++ )
- {
- QString name = SDL_JoystickName(i);
- qDebug() << QString("\t%1").arg(name);
-
- // If we've matched this joystick to what was last opened, note it.
- // Note: The way this is implemented the LAST joystick of a given name will be opened.
- if (name == joystickName)
- {
- activeJoystick = i;
- }
-
- SDL_Joystick* x = SDL_JoystickOpen(i);
- qDebug() << QString("\tNumber of Axes: %1").arg(QString::number(SDL_JoystickNumAxes(x)));
- qDebug() << QString("\tNumber of Buttons: %1").arg(QString::number(SDL_JoystickNumButtons(x)));
- SDL_JoystickClose(x);
- }
-
- // Set the active joystick based on name, if a joystick was found in the saved settings, otherwise
- // default to the first one.
- setActiveJoystick(activeJoystick);
-
- // Now make sure we know what the current UAS is and track changes to it.
- _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle());
- connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &JoystickInput::_activeVehicleChanged);
-}
-
-void JoystickInput::shutdown()
-{
- done = true;
-}
-
-/**
- * @brief Execute the Joystick process
- * Note that the SDL procedure is polled. This is because connecting and disconnecting while the event checker is running
- * fails as of SDL 1.2. It is therefore much easier to just poll for the joystick we want to sample.
- */
-void JoystickInput::run()
-{
- init();
-
- forever
- {
- if (done)
- {
- done = false;
- exit();
- return;
- }
-
- // Poll the joystick for new values.
- SDL_JoystickUpdate();
-
- // Emit all necessary signals for all axes.
- for (int i = 0; i < joystickNumAxes; i++)
- {
- // First emit the uncalibrated values for each axis based on their ID.
- // This is generally not used for controlling a vehicle, but a UI representation, so it being slightly off is fine.
- // Here we map the joystick axis value into the initial range of [0:1].
- float axisValue = SDL_JoystickGetAxis(joystick, i);
-
- // during calibration save min and max values
- if (isCalibrating)
- {
- if (joystickSettings[autopilotType][systemType].axesMinRange.value(i) > axisValue)
- {
- joystickSettings[autopilotType][systemType].axesMinRange[i] = axisValue;
- }
-
- if (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) < axisValue)
- {
- joystickSettings[autopilotType][systemType].axesMaxRange[i] = axisValue;
- }
- }
-
- if (joystickSettings[autopilotType][systemType].axesInverted[i])
- {
- axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMinRange.value(i)) /
- (joystickSettings[autopilotType][systemType].axesMaxRange.value(i) -
- joystickSettings[autopilotType][systemType].axesMinRange.value(i));
- }
- else
- {
- axisValue = (axisValue - joystickSettings[autopilotType][systemType].axesMaxRange.value(i)) /
- (joystickSettings[autopilotType][systemType].axesMinRange.value(i) -
- joystickSettings[autopilotType][systemType].axesMaxRange.value(i));
- }
- axisValue = 1.0f - axisValue;
-
- // For non-throttle axes or if the UAS can reverse, go ahead and convert this into the range [-1:1].
-
- //if (uasCanReverse || throttleAxis != i)
- // don't take into account if UAS can reverse. This means to reverse position but not throttle
- // therefore deactivated for now
- if (throttleAxis != i)
- {
- axisValue = axisValue * 2.0f - 1.0f;
- }
- // Otherwise if this vehicle can only go forward, scale it to [0:1].
- else if (throttleAxis == i && joystickSettings[autopilotType][systemType].axesLimited.value(i))
- {
- if (axisValue < 0.0f)
- {
- axisValue = 0.0f;
- }
- }
-
- // Bound rounding errors
- if (axisValue > 1.0f) axisValue = 1.0f;
- if (axisValue < -1.0f) axisValue = -1.0f;
- if (joystickAxes[i] != axisValue)
- {
- joystickAxes[i] = axisValue;
- emit axisValueChanged(i, axisValue);
- }
- }
-
- // Build up vectors describing the hat position
- int hatPosition = SDL_JoystickGetHat(joystick, 0);
- qint8 newYHat = 0;
- if ((SDL_HAT_UP & hatPosition) > 0) newYHat = 1;
- if ((SDL_HAT_DOWN & hatPosition) > 0) newYHat = -1;
- qint8 newXHat = 0;
- if ((SDL_HAT_LEFT & hatPosition) > 0) newXHat = -1;
- if ((SDL_HAT_RIGHT & hatPosition) > 0) newXHat = 1;
- if (newYHat != yHat || newXHat != xHat)
- {
- xHat = newXHat;
- yHat = newYHat;
- emit hatDirectionChanged(newXHat, newYHat);
- }
-
- // Emit signals for each button individually
- for (int i = 0; i < joystickNumButtons; i++)
- {
- // If the button was down, but now it's up, trigger a buttonPressed event
- quint16 lastButtonState = joystickButtons & (1 << i);
- if (SDL_JoystickGetButton(joystick, i) && !lastButtonState)
- {
- emit buttonPressed(i);
- joystickButtons |= 1 << i;
- }
- else if (!SDL_JoystickGetButton(joystick, i) && lastButtonState)
- {
- emit buttonReleased(i);
- if (isEnabled && joystickSettings[autopilotType][systemType].buttonActions.contains(i))
- {
- emit actionTriggered(joystickSettings[autopilotType][systemType].buttonActions.value(i));
- }
- joystickButtons &= ~(1 << i);
- }
- }
-
- // Now signal an update for all UI together.
- if (isEnabled)
- {
- float roll = rollAxis > -1?joystickAxes[rollAxis]:numeric_limits::quiet_NaN();
- float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:numeric_limits::quiet_NaN();
- float yaw = yawAxis > -1?joystickAxes[yawAxis]:numeric_limits::quiet_NaN();
- float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:numeric_limits::quiet_NaN();
- emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons, mode);
- }
-
- // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms)
- QGC::SLEEP::msleep(40);
- }
-}
-
-void JoystickInput::setActiveJoystick(int id)
-{
- // If we already had a joystick, close that one before opening a new one.
- if (joystick && SDL_JoystickOpened(joystickID))
- {
- storeJoystickSettings();
- SDL_JoystickClose(joystick);
- joystick = NULL;
- joystickID = -1;
- }
-
- joystickID = id;
- joystick = SDL_JoystickOpen(joystickID);
- if (joystick && SDL_JoystickOpened(joystickID))
- {
- // Update joystick configuration.
- joystickName = QString(SDL_JoystickName(joystickID));
- joystickNumButtons = SDL_JoystickNumButtons(joystick);
- joystickNumAxes = SDL_JoystickNumAxes(joystick);
-
- // Restore saved settings for this joystick.
- loadJoystickSettings();
-
- // Update cached joystick axes values.
- // Also emit any signals for currently-triggering events
- joystickAxes.clear();
- for (int i = 0; i < joystickNumAxes; i++)
- {
- joystickAxes.append(numeric_limits::quiet_NaN());
- }
-
- // Update cached joystick button values.
- // Emit signals for any button events.
- joystickButtons = 0;
- }
- else
- {
- joystickNumButtons = 0;
- joystickNumAxes = 0;
- }
-
- // Specify that a new joystick has been selected, so that any UI elements can update.
- emit newJoystickSelected();
- // And then trigger an update of this new UI.
- emit joystickSettingsChanged();
-}
-
-void JoystickInput::setCalibrating(bool active)
-{
- if (active)
- {
- setEnabled(false);
- isCalibrating = true;
-
- // set range small so that limits can be re-found
- for (int i = 0; i < joystickNumAxes; i++)
- {
- joystickSettings[autopilotType][systemType].axesMinRange[i] = -10.0f;
- joystickSettings[autopilotType][systemType].axesMaxRange[i] = 10.0f;
- }
-
- } else {
-
- // store calibration values
- storeJoystickSettings();
-
- qDebug() << "Calibration result:";
- for (int i = 0; i < joystickNumAxes; i++)
- {
- qDebug() << i << ": " <<
- joystickSettings[autopilotType][systemType].axesMinRange[i] <<
- " - " <<
- joystickSettings[autopilotType][systemType].axesMaxRange[i];
- }
- setEnabled(true);
- isCalibrating = false;
- }
-}
-
-void JoystickInput::setAxisMapping(int axis, JOYSTICK_INPUT_MAPPING newMapping)
-{
- switch (newMapping)
- {
- case JOYSTICK_INPUT_MAPPING_ROLL:
- rollAxis = axis;
- break;
- case JOYSTICK_INPUT_MAPPING_PITCH:
- pitchAxis = axis;
- break;
- case JOYSTICK_INPUT_MAPPING_YAW:
- yawAxis = axis;
- break;
- case JOYSTICK_INPUT_MAPPING_THROTTLE:
- throttleAxis = axis;
- break;
- case JOYSTICK_INPUT_MAPPING_NONE:
- default:
- if (rollAxis == axis)
- {
- rollAxis = -1;
- }
- if (pitchAxis == axis)
- {
- pitchAxis = -1;
- }
- if (yawAxis == axis)
- {
- yawAxis = -1;
- }
- if (throttleAxis == axis)
- {
- throttleAxis = -1;
- joystickSettings[autopilotType][systemType].axesLimited.remove(axis);
- }
- break;
- }
- storeJoystickSettings();
-}
-
-void JoystickInput::setAxisInversion(int axis, bool inverted)
-{
- if (axis < joystickNumAxes)
- {
- joystickSettings[autopilotType][systemType].axesInverted[axis] = inverted;
- storeJoystickSettings();
- }
-}
-
-void JoystickInput::setAxisRangeLimit(int axis, bool limitRange)
-{
- if (axis < joystickNumAxes)
- {
- joystickSettings[autopilotType][systemType].axesLimited[axis] = limitRange;
- storeJoystickSettings();
- }
-}
-
-void JoystickInput::setAxisRangeLimitMin(int axis, float min)
-{
- if (axis < joystickNumAxes)
- {
- joystickSettings[autopilotType][systemType].axesMinRange[axis] = min;
- storeJoystickSettings();
- }
-}
-
-void JoystickInput::setAxisRangeLimitMax(int axis, float max)
-{
- if (axis < joystickNumAxes)
- {
- joystickSettings[autopilotType][systemType].axesMaxRange[axis] = max;
- storeJoystickSettings();
- }
-}
-
-void JoystickInput::setButtonAction(int button, int action)
-{
- if (button < joystickNumButtons)
- {
- joystickSettings[autopilotType][systemType].buttonActions[button] = action;
- storeJoystickSettings();
- }
-}
-
-float JoystickInput::getCurrentValueForAxis(int axis) const
-{
- if (axis < joystickNumAxes)
- {
- return joystickAxes.at(axis);
- }
- return 0.0f;
-}
-
-bool JoystickInput::getInvertedForAxis(int axis) const
-{
- if (axis < joystickNumAxes)
- {
- return joystickSettings[autopilotType][systemType].axesInverted.value(axis);
- }
- return false;
-}
-
-bool JoystickInput::getRangeLimitForAxis(int axis) const
-{
- if (axis < joystickNumAxes)
- {
- return joystickSettings[autopilotType][systemType].axesLimited.value(axis);
- }
- return false;
-}
-
-float JoystickInput::getAxisRangeLimitMinForAxis(int axis) const
-{
- if (axis < joystickNumAxes)
- {
- return joystickSettings[autopilotType][systemType].axesMinRange.value(axis);
- }
- return sdlJoystickMin;
-}
-
-float JoystickInput::getAxisRangeLimitMaxForAxis(int axis) const
-{
- if (axis < joystickNumAxes)
- {
- return joystickSettings[autopilotType][systemType].axesMaxRange.value(axis);
- }
- return sdlJoystickMax;
-}
-
-int JoystickInput::getActionForButton(int button) const
-{
- if (button < joystickNumButtons && joystickSettings[autopilotType][systemType].buttonActions.contains(button))
- {
- return joystickSettings[autopilotType][systemType].buttonActions.value(button);
- }
- return -1;
-}
diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h
deleted file mode 100644
index c3ebc7c7a1c059a008f569128ef12e98d6996806..0000000000000000000000000000000000000000
--- a/src/input/JoystickInput.h
+++ /dev/null
@@ -1,378 +0,0 @@
-/*=====================================================================
-
-PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
-
-(c) 2009, 2010 PIXHAWK PROJECT
-
-This file is part of the PIXHAWK project
-
- PIXHAWK 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.
-
- PIXHAWK 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 PIXHAWK. If not, see .
-
-======================================================================*/
-
-/**
- * @file
- * @brief Definition of joystick interface
- *
- * This class defines a new thread to operate the reading of any joystick/controllers
- * via the Simple Directmedia Library (libsdl.org). This relies on polling of the SDL,
- * which is not their recommended method, instead they suggest use event checking. That
- * does not seem to support switching joysticks after their internal event loop has started,
- * so it was abandoned.
- *
- * All joystick-related functionality is done in this class, though the JoystickWidget provides
- * a UI around modifying its settings. Additionally controller buttons can be mapped to
- * actions defined by any UASInterface object through the `UASInterface::getActions()` function.
- *
- * @author Lorenz Meier
- * @author Andreas Romer
- * @author Julian Oes
- */
-
-#ifndef _JOYSTICKINPUT_H_
-#define _JOYSTICKINPUT_H_
-
-#include
-#include
-#include
-#ifdef Q_OS_MAC
-#include
-#else
-#include
-#endif
-
-#include "UAS.h"
-
-struct JoystickSettings {
- QMap axesInverted; ///< Whether each axis should be used inverted from what was reported.
- QMap axesLimited; ///< Whether each axis should be limited to only the positive range. Currently this only applies to the throttle axis, but is kept generic here to possibly support other axes.
- QMap axesMaxRange; ///< The maximum values per axis
- QMap axesMinRange; ///< The minimum values per axis
- QMap buttonActions; ///< The index of the action associated with every button.
-};
-Q_DECLARE_METATYPE(JoystickSettings)
-
-/**
- * @brief Joystick input
- */
-class JoystickInput : public QThread
-{
- Q_OBJECT
-
-public:
- JoystickInput();
- ~JoystickInput();
- void run();
- void shutdown();
-
- /**
- * @brief The JOYSTICK_INPUT_MAPPING enum storing the values for each item in the mapping combobox.
- * This should match the order of items in the mapping combobox in JoystickAxis.ui.
- */
- enum JOYSTICK_INPUT_MAPPING
- {
- JOYSTICK_INPUT_MAPPING_NONE = 0,
- JOYSTICK_INPUT_MAPPING_YAW = 1,
- JOYSTICK_INPUT_MAPPING_PITCH = 2,
- JOYSTICK_INPUT_MAPPING_ROLL = 3,
- JOYSTICK_INPUT_MAPPING_THROTTLE = 4
- };
-
- /**
- * @brief The JOYSTICK_MODE enum stores the values for each item in the mode combobox.
- * This should match the order of items in the mode combobox in JoystickWidget.ui.
- */
- enum JOYSTICK_MODE
- {
- JOYSTICK_MODE_ATTITUDE = 0,
- JOYSTICK_MODE_POSITION = 1,
- JOYSTICK_MODE_FORCE = 2,
- JOYSTICK_MODE_VELOCITY = 3,
- JOYSTICK_MODE_MANUAL = 4
- };
-
- /**
- * @brief Load joystick-specific settings.
- */
- void loadJoystickSettings();
- /**
- * @brief Load joystick-independent settings.
- */
- void loadGeneralSettings();
-
- /**
- * @brief Store joystick-specific settings.
- */
- void storeJoystickSettings() const;
- /**
- * @brief Store joystick-independent settings.
- */
- void storeGeneralSettings() const;
-
- bool enabled() const
- {
- return isEnabled;
- }
-
- bool calibrating() const
- {
- return isCalibrating;
- }
-
- int getMappingThrottleAxis() const
- {
- return throttleAxis;
- }
-
- int getMappingRollAxis() const
- {
- return rollAxis;
- }
-
- int getMappingPitchAxis() const
- {
- return pitchAxis;
- }
-
- int getMappingYawAxis() const
- {
- return yawAxis;
- }
-
- int getJoystickNumButtons() const
- {
- return joystickNumButtons;
- }
-
- int getJoystickNumAxes() const
- {
- return joystickNumAxes;
- }
-
- int getJoystickID() const
- {
- return joystickID;
- }
-
- const QString& getName() const
- {
- return joystickName;
- }
-
- int getNumJoysticks() const
- {
- return numJoysticks;
- }
-
- JOYSTICK_MODE getMode() const
- {
- return mode;
- }
-
- QString getJoystickNameById(int id) const
- {
- return QString(SDL_JoystickName(id));
- }
-
- float getCurrentValueForAxis(int axis) const;
- bool getInvertedForAxis(int axis) const;
- bool getRangeLimitForAxis(int axis) const;
- float getAxisRangeLimitMinForAxis(int axis) const;
- float getAxisRangeLimitMaxForAxis(int axis) const;
- int getActionForButton(int button) const;
-
- const double sdlJoystickMin;
- const double sdlJoystickMax;
-
-protected:
-
- bool isEnabled; ///< Track whether the system should emit the higher-level signals: joystickChanged & actionTriggered.
- bool isCalibrating; ///< Track if calibration in progress
- bool done;
-
- SDL_Joystick* joystick;
- UAS* uas; ///< Track the current UAS.
- int autopilotType; ///< Cache the autopilotType
- int systemType; ///< Cache the systemType
- bool uasCanReverse; ///< Track whether the connect UAS can drive a reverse speed.
-
- // Store the mapping between axis numbers and the roll/pitch/yaw/throttle configuration.
- // Value is one of JoystickAxis::JOYSTICK_INPUT_MAPPING.
- int rollAxis;
- int pitchAxis;
- int yawAxis;
- int throttleAxis;
-
- // Cache information on the joystick instead of polling the SDL everytime.
- int numJoysticks; ///< Total number of joysticks detected by the SDL.
- QString joystickName;
- int joystickID;
- int joystickNumAxes;
- int joystickNumButtons;
-
- // mode of joystick (attitude, position, force, ... (see JOYSTICK_MODE enum))
- JOYSTICK_MODE mode;
-
- // Track axis/button settings based on a Joystick/AutopilotType/SystemType triplet.
- // This is only a double-map, because settings are stored/loaded based on joystick
- // name first, so only the settings for the current joystick need to be stored at any given time.
- // Pointers are kept to the various settings field to reduce lookup times.
- // Note that the mapping (0,0) corresponds to when no UAS is connected. Since this corresponds
- // to a generic vehicle type and a generic autopilot, this is a pretty safe default.
- QMap > joystickSettings;
-
- // Track the last state of the axes, buttons, and hats for only emitting change signals.
- QList joystickAxes; ///< The values of every axes during the last sample.
- quint16 joystickButtons; ///< The state of every button. Bitfield supporting 16 buttons with 1s indicating that the button is down.
- qint8 xHat, yHat; ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left.
-
- /**
- * @brief Called before main run() event loop starts. Waits for joysticks to be connected.
- */
- void init();
-
-signals:
-
- /**
- * @brief Signal containing all joystick raw positions
- *
- * @param roll forward / pitch / x axis, front: 1, center: 0, back: -1. If the roll axis isn't defined, NaN is transmit instead.
- * @param pitch left / roll / y axis, left: -1, middle: 0, right: 1. If the roll axis isn't defined, NaN is transmit instead.
- * @param yaw turn axis, left-turn: -1, centered: 0, right-turn: 1. If the roll axis isn't defined, NaN is transmit instead.
- * @param throttle Throttle, -100%:-1.0, 0%: 0.0, 100%: 1.0. If the roll axis isn't defined, NaN is transmit instead.
- * @param xHat hat vector in forward-backward direction, +1 forward, 0 center, -1 backward
- * @param yHat hat vector in left-right direction, -1 left, 0 center, +1 right
- * @param mode (setpoint type) see JOYSTICK_MODE enum
- */
- void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons, quint8 mode);
-
- /**
- * @brief Emit a new value for an axis
- *
- * @param value Value of the axis, between -1.0 and 1.0.
- */
- void axisValueChanged(int axis, float value);
-
- /**
- * @brief Joystick button has changed state from unpressed to pressed.
- * @param key index of the pressed key
- */
- void buttonPressed(int key);
-
- /**
- * @brief Joystick button has changed state from pressed to unpressed.
- *
- * @param key index of the released key
- */
- void buttonReleased(int key);
-
- /**
- * @brief A joystick button was pressed that had a corresponding action.
- * @param action The index of the action to trigger. Dependent on UAS.
- */
- void actionTriggered(int action);
-
- /**
- * @brief Hat (8-way switch on the top) has changed position
- *
- * Coordinate frame for joystick hat:
- *
- * y
- * ^
- * |
- * |
- * 0 ----> x
- *
- *
- * @param x vector in left-right direction
- * @param y vector in forward-backward direction
- */
- void hatDirectionChanged(qint8 x, qint8 y);
-
- /** @brief Signal that the UAS has been updated for this JoystickInput
- * Note that any UI updates should NOT query this object for joystick details. That should be done in response to the joystickSettingsChanged signal.
- */
- void activeVehicleChanged(Vehicle* vehicle);
-
- /** @brief Signals that new joystick-specific settings were changed. Useful for triggering updates that at dependent on the current joystick. */
- void joystickSettingsChanged();
-
- /** @brief The JoystickInput has switched to a different joystick. UI should be adjusted accordingly. */
- void newJoystickSelected();
-
-public slots:
- /** @brief Enable or disable emitting the high-level control signals from the joystick. */
- void setEnabled(bool enable);
- /** @brief Switch to a new joystick by ID number. Both buttons and axes are updated with the proper signals emitted. */
- void setActiveJoystick(int id);
- /** @brief Switch calibration mode active */
- void setCalibrating(bool active);
- /**
- * @brief Change the control mapping for a given joystick axis.
- * @param axisID The axis to modify (0-indexed)
- * @param newMapping The mapping to use.
- * @see JOYSTICK_INPUT_MAPPING
- */
- void setAxisMapping(int axis, JoystickInput::JOYSTICK_INPUT_MAPPING newMapping);
- /**
- * @brief Specify if an axis should be inverted.
- * @param axis The ID of the axis.
- * @param inverted True indicates inverted from normal. Varies by controller.
- */
- void setAxisInversion(int axis, bool inverted);
-
- /**
- * @brief Specify that an axis should only transmit the positive values. Useful for controlling throttle from auto-centering axes.
- * @param axis Which axis has its range limited.
- * @param limitRange If true only the positive half of this axis will be read.
- */
- void setAxisRangeLimit(int axis, bool limitRange);
-
- /**
- * @brief Specify minimum value for axis.
- * @param axis Which axis should be set.
- * @param min Value to be set.
- */
- void setAxisRangeLimitMin(int axis, float min);
-
- /**
- * @brief Specify maximum value for axis.
- * @param axis Which axis should be set.
- * @param max Value to be set.
- */
- void setAxisRangeLimitMax(int axis, float max);
-
- /**
- * @brief Specify a button->action mapping for the given uas.
- * This mapping is applied based on UAS autopilot type and UAS system type.
- * Connects the buttonEmitted signal for the corresponding button to the corresponding action for the current UAS.
- * @param button The numeric ID for the button
- * @param action The numeric ID of the action for this UAS to map to.
- */
- void setButtonAction(int button, int action);
-
- /**
- * @brief Specify which setpoints should be sent to the UAS when moving the joystick
- * @param newMode the mode (setpoint type) see the JOYSTICK_MODE enum
- */
- void setMode(int newMode)
- {
- mode = (JOYSTICK_MODE)newMode;
- }
-
-private slots:
- void _activeVehicleChanged(Vehicle* vehicle);
-};
-
-#endif // _JOYSTICKINPUT_H_
diff --git a/src/qgcunittest/FileManagerTest.cc b/src/qgcunittest/FileManagerTest.cc
index 5bce2a431874568531fe410ad6e7c4bbed2ccb29..1ea71193b897d672bcde455fc80114932bac4eed 100644
--- a/src/qgcunittest/FileManagerTest.cc
+++ b/src/qgcunittest/FileManagerTest.cc
@@ -26,6 +26,7 @@
#include "FileManagerTest.h"
#include "MultiVehicleManager.h"
+#include "UAS.h"
//UT_REGISTER_TEST(FileManagerTest)
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index e766ebca28fe3edb1646e654d5c84c76cb6bb74d..75bb69abbe19a2b30664fbe2688315f0e87a8961 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -37,6 +37,7 @@
#include "QGCMessageBox.h"
#include "QGCLoggingCategory.h"
#include "Vehicle.h"
+#include "Joystick.h"
QGC_LOGGING_CATEGORY(UASLog, "UASLog")
@@ -1372,7 +1373,7 @@ void UAS::receiveMessage(mavlink_message_t message)
*/
void UAS::setHomePosition(double lat, double lon, double alt)
{
- if (blockHomePositionChanges)
+ if (!_vehicle || blockHomePositionChanges)
return;
QString uasName = (getUASName() == "")?
@@ -1409,6 +1410,10 @@ void UAS::setHomePosition(double lat, double lon, double alt)
**/
void UAS::setLocalOriginAtCurrentGPSPosition()
{
+ if (!_vehicle) {
+ return;
+ }
+
QMessageBox::StandardButton button = QGCMessageBox::question(tr("Set the home position at the current GPS position?"),
tr("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"),
QMessageBox::Yes | QMessageBox::Cancel,
@@ -1453,6 +1458,10 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
void UAS::startCalibration(UASInterface::StartCalibrationType calType)
{
+ if (!_vehicle) {
+ return;
+ }
+
int gyroCal = 0;
int magCal = 0;
int airspeedCal = 0;
@@ -1510,6 +1519,10 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
void UAS::stopCalibration(void)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
@@ -1530,7 +1543,11 @@ void UAS::stopCalibration(void)
void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
{
- int actuatorCal = 0;
+ if (!_vehicle) {
+ return;
+ }
+
+ int actuatorCal = 0;
switch (calType) {
case StartBusConfigActuators:
@@ -1558,6 +1575,10 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
void UAS::stopBusConfig(void)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
@@ -1578,6 +1599,10 @@ void UAS::stopBusConfig(void)
void UAS::startDataRecording()
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 2, 0, 0, 0);
_vehicle->sendMessage(msg);
@@ -1585,7 +1610,11 @@ void UAS::startDataRecording()
void UAS::stopDataRecording()
{
- mavlink_message_t msg;
+ if (!_vehicle) {
+ return;
+ }
+
+ mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 0, 0, 0, 0);
_vehicle->sendMessage(msg);
}
@@ -1752,7 +1781,11 @@ void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode)
*/
void UAS::setModeArm(uint8_t newBaseMode, uint32_t newCustomMode)
{
- if (receivedMode)
+ if (!_vehicle) {
+ return;
+ }
+
+ if (receivedMode)
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode);
@@ -1882,7 +1915,11 @@ QImage UAS::getImage()
void UAS::requestImage()
{
- qDebug() << "trying to get an image from the uas...";
+ if (!_vehicle) {
+ return;
+ }
+
+ qDebug() << "trying to get an image from the uas...";
// check if there is already an image transmission going on
if (imagePacketsArrived == 0)
@@ -1944,6 +1981,10 @@ bool UAS::isFixedWing()
*/
void UAS::enableAllDataTransmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -1972,6 +2013,10 @@ void UAS::enableAllDataTransmission(int rate)
*/
void UAS::enableRawSensorDataTransmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -1996,6 +2041,10 @@ void UAS::enableRawSensorDataTransmission(int rate)
*/
void UAS::enableExtendedSystemStatusTransmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -2020,6 +2069,10 @@ void UAS::enableExtendedSystemStatusTransmission(int rate)
*/
void UAS::enableRCChannelDataTransmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
#if defined(MAVLINK_ENABLED_UALBERTA_MESSAGES)
mavlink_message_t msg;
mavlink_msg_request_rc_channels_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, enabled);
@@ -2049,6 +2102,10 @@ void UAS::enableRCChannelDataTransmission(int rate)
*/
void UAS::enableRawControllerDataTransmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -2095,6 +2152,10 @@ void UAS::enableRawControllerDataTransmission(int rate)
*/
void UAS::enablePositionTransmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -2119,6 +2180,10 @@ void UAS::enablePositionTransmission(int rate)
*/
void UAS::enableExtra1Transmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -2144,6 +2209,10 @@ void UAS::enableExtra1Transmission(int rate)
*/
void UAS::enableExtra2Transmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -2169,6 +2238,10 @@ void UAS::enableExtra2Transmission(int rate)
*/
void UAS::enableExtra3Transmission(int rate)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Buffers to write data to
mavlink_message_t msg;
mavlink_request_data_stream_t stream;
@@ -2303,6 +2376,10 @@ void UAS::setUASName(const QString& name)
void UAS::executeCommand(MAV_CMD command)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_command_long_t cmd;
cmd.command = (uint16_t)command;
@@ -2321,6 +2398,10 @@ void UAS::executeCommand(MAV_CMD command)
}
void UAS::executeCommandAck(int num, bool success)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_command_ack_t ack;
ack.command = num;
@@ -2331,6 +2412,10 @@ void UAS::executeCommandAck(int num, bool success)
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_command_long_t cmd;
cmd.command = (uint16_t)command;
@@ -2354,6 +2439,10 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
*/
void UAS::launch()
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_NAV_TAKEOFF, 1, 0, 0, 0, 0, 0, 0, 0);
_vehicle->sendMessage(msg);
@@ -2405,11 +2494,12 @@ void UAS::toggleAutonomy()
* This can only be done if the system has manual inputs enabled and is armed.
*/
#ifndef __mobile__
-void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8 joystickMode)
+void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
{
- Q_UNUSED(xHat);
- Q_UNUSED(yHat);
-
+ if (!_vehicle) {
+ return;
+ }
+
// Store the previous manual commands
static float manualRollAngle = 0.0;
static float manualPitchAngle = 0.0;
@@ -2447,7 +2537,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
mavlink_message_t message;
- if (joystickMode == JoystickInput::JOYSTICK_MODE_ATTITUDE) {
+ if (joystickMode == Vehicle::JoystickModeAttitude) {
// send an external attitude setpoint command (rate control disabled)
float attitudeQuaternion[4];
mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
@@ -2465,7 +2555,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0,
thrust
);
- } else if (joystickMode == JoystickInput::JOYSTICK_MODE_POSITION) {
+ } else if (joystickMode == Vehicle::JoystickModePosition) {
// Send the the local position setpoint (local pos sp external message)
static float px = 0;
static float py = 0;
@@ -2495,7 +2585,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
yaw,
0
);
- } else if (joystickMode == JoystickInput::JOYSTICK_MODE_FORCE) {
+ } else if (joystickMode == Vehicle::JoystickModeForce) {
// Send the the force setpoint (local pos sp external message)
float dcm[3][3];
mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
@@ -2523,7 +2613,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0,
0
);
- } else if (joystickMode == JoystickInput::JOYSTICK_MODE_VELOCITY) {
+ } else if (joystickMode == Vehicle::JoystickModeVelocity) {
// Send the the local velocity setpoint (local pos sp external message)
static float vx = 0;
static float vy = 0;
@@ -2555,7 +2645,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0,
yawrate
);
- } else if (joystickMode == JoystickInput::JOYSTICK_MODE_MANUAL) {
+ } else if (joystickMode == Vehicle::JoystickModeRC) {
// Save the new manual control inputs
manualRollAngle = roll;
@@ -2566,7 +2656,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
// Store scaling values for all 3 axes
const float axesScaling = 1.0 * 1000.0;
-
+
// Calculate the new commands for roll, pitch, yaw, and thrust
const float newRollCommand = roll * axesScaling;
// negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward
@@ -2574,6 +2664,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
const float newYawCommand = yaw * axesScaling;
const float newThrustCommand = thrust * axesScaling;
+ //qDebug() << newRollCommand << newPitchCommand << newYawCommand << newThrustCommand;
+
// Send the MANUAL_COMMAND message
mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
}
@@ -2588,7 +2680,11 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
#ifndef __mobile__
void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{
- // If system has manual inputs enabled and is armed
+ if (!_vehicle) {
+ return;
+ }
+
+ // If system has manual inputs enabled and is armed
if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
{
mavlink_message_t message;
@@ -2649,7 +2745,11 @@ bool UAS::isAirplane()
*/
void UAS::halt()
{
- mavlink_message_t msg;
+ if (!_vehicle) {
+ return;
+ }
+
+ mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_HOLD, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0);
_vehicle->sendMessage(msg);
}
@@ -2659,7 +2759,11 @@ void UAS::halt()
*/
void UAS::go()
{
- mavlink_message_t msg;
+ if (!_vehicle) {
+ return;
+ }
+
+ mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0);
_vehicle->sendMessage(msg);
}
@@ -2669,6 +2773,10 @@ void UAS::go()
*/
void UAS::home()
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
double latitude = HomePositionManager::instance()->getHomeLatitude();
@@ -2685,7 +2793,11 @@ void UAS::home()
*/
void UAS::land()
{
- mavlink_message_t msg;
+ if (!_vehicle) {
+ return;
+ }
+
+ mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_LAND, 1, 0, 0, 0, 0, 0, 0, 0);
_vehicle->sendMessage(msg);
@@ -2696,6 +2808,10 @@ void UAS::land()
*/
void UAS::pairRX(int rxType, int rxSubType)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0);
@@ -2928,6 +3044,10 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
float pitchspeed, float yawspeed, double lat, double lon, double alt,
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
{
+ if (!_vehicle) {
+ return;
+ }
+
if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{
float q[4];
@@ -3006,6 +3126,10 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
{
+ if (!_vehicle) {
+ return;
+ }
+
if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{
float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
@@ -3043,6 +3167,10 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance)
{
+ if (!_vehicle) {
+ return;
+ }
+
// FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api
Q_UNUSED(time_us);
@@ -3077,6 +3205,10 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
#ifndef __mobile__
void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites)
{
+ if (!_vehicle) {
+ return;
+ }
+
// Only send at 10 Hz max rate
if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
return;
@@ -3139,6 +3271,10 @@ void UAS::stopHil()
void UAS::shutdown()
{
+ if (!_vehicle) {
+ return;
+ }
+
QMessageBox::StandardButton button = QGCMessageBox::question(tr("Shutting down the UAS"),
tr("Do you want to shut down the onboard computer?"),
QMessageBox::Yes | QMessageBox::Cancel,
@@ -3160,6 +3296,10 @@ void UAS::shutdown()
*/
void UAS::setTargetPosition(float x, float y, float z, float yaw)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t msg;
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 1, 0, yaw, x, y, z);
_vehicle->sendMessage(msg);
@@ -3268,6 +3408,10 @@ void UAS::stopLowBattAlarm()
void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
{
+ if (!_vehicle) {
+ return;
+ }
+
mavlink_message_t message;
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
@@ -3298,6 +3442,10 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p
void UAS::unsetRCToParameterMap()
{
+ if (!_vehicle) {
+ return;
+ }
+
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
for (int i = 0; i < 3; i++) {
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 977b3914e00e5019f23cc15040a460f0070749f1..e6abf766a3428ef48d84fb967ec1a433a09c6e2a 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -37,8 +37,9 @@ This file is part of the QGROUNDCONTROL project
#include
#include "QGCMAVLink.h"
#include "FileManager.h"
+#include "Vehicle.h"
+
#ifndef __mobile__
-#include "JoystickInput.h"
#include "QGCHilLink.h"
#include "QGCFlightGearLink.h"
#include "QGCJSBSimLink.h"
@@ -110,6 +111,8 @@ public:
Q_PROPERTY(double altitudeWGS84 READ getAltitudeWGS84 WRITE setAltitudeWGS84 NOTIFY altitudeWGS84Changed)
Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged)
+ void clearVehicle(void) { _vehicle = NULL; }
+
void setGroundSpeed(double val)
{
groundSpeed = val;
@@ -847,7 +850,7 @@ public slots:
/** @brief Set the values for the manual control of the vehicle */
#ifndef __mobile__
- void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons, quint8);
+ void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode);
#endif
/** @brief Set the values for the 6dof manual control of the vehicle */
diff --git a/src/uas/UASMessageHandler.cc b/src/uas/UASMessageHandler.cc
index 3ad901b1e1cfa04dfda8bd726b31d6309e0cd37e..d95b770215523fb00e88ac99aaa6318969a0b77d 100644
--- a/src/uas/UASMessageHandler.cc
+++ b/src/uas/UASMessageHandler.cc
@@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCApplication.h"
#include "UASMessageHandler.h"
#include "MultiVehicleManager.h"
+#include "UAS.h"
UASMessage::UASMessage(int componentid, int severity, QString text)
{
diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc
index f732d1edfcc079d5e5567d774f497fa0d046b6db..a9ba7f281ef7f7fa30248e966866e00958096530 100644
--- a/src/ui/HDDisplay.cc
+++ b/src/ui/HDDisplay.cc
@@ -18,14 +18,17 @@
#include
#include
#include
+#include
+
#include
+
#include "HDDisplay.h"
#include "ui_HDDisplay.h"
#include "MG.h"
#include "QGC.h"
#include "QGCApplication.h"
-#include
#include "MultiVehicleManager.h"
+#include "UAS.h"
HDDisplay::HDDisplay(const QStringList &plotList, QString title, QWidget *parent) :
QGraphicsView(parent),
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index d0c25e21fe22040925799b529d57f9adc0577ecd..3df5d60c19130c7abfb652b059334df78b601934 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -46,6 +46,7 @@ This file is part of the QGROUNDCONTROL project
#include
#include "MAV2DIcon.h"
#include "QGCApplication.h"
+#include "UAS.h"
HSIDisplay::HSIDisplay(QWidget *parent) :
HDDisplay(QStringList(), "HSI", parent),
diff --git a/src/ui/JoystickAxis.cc b/src/ui/JoystickAxis.cc
deleted file mode 100644
index cfe741b21c65cee3cb9e8c0ec56443b878feb0de..0000000000000000000000000000000000000000
--- a/src/ui/JoystickAxis.cc
+++ /dev/null
@@ -1,98 +0,0 @@
-#include "JoystickAxis.h"
-#include "JoystickInput.h"
-#include "ui_JoystickAxis.h"
-#include "MultiVehicleManager.h"
-#include
-
-JoystickAxis::JoystickAxis(int id, QWidget *parent) :
- QWidget(parent),
- id(id),
- ui(new Ui::JoystickAxis)
-{
- ui->setupUi(this);
- mappingComboBoxChanged(JoystickInput::JOYSTICK_INPUT_MAPPING_NONE);
- ui->label->setText(QString::number(id));
- connect(ui->comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(mappingComboBoxChanged(int)));
- connect(ui->invertedCheckBox, SIGNAL(clicked(bool)), this, SLOT(inversionCheckBoxChanged(bool)));
- connect(ui->rangeCheckBox, SIGNAL(clicked(bool)), this, SLOT(rangeCheckBoxChanged(bool)));
-}
-
-JoystickAxis::~JoystickAxis()
-{
- delete ui;
-}
-
-void JoystickAxis::setValue(float value)
-{
- ui->progressBar->setValue(100.0f * value);
-}
-
-void JoystickAxis::setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING newMapping)
-{
- ui->comboBox->setCurrentIndex(newMapping);
- if (newMapping == JoystickInput::JOYSTICK_INPUT_MAPPING_THROTTLE)
- {
- ui->rangeCheckBox->show();
- }
- else
- {
- ui->rangeCheckBox->hide();
- }
- this->activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle());
-}
-
-void JoystickAxis::setInverted(bool newValue)
-{
- ui->invertedCheckBox->setChecked(newValue);
-}
-
-void JoystickAxis::setRangeLimit(bool newValue)
-{
- ui->rangeCheckBox->setChecked(newValue);
-}
-
-void JoystickAxis::mappingComboBoxChanged(int newMapping)
-{
- JoystickInput::JOYSTICK_INPUT_MAPPING mapping = (JoystickInput::JOYSTICK_INPUT_MAPPING)newMapping;
- emit mappingChanged(id, mapping);
- updateUIBasedOnUAS(MultiVehicleManager::instance()->activeVehicle(), mapping);
-}
-
-void JoystickAxis::inversionCheckBoxChanged(bool inverted)
-{
- emit inversionChanged(id, inverted);
-}
-
-void JoystickAxis::rangeCheckBoxChanged(bool limited)
-{
- emit rangeChanged(id, limited);
-}
-
-void JoystickAxis::activeVehicleChanged(Vehicle* vehicle)
-{
- updateUIBasedOnUAS(vehicle, (JoystickInput::JOYSTICK_INPUT_MAPPING)ui->comboBox->currentIndex());
-}
-
-void JoystickAxis::updateUIBasedOnUAS(Vehicle* vehicle, JoystickInput::JOYSTICK_INPUT_MAPPING axisMapping)
-{
- UAS* uas = NULL;
-
- if (vehicle) {
- uas = vehicle->uas();
- }
-
- // Set the throttle display to only positive if:
- // * This is the throttle axis AND
- // * The current UAS can't reverse OR there is no current UAS
- // This causes us to default to systems with no negative throttle.
- if (((uas && !uas->systemCanReverse()) || !uas) && axisMapping == JoystickInput::JOYSTICK_INPUT_MAPPING_THROTTLE)
- {
- ui->progressBar->setRange(0, 100);
- ui->rangeCheckBox->show();
- }
- else
- {
- ui->progressBar->setRange(-100, 100);
- ui->rangeCheckBox->hide();
- }
-}
diff --git a/src/ui/JoystickAxis.h b/src/ui/JoystickAxis.h
deleted file mode 100644
index 9693ee994c069947591f2b75a1ad903a694157f7..0000000000000000000000000000000000000000
--- a/src/ui/JoystickAxis.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*=====================================================================
-
-QGroundControl Open Source Ground Control Station
-
-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 .
-
-======================================================================*/
-
-/**
- * @file
- * This class defines a UI element to represent a single controller axis.
- * It is used by the JoystickWidget to simplify some of the logic in that class.
- */
-
-#ifndef JOYSTICKAXIS_H
-#define JOYSTICKAXIS_H
-
-#include
-#include "JoystickInput.h"
-#include "Vehicle.h"
-
-namespace Ui {
-class JoystickAxis;
-}
-
-class JoystickAxis : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit JoystickAxis(int id, QWidget *parent = 0);
- ~JoystickAxis();
- void setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING newMapping);
- void setInverted(bool newValue);
- void setRangeLimit(bool newValue);
- void setAxisRangeMin(float min);
- void setAxisRangeMax(float max);
-
-signals:
- /** @brief Signal a change in this axis' yaw/pitch/roll mapping */
- void mappingChanged(int id, JoystickInput::JOYSTICK_INPUT_MAPPING newMapping);
- /** @brief Signal a change in this axis' inversion status */
- void inversionChanged(int id, bool inversion);
- /** @brief Signal a change in this axis' range setting. If limited is true then only the positive values should be read from this axis. */
- void rangeChanged(int id, bool limited);
-
-public slots:
- /** @brief Update the displayed value of the included progressbar.
- * @param value A value between -1.0 and 1.0.
- */
- void setValue(float value);
- /** @brief Specify the UAS that this axis should track for displaying throttle properly. */
- void activeVehicleChanged(Vehicle* vehicle);
-
-private:
- int id; ///< The ID for this axis. Corresponds to the IDs used by JoystickInput.
- Ui::JoystickAxis *ui;
- /**
- * @brief Update the UI based on both the current UAS and the current axis mapping.
- * @param uas The currently-active UAS.
- * @param axisMapping The new mapping for this axis.
- */
- void updateUIBasedOnUAS(Vehicle* vehicle, JoystickInput::JOYSTICK_INPUT_MAPPING axisMapping);
-
-private slots:
- /** @brief Handle changes to the mapping dropdown bar. */
- void mappingComboBoxChanged(int newMapping);
- /** @brief Emit signal when the inversion checkbox is changed. */
- void inversionCheckBoxChanged(bool inverted);
- /** @brief Emit signal when the range checkbox is changed. */
- void rangeCheckBoxChanged(bool inverted);
-};
-
-#endif // JOYSTICKAXIS_H
diff --git a/src/ui/JoystickAxis.ui b/src/ui/JoystickAxis.ui
deleted file mode 100644
index 212133c5033baf8c50e045f6c407bf56c42f432d..0000000000000000000000000000000000000000
--- a/src/ui/JoystickAxis.ui
+++ /dev/null
@@ -1,168 +0,0 @@
-
-
- JoystickAxis
-
-
-
- 0
- 0
- 80
- 200
-
-
-
-
- 0
- 0
-
-
-
-
- 40
- 200
-
-
-
- Form
-
-
-
- QLayout::SetMinimumSize
-
-
- 1
-
-
- 2
-
-
- 1
-
-
- 2
-
- -
-
-
-
-
-
- Qt::AlignCenter
-
-
-
- -
-
-
-
- 0
- 0
-
-
-
- Specify what property of the UAS this axis should command
-
-
-
-
- --
-
-
- -
-
- Yaw
-
-
- -
-
- Pitch
-
-
- -
-
- Roll
-
-
- -
-
- Throttle
-
-
-
-
- -
-
-
- Half range
-
-
-
- -
-
-
- true
-
-
-
- 0
- 0
-
-
-
-
- 0
- 100
-
-
-
- Only use the positive values from this axis for control
-
-
- -100
-
-
- 0
-
-
- Qt::AlignCenter
-
-
- true
-
-
- Qt::Vertical
-
-
- QProgressBar::TopToBottom
-
-
- %v
-
-
-
- -
-
-
-
- 0
- 0
-
-
-
-
- 0
- 25
-
-
-
- Reverse the values for this axis
-
-
- Inverted
-
-
-
-
-
-
-
-
diff --git a/src/ui/JoystickButton.cc b/src/ui/JoystickButton.cc
deleted file mode 100644
index 5f094ff51f09935f799514cb25195fb05cb99364..0000000000000000000000000000000000000000
--- a/src/ui/JoystickButton.cc
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "JoystickButton.h"
-#include "ui_JoystickButton.h"
-#include "MultiVehicleManager.h"
-
-JoystickButton::JoystickButton(int id, QWidget *parent) :
- QWidget(parent),
- id(id),
- m_ui(new Ui::JoystickButton)
-{
- m_ui->setupUi(this);
- m_ui->joystickButtonLabel->setText(QString::number(id));
- this->activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle());
- connect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int)));
-}
-
-JoystickButton::~JoystickButton()
-{
- delete m_ui;
-}
-
-void JoystickButton::activeVehicleChanged(Vehicle* vehicle)
-{
- // Disable signals so that changes to joystickAction don't trigger JoystickInput updates.
- disconnect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int)));
- if (vehicle)
- {
- UAS* uas = vehicle->uas();
-
- m_ui->joystickAction->setEnabled(true);
- m_ui->joystickAction->clear();
- m_ui->joystickAction->addItem("--");
- QList actions = uas->getActions();
- foreach (QAction* a, actions)
- {
- m_ui->joystickAction->addItem(a->text());
- }
- m_ui->joystickAction->setCurrentIndex(0);
- } else {
- m_ui->joystickAction->setEnabled(false);
- m_ui->joystickAction->clear();
- m_ui->joystickAction->addItem("--");
- m_ui->joystickAction->setCurrentIndex(0);
- }
- connect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int)));
-}
-
-void JoystickButton::setAction(int index)
-{
- // Disable signals so that changes to joystickAction don't trigger JoystickInput updates.
- disconnect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int)));
- // Add one because the default no-action takes the 0-spot.
- m_ui->joystickAction->setCurrentIndex(index + 1);
- connect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int)));
-}
-
-void JoystickButton::actionComboBoxChanged(int index)
-{
- // Subtract one because the default no-action takes the 0-spot.
- emit actionChanged(id, index - 1);
-}
diff --git a/src/ui/JoystickButton.h b/src/ui/JoystickButton.h
deleted file mode 100644
index 4e76cde2133e280bd50e59862c9e4a1078384f9e..0000000000000000000000000000000000000000
--- a/src/ui/JoystickButton.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*=====================================================================
-
-QGroundControl Open Source Ground Control Station
-
-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 .
-
-======================================================================*/
-
-/**
- * @file
- * This class defines a UI element to represent a single controller axis.
- * It is used by the JoystickWidget to simplify some of the logic in that class.
- */
-
-#ifndef JOYSTICKBUTTON_H
-#define JOYSTICKBUTTON_H
-
-#include
-
-#include "Vehicle.h"
-
-namespace Ui
-{
-class JoystickButton;
-}
-
-class JoystickButton : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit JoystickButton(int id, QWidget *parent = 0);
- virtual ~JoystickButton();
-
-public slots:
- /** @brief Specify the UAS that this axis should track for displaying throttle properly. */
- void activeVehicleChanged(Vehicle* vehicle);
- /** @brieft Specify which action this button should correspond to.
- * Values 0 and higher indicate a specific action, while -1 indicates no action. */
- void setAction(int index);
-
-signals:
- /** @brief Signal a change in this buttons' action mapping */
- void actionChanged(int id, int index);
-
-private:
- int id;
- Ui::JoystickButton *m_ui;
-
-private slots:
- void actionComboBoxChanged(int index);
-};
-#endif // JOYSTICKBUTTON_H
diff --git a/src/ui/JoystickButton.ui b/src/ui/JoystickButton.ui
deleted file mode 100644
index 1598d22273c2a4929d4d60c896fd37624158ca4c..0000000000000000000000000000000000000000
--- a/src/ui/JoystickButton.ui
+++ /dev/null
@@ -1,92 +0,0 @@
-
-
- JoystickButton
-
-
-
- 0
- 0
- 125
- 29
-
-
-
-
- 0
- 0
-
-
-
-
- 50
- 0
-
-
-
- Form
-
-
-
- QLayout::SetMinimumSize
-
-
- 2
-
-
- 1
-
-
- 2
-
-
- 1
-
- -
-
-
-
- 0
- 0
-
-
-
-
- 20
- 0
-
-
-
-
-
-
- Qt::AlignCenter
-
-
-
- -
-
-
- false
-
-
-
- 0
- 0
-
-
-
-
- 60
- 0
-
-
-
- QComboBox::AdjustToContents
-
-
-
-
-
-
-
-
diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc
deleted file mode 100644
index 5926322befaa0f5b2cf2bcbaacdb2cbd96e2675b..0000000000000000000000000000000000000000
--- a/src/ui/JoystickWidget.cc
+++ /dev/null
@@ -1,279 +0,0 @@
-#include "JoystickWidget.h"
-#include "QGCApplication.h"
-#include "ui_JoystickWidget.h"
-#include "JoystickButton.h"
-#include "JoystickAxis.h"
-
-#include
-
-JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) :
- QWidget(parent),
- joystick(joystick),
- m_ui(new Ui::JoystickWidget)
-{
- m_ui->setupUi(this);
-
- // Center the window on the screen.
- QRect position = frameGeometry();
- position.moveCenter(QDesktopWidget().availableGeometry().center());
- move(position.topLeft());
-
- // Initialize the UI based on the current joystick
- initUI();
-
- // Watch for button, axis, and hat input events from the joystick.
- connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(joystickButtonPressed(int)));
- connect(this->joystick, SIGNAL(buttonReleased(int)), this, SLOT(joystickButtonReleased(int)));
- connect(this->joystick, SIGNAL(axisValueChanged(int,float)), this, SLOT(updateAxisValue(int,float)));
- connect(this->joystick, SIGNAL(hatDirectionChanged(qint8,qint8)), this, SLOT(setHat(qint8,qint8)));
-
- // Also watch for when new settings were loaded for the current joystick to do a mass UI refresh.
- connect(this->joystick, SIGNAL(joystickSettingsChanged()), this, SLOT(updateUI()));
-
- // If the selected joystick is changed, update the JoystickInput.
- connect(m_ui->joystickNameComboBox, SIGNAL(currentIndexChanged(int)), this->joystick, SLOT(setActiveJoystick(int)));
- // Also wait for the JoystickInput to switch, then update our UI.
- connect(this->joystick, SIGNAL(newJoystickSelected()), this, SLOT(createUIForJoystick()));
-
- // Initialize the UI to the current JoystickInput state. Also make sure to listen for future changes
- // so that the UI can be updated.
- connect(m_ui->enableCheckBox, SIGNAL(toggled(bool)), m_ui->joystickFrame, SLOT(setEnabled(bool)));
- m_ui->enableCheckBox->setChecked(this->joystick->enabled()); // Needs to be after connecting to the joystick frame and before watching for enabled events from JoystickInput.
- connect(m_ui->enableCheckBox, SIGNAL(toggled(bool)), this->joystick, SLOT(setEnabled(bool)));
-
- // Update the button label colors based on the current theme and watch for future theme changes.
- styleChanged(qgcApp()->styleIsDark());
- connect(qgcApp(), &QGCApplication::styleChanged, this, &JoystickWidget::styleChanged);
-
- // change mode when mode combobox is changed
- connect(m_ui->joystickModeComboBox, SIGNAL(currentIndexChanged(int)), this->joystick, SLOT(setMode(int)));
-
- // Display the widget above all other windows.
- this->raise();
- this->show();
-}
-
-void JoystickWidget::initUI()
-{
- // Add the joysticks to the top combobox. They're indexed by their item number.
- // And set the currently-selected combobox item to the current joystick.
- int joysticks = joystick->getNumJoysticks();
- if (joysticks)
- {
- for (int i = 0; i < joysticks; i++)
- {
- m_ui->joystickNameComboBox->addItem(joystick->getJoystickNameById(i));
- }
- m_ui->joystickNameComboBox->setCurrentIndex(joystick->getJoystickID());
- // And if joystick support is enabled, show the UI.
- if (m_ui->enableCheckBox->isChecked())
- {
- m_ui->joystickFrame->setEnabled(true);
- }
-
- // mode combo box
- m_ui->joystickModeComboBox->addItem("Attitude");
- m_ui->joystickModeComboBox->addItem("Position");
- m_ui->joystickModeComboBox->addItem("Force");
- m_ui->joystickModeComboBox->addItem("Velocity");
- m_ui->joystickModeComboBox->addItem("Manual");
- m_ui->joystickModeComboBox->setCurrentIndex(joystick->getMode());
-
- // Create the initial UI.
- createUIForJoystick();
- }
- // But if there're no joysticks, disable everything and hide empty UI.
- else
- {
- m_ui->enableCheckBox->setEnabled(false);
- m_ui->joystickNameComboBox->addItem(tr("No joysticks found. Connect and restart QGC to add one."));
- m_ui->joystickNameComboBox->setEnabled(false);
- m_ui->joystickFrame->hide();
- }
-}
-
-void JoystickWidget::styleChanged(bool styleIsDark)
-{
- if (styleIsDark)
- {
- buttonLabelColor = QColor(0x14, 0xC6, 0x14);
- }
- else
- {
- buttonLabelColor = QColor(0x73, 0xD9, 0x5D);
- }
-}
-
-JoystickWidget::~JoystickWidget()
-{
- delete m_ui;
-}
-
-void JoystickWidget::changeEvent(QEvent *e)
-{
- switch (e->type()) {
- case QEvent::LanguageChange:
- m_ui->retranslateUi(this);
- break;
- default:
- break;
- }
-}
-
-void JoystickWidget::updateUI()
-{
- // Update the actions for all of the buttons
- for (int i = 0; i < buttons.size(); i++)
- {
- JoystickButton* button = buttons[i];
- int action = joystick->getActionForButton(i);
- button->setAction(action);
- }
-
- // Update the axis mappings
- int rollAxis = joystick->getMappingRollAxis();
- int pitchAxis = joystick->getMappingPitchAxis();
- int yawAxis = joystick->getMappingYawAxis();
- int throttleAxis = joystick->getMappingThrottleAxis();
- for (int i = 0; i < axes.size(); i++)
- {
- JoystickAxis* axis = axes[i];
- JoystickInput::JOYSTICK_INPUT_MAPPING mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_NONE;
- if (i == rollAxis)
- {
- mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_ROLL;
- }
- else if (i == pitchAxis)
- {
- mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_PITCH;
- }
- else if (i == yawAxis)
- {
- mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_YAW;
- }
- else if (i == throttleAxis)
- {
- mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_THROTTLE;
- }
- axis->setMapping(mapping);
- bool inverted = joystick->getInvertedForAxis(i);
- axis->setInverted(inverted);
- bool limited = joystick->getRangeLimitForAxis(i);
- axis->setRangeLimit(limited);
- }
-}
-
-void JoystickWidget::createUIForJoystick()
-{
- // Delete all the old UI elements
- foreach (JoystickButton* b, buttons)
- {
- delete b;
- }
- buttons.clear();
- foreach (JoystickAxis* a, axes)
- {
- delete a;
- }
- axes.clear();
-
- // And add the necessary button displays for this joystick.
- int newButtons = joystick->getJoystickNumButtons();
- if (newButtons)
- {
- m_ui->buttonBox->show();
- for (int i = 0; i < newButtons; i++)
- {
- JoystickButton* button = new JoystickButton(i, m_ui->buttonBox);
- button->setAction(joystick->getActionForButton(i));
- connect(button, SIGNAL(actionChanged(int,int)), this->joystick, SLOT(setButtonAction(int,int)));
- connect(this->joystick, &JoystickInput::activeVehicleChanged, button, &JoystickButton::activeVehicleChanged);
- m_ui->buttonLayout->addWidget(button);
- buttons.append(button);
- }
- }
- else
- {
- m_ui->buttonBox->hide();
- }
-
- // Do the same for the axes supported by this joystick.
- int rollAxis = joystick->getMappingRollAxis();
- int pitchAxis = joystick->getMappingPitchAxis();
- int yawAxis = joystick->getMappingYawAxis();
- int throttleAxis = joystick->getMappingThrottleAxis();
- int newAxes = joystick->getJoystickNumAxes();
- if (newAxes)
- {
- for (int i = 0; i < newAxes; i++)
- {
- JoystickAxis* axis = new JoystickAxis(i, m_ui->axesBox);
- axis->setValue(joystick->getCurrentValueForAxis(i));
- if (i == rollAxis)
- {
- axis->setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING_ROLL);
- }
- else if (i == pitchAxis)
- {
- axis->setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING_PITCH);
- }
- else if (i == yawAxis)
- {
- axis->setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING_YAW);
- }
- else if (i == throttleAxis)
- {
- axis->setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING_THROTTLE);
- }
- axis->setInverted(joystick->getInvertedForAxis(i));
- axis->setRangeLimit(joystick->getRangeLimitForAxis(i));
- connect(axis, SIGNAL(mappingChanged(int,JoystickInput::JOYSTICK_INPUT_MAPPING)), this->joystick, SLOT(setAxisMapping(int,JoystickInput::JOYSTICK_INPUT_MAPPING)));
- connect(axis, SIGNAL(inversionChanged(int,bool)), this->joystick, SLOT(setAxisInversion(int,bool)));
- connect(axis, SIGNAL(rangeChanged(int,bool)), this->joystick, SLOT(setAxisRangeLimit(int,bool)));
- connect(this->joystick, &JoystickInput::activeVehicleChanged, axis, &JoystickAxis::activeVehicleChanged);
- m_ui->axesLayout->addWidget(axis);
- axes.append(axis);
- }
- }
- else
- {
- m_ui->axesBox->hide();
- }
-
- connect(m_ui->calibrationButton, SIGNAL(clicked()), this, SLOT(cycleCalibrationButton()));
-}
-
-void JoystickWidget::updateAxisValue(int axis, float value)
-{
- if (axis < axes.size())
- {
- axes.at(axis)->setValue(value);
- }
-}
-
-void JoystickWidget::setHat(qint8 x, qint8 y)
-{
- m_ui->statusLabel->setText(tr("Hat position: x: %1, y: %2").arg(x).arg(y));
-}
-
-void JoystickWidget::joystickButtonPressed(int key)
-{
- QString colorStyle = QString("QLabel { background-color: %1;}").arg(buttonLabelColor.name());
- buttons.at(key)->setStyleSheet(colorStyle);
-}
-
-void JoystickWidget::joystickButtonReleased(int key)
-{
- buttons.at(key)->setStyleSheet("");
-}
-
-void JoystickWidget::cycleCalibrationButton()
-{
- if (this->joystick->calibrating()) {
- this->joystick->setCalibrating(false);
- m_ui->calibrationButton->setText("Calibrate range");
- } else {
- this->joystick->setCalibrating(true);
- m_ui->calibrationButton->setText("End calibration");
- }
-}
diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h
deleted file mode 100644
index 6f7cbbb054db583529982203198cbd66e4f5788c..0000000000000000000000000000000000000000
--- a/src/ui/JoystickWidget.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*=====================================================================
-
-PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
-
-(c) 2009, 2010 PIXHAWK PROJECT
-
-This file is part of the PIXHAWK project
-
- PIXHAWK 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.
-
- PIXHAWK 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 PIXHAWK. If not, see .
-
-======================================================================*/
-
-/**
- * @file
- * @brief Definition of joystick widget. Provides a UI for configuring the joystick settings.
- * @author Lorenz Meier
- *
- */
-
-#ifndef JOYSTICKWIDGET_H
-#define JOYSTICKWIDGET_H
-
-#include
-#include
-#include
-#include "JoystickInput.h"
-#include "MainWindow.h"
-#include "JoystickAxis.h"
-#include "JoystickButton.h"
-
-namespace Ui
-{
-class JoystickWidget;
-}
-
-class JoystickWidget : public QWidget
-{
- Q_OBJECT
- Q_DISABLE_COPY(JoystickWidget)
-public:
- explicit JoystickWidget(JoystickInput* joystick, QWidget *parent = 0);
- virtual ~JoystickWidget();
-
-public slots:
- /** @brief Update the UI for a new joystick based on SDL ID. */
- void createUIForJoystick();
- /**
- * @brief Update a given axis with a new value
- * @param axis The index of the axis to update.
- * @param value The new value for the axis, [-1.0:1.0].
- * @see JoystickInput:axisValueChanged
- */
- void updateAxisValue(int axis, float value);
- /** @brief Update the UI with new values for the hat.
- * @see JoystickInput::hatDirectionChanged
- */
- void setHat(qint8 x, qint8 y);
- /** @brief Trigger a UI change based on a button being pressed */
- void joystickButtonPressed(int key);
- /** @brief Trigger a UI change based on a button being released */
- void joystickButtonReleased(int key);
- /** @brief Toggle the calibration button */
- void cycleCalibrationButton();
- /** @brief Update the UI color scheme when the MainWindow theme changes. */
- void styleChanged(bool styleIsDark);
- /** Update the UI assuming the joystick has stayed the same. */
- void updateUI();
-
-protected:
- /** @brief UI change event */
- virtual void changeEvent(QEvent *e);
- JoystickInput* joystick; ///< Reference to the joystick
- /** @brief a list of all button labels generated for this joystick. */
- QList buttons;
- /** @brief a lit of all joystick axes generated for this joystick. */
- QList axes;
- /** @brief The color to use for button labels when their corresponding button is pressed */
- QColor buttonLabelColor;
-
-private:
- Ui::JoystickWidget *m_ui;
- /** @brief Initialize all dynamic UI elements (button list, joystick names, etc.).
- * Only done once at startup.
- */
- void initUI();
-};
-
-#endif // JOYSTICKWIDGET_H
diff --git a/src/ui/JoystickWidget.ui b/src/ui/JoystickWidget.ui
deleted file mode 100644
index dbd28428aba6cdb44a42cfc1818c617a35b0f2f6..0000000000000000000000000000000000000000
--- a/src/ui/JoystickWidget.ui
+++ /dev/null
@@ -1,191 +0,0 @@
-
-
- JoystickWidget
-
-
-
- 0
- 0
- 368
- 274
-
-
-
-
- 0
- 0
-
-
-
-
- 368
- 274
-
-
-
- Joystick Settings
-
-
-
- 8
-
-
- QLayout::SetDefaultConstraint
-
-
- 8
-
-
- 8
-
-
- 8
-
-
- 8
-
- -
-
-
- true
-
-
- Enable controllers
-
-
-
- -
-
-
- true
-
-
-
- 0
- 0
-
-
-
-
- -
-
-
- -
-
-
- false
-
-
- QFrame::StyledPanel
-
-
- QFrame::Sunken
-
-
-
- QLayout::SetMinimumSize
-
-
-
-
-
-
-
-
-
- 0
- 0
-
-
-
-
- 100
- 0
-
-
-
-
- 16777215
- 16777215
-
-
-
- Buttons
-
-
- Qt::AlignCenter
-
-
- false
-
-
-
- 1
-
-
- QLayout::SetMinimumSize
-
-
- 3
-
-
- 3
-
-
- 3
-
-
- 3
-
-
-
-
- -
-
-
-
- 0
- 0
-
-
-
-
- 100
- 0
-
-
-
- Axes
-
-
- Qt::AlignCenter
-
-
-
- QLayout::SetMinimumSize
-
-
-
-
-
-
- -
-
-
-
-
-
-
- -
-
-
- Calibrate range
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 09545d01ca139b33aeee15d6ab6dff53a71aae70..e2bc2177812da9772338fbbc4e1649fa62c07352 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -48,9 +48,6 @@ This file is part of the QGROUNDCONTROL project
#include "MAVLinkProtocol.h"
#include "QGCWaypointListMulti.h"
#include "MainWindow.h"
-#ifndef __mobile__
-#include "JoystickWidget.h"
-#endif
#include "GAudioOutput.h"
#include "QGCMAVLinkLogPlayer.h"
#include "SettingsDialog.h"
@@ -200,10 +197,6 @@ MainWindow::MainWindow(QSplashScreen* splashScreen)
// Create actions
connectCommonActions();
// Connect user interface devices
- emit initStatusChanged(tr("Initializing joystick interface"), Qt::AlignLeft | Qt::AlignBottom, QColor(62, 93, 141));
-#ifndef __mobile__
- joystick = new JoystickInput();
-#endif
#ifdef QGC_MOUSE_ENABLED_WIN
emit initStatusChanged(tr("Initializing 3D mouse interface"), Qt::AlignLeft | Qt::AlignBottom, QColor(62, 93, 141));
mouseInput = new Mouse3DInput(this);
@@ -325,15 +318,6 @@ MainWindow::MainWindow(QSplashScreen* splashScreen)
MainWindow::~MainWindow()
{
-#ifndef __mobile__
- if (joystick)
- {
- joystick->shutdown();
- joystick->wait(5000);
- joystick->deleteLater();
- joystick = NULL;
- }
-#endif
// Delete all UAS objects
for (int i=0;i<_commsWidgetList.size();i++)
{
@@ -809,11 +793,7 @@ void MainWindow::showRoadMap()
void MainWindow::showSettings()
{
-#ifndef __mobile__
- SettingsDialog settings(joystick, this);
-#else
SettingsDialog settings(this);
-#endif
settings.exec();
}
@@ -1057,11 +1037,7 @@ void MainWindow::hideSplashScreen(void)
void MainWindow::manageLinks()
{
-#ifndef __mobile__
- SettingsDialog settings(joystick, this, SettingsDialog::ShowCommLinks);
-#else
SettingsDialog settings(this, SettingsDialog::ShowCommLinks);
-#endif
settings.exec();
}
diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h
index beb4286016d05d6bc791c8c86250e4ec77efdb05..8a31ed2f510560949823c18dee14291fcbaac8cc 100644
--- a/src/ui/MainWindow.h
+++ b/src/ui/MainWindow.h
@@ -46,9 +46,6 @@ This file is part of the QGROUNDCONTROL project
#include "WaypointList.h"
#include "CameraView.h"
#include "UASListWidget.h"
-#ifndef __mobile__
-#include "input/JoystickInput.h"
-#endif
#if (defined QGC_MOUSE_ENABLED_WIN) | (defined QGC_MOUSE_ENABLED_LINUX)
#include "Mouse6dofInput.h"
#endif // QGC_MOUSE_ENABLED_WIN
@@ -241,10 +238,6 @@ protected:
QPointer fileWidget;
-#ifndef __mobile__
- JoystickInput* joystick; ///< The joystick manager for QGC
-#endif
-
#ifdef QGC_MOUSE_ENABLED_WIN
/** @brief 3d Mouse support (WIN only) */
Mouse3DInput* mouseInput; ///< 3dConnexion 3dMouse SDK
diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc
index bd0f2a0e3abf946864852626a3b47814bd97e302..fd5c01f65b2dffd558cc038e2bb043d87d60e23d 100644
--- a/src/ui/QGCMAVLinkInspector.cc
+++ b/src/ui/QGCMAVLinkInspector.cc
@@ -3,6 +3,8 @@
#include "QGCMAVLink.h"
#include "QGCMAVLinkInspector.h"
#include "MultiVehicleManager.h"
+#include "UAS.h"
+
#include "ui_QGCMAVLinkInspector.h"
#include
diff --git a/src/ui/QGCWaypointListMulti.cc b/src/ui/QGCWaypointListMulti.cc
index d272868cd33a756baa6400e3e70a1463d34a0f61..12cd90de962155ae3fa77d29c912b0f1efe20f80 100644
--- a/src/ui/QGCWaypointListMulti.cc
+++ b/src/ui/QGCWaypointListMulti.cc
@@ -1,6 +1,8 @@
#include "QGCWaypointListMulti.h"
-#include "ui_QGCWaypointListMulti.h"
#include "MultiVehicleManager.h"
+#include "UAS.h"
+
+#include "ui_QGCWaypointListMulti.h"
void* QGCWaypointListMulti::_offlineUAS = NULL;
diff --git a/src/ui/SettingsDialog.cc b/src/ui/SettingsDialog.cc
index fe885ae3774244860620596a4ea1c74f7df01922..8200c9f69dd611a533238f3b31bbe6312455a505 100644
--- a/src/ui/SettingsDialog.cc
+++ b/src/ui/SettingsDialog.cc
@@ -28,9 +28,6 @@
#include "MainWindow.h"
#include "ui_SettingsDialog.h"
-#ifndef __mobile__
-#include "JoystickWidget.h"
-#endif
#include "LinkManager.h"
#include "MAVLinkProtocol.h"
#include "MAVLinkSettingsWidget.h"
@@ -41,11 +38,7 @@
#include "QGCMessageBox.h"
#include "MainToolBar.h"
-#ifndef __mobile__
-SettingsDialog::SettingsDialog(JoystickInput *joystick, QWidget *parent, int showTab, Qt::WindowFlags flags) :
-#else
SettingsDialog::SettingsDialog(QWidget *parent, int showTab, Qt::WindowFlags flags) :
-#endif
QDialog(parent, flags),
_mainWindow(MainWindow::instance()),
_ui(new Ui::SettingsDialog)
@@ -58,17 +51,10 @@ _ui(new Ui::SettingsDialog)
move(position.topLeft());
QGCLinkConfiguration* pLinkConf = new QGCLinkConfiguration(this);
-#ifndef __mobile__
- JoystickWidget* pJoystickConf = new JoystickWidget(joystick, this);
-#endif
MAVLinkSettingsWidget* pMavsettings = new MAVLinkSettingsWidget(MAVLinkProtocol::instance(), this);
// Add the link settings pane
_ui->tabWidget->addTab(pLinkConf, "Comm Links");
-#ifndef __mobile__
- // Add the joystick settings pane
- _ui->tabWidget->addTab(pJoystickConf, "Controllers");
-#endif
// Add the MAVLink settings pane
_ui->tabWidget->addTab(pMavsettings, "MAVLink");
@@ -113,11 +99,6 @@ _ui(new Ui::SettingsDialog)
case ShowCommLinks:
_ui->tabWidget->setCurrentWidget(pLinkConf);
break;
-#ifndef __mobile__
- case ShowControllers:
- _ui->tabWidget->setCurrentWidget(pJoystickConf);
- break;
-#endif
case ShowMavlink:
_ui->tabWidget->setCurrentWidget(pMavsettings);
break;
diff --git a/src/ui/SettingsDialog.h b/src/ui/SettingsDialog.h
index bf1c85eafa0dd2d43560f8ef1e0b43c7700c5f20..ac1a31108ea065452367adf75ff2e0d74048538a 100644
--- a/src/ui/SettingsDialog.h
+++ b/src/ui/SettingsDialog.h
@@ -45,11 +45,7 @@ public:
ShowMavlink
};
-#ifdef __mobile__
SettingsDialog(QWidget *parent = 0, int showTab = ShowDefault, Qt::WindowFlags flags = Qt::Sheet);
-#else
- SettingsDialog(JoystickInput *joystick, QWidget *parent = 0, int showTab = ShowDefault, Qt::WindowFlags flags = Qt::Sheet);
-#endif
~SettingsDialog();
public slots:
diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc
index 8f3bd0177f30aab4079ac711bbe6c65f7607cc1d..9446eb350df2965ce61093c0001e1ca63175558a 100644
--- a/src/ui/map/MAV2DIcon.cc
+++ b/src/ui/map/MAV2DIcon.cc
@@ -5,6 +5,7 @@
#include "QGC.h"
#include "MultiVehicleManager.h"
+#include "UAS.h"
MAV2DIcon::MAV2DIcon(mapcontrol::MapGraphicItem* map,mapcontrol::OPMapWidget* parent, UASInterface* uas, int radius, int type)
: UAVItem(map,parent),
diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc
index 3fce669452af8ffb5921d3fd5a37e244edbcd75d..18e954c5a668ba2567d9f9ef255f974dc3d4ff11 100644
--- a/src/ui/uas/UASInfoWidget.cc
+++ b/src/ui/uas/UASInfoWidget.cc
@@ -30,17 +30,18 @@ This file is part of the PIXHAWK project
*/
#include
-
-#include
-#include
-#include
-#include
#include
#include
+#include
+
+#include
#include
#include
-#include
+#include "UASInfoWidget.h"
+#include "MultiVehicleManager.h"
+#include "QGC.h"
+#include "UAS.h"
UASInfoWidget::UASInfoWidget(QWidget *parent, QString name) : QWidget(parent)
{
diff --git a/src/ui/uas/UASQuickView.cc b/src/ui/uas/UASQuickView.cc
index b52ab4a71d23f6f9146524fcb28533c6f3d689c8..93e946f4e37a15992cd0dbb42b00941df7361eaf 100644
--- a/src/ui/uas/UASQuickView.cc
+++ b/src/ui/uas/UASQuickView.cc
@@ -1,11 +1,14 @@
#include "UASQuickView.h"
-#include
-#include
#include "UASQuickViewItemSelect.h"
#include "UASQuickViewTextItem.h"
+#include "MultiVehicleManager.h"
+#include "UAS.h"
+
+#include
+#include
#include
#include
-#include "MultiVehicleManager.h"
+
UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent),
uas(NULL)
{