From 3190f0b9b257dba0d48e8ae3f2d4de661617c3dc Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 3 Sep 2015 14:59:23 -0700 Subject: [PATCH] Correcting joystick implementation --- .../PX4/FlightModesComponent.cc | 24 +++-- .../PX4/FlightModesComponent.qml | 23 ++++- .../PX4/PX4AutoPilotPlugin.cc | 26 ++--- src/AutoPilotPlugins/PX4/RadioComponent.cc | 31 +++--- src/AutoPilotPlugins/PX4/RadioComponent.qml | 4 +- src/Joystick/Joystick.cc | 94 +++++++++---------- src/Joystick/Joystick.h | 16 ++-- src/Vehicle/MultiVehicleManager.cc | 39 ++------ src/Vehicle/MultiVehicleManager.h | 2 +- src/Vehicle/Vehicle.cc | 70 ++++++++++++-- src/Vehicle/Vehicle.h | 24 ++++- src/VehicleSetup/JoystickConfig.qml | 45 +++------ src/VehicleSetup/JoystickConfigController.cc | 47 ++++++---- src/VehicleSetup/JoystickConfigController.h | 3 + src/VehicleSetup/SetupView.qml | 1 + 15 files changed, 252 insertions(+), 197 deletions(-) diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponent.cc b/src/AutoPilotPlugins/PX4/FlightModesComponent.cc index 2e6193b85..6eac6335e 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponent.cc +++ b/src/AutoPilotPlugins/PX4/FlightModesComponent.cc @@ -68,12 +68,13 @@ QString FlightModesComponent::iconResource(void) const bool FlightModesComponent::requiresSetup(void) const { - return true; + return _autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->value().toInt() == 1 ? false : true; } bool FlightModesComponent::setupComplete(void) const { - return _autopilot->getParameterFact(FactSystem::defaultComponentId, "RC_MAP_MODE_SW")->value().toInt() != 0; + return _autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->value().toInt() == 1 || + _autopilot->getParameterFact(FactSystem::defaultComponentId, "RC_MAP_MODE_SW")->value().toInt() != 0; } QString FlightModesComponent::setupStateDescription(void) const @@ -116,13 +117,18 @@ QUrl FlightModesComponent::summaryQmlSource(void) const QString FlightModesComponent::prerequisiteSetup(void) const { - PX4AutoPilotPlugin* plugin = dynamic_cast(_autopilot); - Q_ASSERT(plugin); - - if (!plugin->airframeComponent()->setupComplete()) { - return plugin->airframeComponent()->name(); - } else if (!plugin->radioComponent()->setupComplete()) { - return plugin->radioComponent()->name(); + if (_autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->value().toInt() == 1) { + // No RC input + return QString(); + } else { + PX4AutoPilotPlugin* plugin = dynamic_cast(_autopilot); + Q_ASSERT(plugin); + + if (!plugin->airframeComponent()->setupComplete()) { + return plugin->airframeComponent()->name(); + } else if (!plugin->radioComponent()->setupComplete()) { + return plugin->radioComponent()->name(); + } } return QString(); diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponent.qml b/src/AutoPilotPlugins/PX4/FlightModesComponent.qml index 48e8bc575..7b8fdd5fe 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponent.qml +++ b/src/AutoPilotPlugins/PX4/FlightModesComponent.qml @@ -42,6 +42,9 @@ QGCView { // User visible strings + readonly property string title: "FLIGHT MODES CONFIG" + + property string topHelpText: "Assign Flight Modes to radio control channels and adjust the thresholds for triggering them. " + "You can assign multiple flight modes to a single channel. " + "Turn your radio control on to test switch settings. " + @@ -117,6 +120,8 @@ QGCView { readonly property real modeSpacing: ScreenTools.defaultFontPixelHeight / 3 + property Fact rcInMode: controller.getParameterFact(-1, "COM_RC_IN_MODE") + QGCPalette { id: qgcPal; colorGroupEnabled: panel.enabled } FlightModesComponentController { @@ -130,7 +135,13 @@ QGCView { interval: 200 running: true - onTriggered: recalcModePositions() + onTriggered: { + if (rcInMode.value == 1) { + showDialog(joystickEnabledDialogComponent, title, 50, 0) + } else { + recalcModePositions() + } + } } function recalcModePositions() { @@ -188,6 +199,14 @@ QGCView { id: panel anchors.fill: parent + Component { + id: joystickEnabledDialogComponent + + QGCViewMessage { + message: "Flight Mode Config is disabled since you have a Joystick enabled." + } + } + ScrollView { id: scroll anchors.fill: parent @@ -201,7 +220,7 @@ QGCView { id: header width: parent.width font.pixelSize: ScreenTools.largeFontPixelSize - text: "FLIGHT MODES CONFIG" + text: title } Item { diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 6ea6c5377..0cfa27130 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -107,28 +107,20 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) Q_ASSERT(_vehicle); if (pluginReady()) { - bool noRCTransmitter = false; - if (parameterExists(FactSystem::defaultComponentId, "COM_RC_IN_MODE")) { - Fact* rcFact = getParameterFact(FactSystem::defaultComponentId, "COM_RC_IN_MODE"); - noRCTransmitter = rcFact->value().toInt() == 1; - } - _airframeComponent = new AirframeComponent(_vehicle->uas(), this); Q_CHECK_PTR(_airframeComponent); _airframeComponent->setupTriggerSignals(); _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent)); - if (!noRCTransmitter) { - _radioComponent = new RadioComponent(_vehicle->uas(), this); - Q_CHECK_PTR(_radioComponent); - _radioComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); - - _flightModesComponent = new FlightModesComponent(_vehicle->uas(), this); - Q_CHECK_PTR(_flightModesComponent); - _flightModesComponent->setupTriggerSignals(); - _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent)); - } + _radioComponent = new RadioComponent(_vehicle->uas(), this); + Q_CHECK_PTR(_radioComponent); + _radioComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent)); + + _flightModesComponent = new FlightModesComponent(_vehicle->uas(), this); + Q_CHECK_PTR(_flightModesComponent); + _flightModesComponent->setupTriggerSignals(); + _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent)); _sensorsComponent = new SensorsComponent(_vehicle->uas(), this); Q_CHECK_PTR(_sensorsComponent); diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.cc b/src/AutoPilotPlugins/PX4/RadioComponent.cc index 1e8c9e48b..02e9f8320 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.cc +++ b/src/AutoPilotPlugins/PX4/RadioComponent.cc @@ -53,18 +53,20 @@ QString RadioComponent::iconResource(void) const bool RadioComponent::requiresSetup(void) const { - return true; + return _autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->value().toInt() == 1 ? false : true; } bool RadioComponent::setupComplete(void) const { - // The best we can do to detect the need for a radio calibration is look for attitude - // controls to be mapped. - QStringList attitudeMappings; - attitudeMappings << "RC_MAP_ROLL" << "RC_MAP_PITCH" << "RC_MAP_YAW" << "RC_MAP_THROTTLE"; - foreach(QString mapParam, attitudeMappings) { - if (_autopilot->getParameterFact(FactSystem::defaultComponentId, mapParam)->value().toInt() == 0) { - return false; + if (_autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->value().toInt() != 1) { + // The best we can do to detect the need for a radio calibration is look for attitude + // controls to be mapped. + QStringList attitudeMappings; + attitudeMappings << "RC_MAP_ROLL" << "RC_MAP_PITCH" << "RC_MAP_YAW" << "RC_MAP_THROTTLE"; + foreach(QString mapParam, attitudeMappings) { + if (_autopilot->getParameterFact(FactSystem::defaultComponentId, mapParam)->value().toInt() == 0) { + return false; + } } } @@ -113,11 +115,14 @@ QUrl RadioComponent::summaryQmlSource(void) const QString RadioComponent::prerequisiteSetup(void) const { - PX4AutoPilotPlugin* plugin = dynamic_cast(_autopilot); - Q_ASSERT(plugin); - - if (!plugin->airframeComponent()->setupComplete()) { - return plugin->airframeComponent()->name(); + if (_autopilot->getParameterFact(-1, "COM_RC_IN_MODE")->value().toInt() != 1) { + PX4AutoPilotPlugin* plugin = dynamic_cast(_autopilot); + Q_ASSERT(plugin); + + if (!plugin->airframeComponent()->setupComplete()) { + return plugin->airframeComponent()->name(); + + } } return QString(); diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.qml b/src/AutoPilotPlugins/PX4/RadioComponent.qml index cacafa4da..aa88b2d2b 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.qml +++ b/src/AutoPilotPlugins/PX4/RadioComponent.qml @@ -47,10 +47,12 @@ QGCView { property bool controllerCompleted: false property bool controllerAndViewReady: false + property Fact rcInMode: controller.getParameterFact(-1, "COM_RC_IN_MODE") + function updateChannelCount() { if (controllerAndViewReady) { - if (joystickManager.activeJoystick && joystickManager.activeJoystick.enabled) { + if (rcInMode.value == 1) { showDialog(joystickEnabledDialogComponent, dialogTitle, 50, 0) } /* diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 04bea5a11..11781f482 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -44,7 +44,6 @@ 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", @@ -58,13 +57,14 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlI : _sdlIndex(sdlIndex) , _exitThread(false) , _name(name) - , _enabled(false) , _calibrated(false) , _calibrating(false) , _axisCount(axisCount) , _buttonCount(buttonCount) , _lastButtonBits(0) , _throttleMode(ThrottleModeCenterZero) + , _activeVehicle(NULL) + , _pollingStartedForCalibration(false) #endif // __mobile__ { #ifdef __mobile__ @@ -105,12 +105,11 @@ void Joystick::_loadSettings(void) 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; + qCDebug(JoystickLog) << "_loadSettings calibrated:throttlemode:badsettings" << _calibrated << _throttleMode << badSettings; QString minTpl ("Axis%1Min"); QString maxTpl ("Axis%1Max"); @@ -154,8 +153,6 @@ void Joystick::_loadSettings(void) if (badSettings) { _calibrated = false; - _enabled = false; - settings.setValue(_calibratedSettingsKey, false); settings.setValue(_calibratedSettingsKey, false); } } @@ -168,10 +165,9 @@ void Joystick::_saveSettings(void) settings.beginGroup(_name); settings.setValue(_calibratedSettingsKey, _calibrated); - settings.setValue(_enabledSettingsKey, _enabled && _calibrated); settings.setValue(_throttleModeSettingsKey, _throttleMode); - qCDebug(JoystickLog) << "_saveSettings calibrated:enabled:throttlemode" << _calibrated << _enabled << _throttleMode; + qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode" << _calibrated << _throttleMode; QString minTpl ("Axis%1Min"); QString maxTpl ("Axis%1Max"); @@ -251,7 +247,6 @@ float Joystick::_adjustRange(int value, Calibration_t calibration) void Joystick::run(void) { SDL_Joystick* sdlJoystick = SDL_JoystickOpen(_sdlIndex); - Vehicle * activeVehicle = MultiVehicleManager::instance()->activeVehicle(); if (!sdlJoystick) { qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError(); @@ -278,7 +273,7 @@ void Joystick::run(void) } } - if (_calibrated && _enabled && !_calibrating) { + if (_calibrated && !_calibrating) { int axis = _rgFunctionAxis[rollFunction]; float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]); @@ -306,7 +301,7 @@ void Joystick::run(void) // Set up button pressed information // We only send the buttons the firmwware has reserved - int reservedButtonCount = activeVehicle->manualControlReservedButtonCount(); + int reservedButtonCount = _activeVehicle->manualControlReservedButtonCount(); if (reservedButtonCount == -1) { reservedButtonCount = _buttonCount; } @@ -347,7 +342,7 @@ void Joystick::run(void) qCDebug(JoystickValuesLog) << "roll:pitch:yaw:throttle" << roll << -pitch << yaw << throttle; - emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, activeVehicle->joystickMode()); + emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode()); } // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms) @@ -357,27 +352,37 @@ void Joystick::run(void) SDL_JoystickClose(sdlJoystick); } -void Joystick::startPolling(void) +void Joystick::startPolling(Vehicle* vehicle) { - if (enabled()) { - UAS* uas = MultiVehicleManager::instance()->activeVehicle()->uas(); + if (isRunning()) { + if (_calibrating && vehicle != _activeVehicle) { + // Joystick was previously disabled, but now enabled from config screen + _activeVehicle = vehicle; + _pollingStartedForCalibration = false; + } + } else { + _activeVehicle = vehicle; + + UAS* uas = _activeVehicle->uas(); connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); + + _exitThread = false; + start(); } - - _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; + if (isRunning()) { + UAS* uas = _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) @@ -484,39 +489,24 @@ void Joystick::setThrottleMode(int mode) emit throttleModeChanged(_throttleMode); } -bool Joystick::enabled(void) +void Joystick::startCalibration(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; - } + _calibrating = true; - return _enabled && _calibrated && (fact->value().toInt() == 2); + if (!isRunning()) { + _pollingStartedForCalibration = true; + startPolling(MultiVehicleManager::instance()->activeVehicle()); + } } -void Joystick::setEnabled(bool enabled) +void Joystick::stopCalibration(void) { - 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(); + if (_calibrating) { + _calibrating = false; + + if (_pollingStartedForCalibration) { + stopPolling(); + } } } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 46af7ed0c..7a87b794e 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -66,7 +66,6 @@ public: 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) @@ -80,7 +79,7 @@ public: 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 startPolling(Vehicle* vehicle); void stopPolling(void); void setCalibration(int axis, Calibration_t& calibration); @@ -97,11 +96,11 @@ public: int throttleMode(void); void setThrottleMode(int mode); - bool enabled(void); - void setEnabled(bool enabled); + /// Calibration is about to start + void startCalibration(void); - bool calibrating(void) { return _calibrating; } - void setCalibrating(bool calibrating) { _calibrating = calibrating; } + /// Calibration has ended + void stopCalibration(void); signals: void calibratedChanged(bool calibrated); @@ -140,7 +139,6 @@ private: bool _exitThread; ///< true: signal thread to exit QString _name; - bool _enabled; bool _calibrated; bool _calibrating; int _axisCount; @@ -157,6 +155,9 @@ private: quint16 _lastButtonBits; ThrottleMode_t _throttleMode; + + Vehicle* _activeVehicle; + bool _pollingStartedForCalibration; #endif // __mobile__ private: @@ -166,7 +167,6 @@ private: static const char* _calibratedSettingsKey; static const char* _buttonActionSettingsKey; static const char* _throttleModeSettingsKey; - static const char* _enabledSettingsKey; }; #endif diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 86e30aa91..7064b35af 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -26,7 +26,6 @@ #include "MultiVehicleManager.h" #include "AutoPilotPlugin.h" -#include "JoystickManager.h" #include "MAVLinkProtocol.h" #include "UAS.h" @@ -87,14 +86,8 @@ bool MultiVehicleManager::notifyHeartbeatInfo(LinkInterface* link, int vehicleId /// This slot is connected to the Vehicle::allLinksDestroyed signal such that the Vehicle is deleted /// and all other right things happen when the Vehicle goes away. -void MultiVehicleManager::_deleteVehiclePhase1(void) +void MultiVehicleManager::_deleteVehiclePhase1(Vehicle* vehicle) { - Vehicle* vehicle = dynamic_cast(sender()); - if (!vehicle) { - qWarning() << "Dynamic cast failed!"; - return; - } - _vehicleBeingDeleted = vehicle; // Remove from map @@ -110,17 +103,9 @@ void MultiVehicleManager::_deleteVehiclePhase1(void) qWarning() << "Vehicle not found in map!"; } - // Disconnect the vehicle from the uas + vehicle->setActive(false); vehicle->uas()->clearVehicle(); -#ifndef __mobile__ - // Disconnect joystick - Joystick* joystick = JoystickManager::instance()->activeJoystick(); - if (joystick) { - joystick->stopPolling(); - } -#endif - // First we must signal that a vehicle is no longer available. _activeVehicleAvailable = false; _parameterReadyVehicleAvailable = false; @@ -151,6 +136,7 @@ void MultiVehicleManager::_deleteVehiclePhase2 (void) emit activeVehicleChanged(newActiveVehicle); if (_activeVehicle) { + _activeVehicle->setActive(true); emit activeVehicleAvailableChanged(true); if (_activeVehicle->autopilotPlugin()->pluginReady()) { emit parameterReadyVehicleAvailableChanged(true); @@ -164,14 +150,8 @@ void MultiVehicleManager::setActiveVehicle(Vehicle* vehicle) { if (vehicle != _activeVehicle) { if (_activeVehicle) { -#ifndef __mobile__ - // Disconnect joystick - Joystick* joystick = JoystickManager::instance()->activeJoystick(); - if (joystick) { - joystick->stopPolling(); - } -#endif - + _activeVehicle->setActive(false); + // The sequence of signals is very important in order to not leave Qml elements connected // to a non-existent vehicle. @@ -197,6 +177,7 @@ void MultiVehicleManager::_setActiveVehiclePhase2(void) // And finally vehicle availability if (_activeVehicle) { + _activeVehicle->setActive(true); _activeVehicleAvailable = true; emit activeVehicleAvailableChanged(true); @@ -217,14 +198,6 @@ void MultiVehicleManager::_autopilotPluginReadyChanged(bool pluginReady) } if (autopilot->vehicle() == _activeVehicle) { -#ifndef __mobile__ - // Connect joystick - Joystick* joystick = JoystickManager::instance()->activeJoystick(); - if (joystick && joystick->enabled()) { - joystick->startPolling(); - } -#endif - _parameterReadyVehicleAvailable = pluginReady; emit parameterReadyVehicleAvailableChanged(pluginReady); } diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index 05c2da55d..9a4c0ff7e 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -82,7 +82,7 @@ signals: void _deleteVehiclePhase2Signal(void); private slots: - void _deleteVehiclePhase1(void); + void _deleteVehiclePhase1(Vehicle* vehicle); void _deleteVehiclePhase2(void); void _setActiveVehiclePhase2(void); void _autopilotPluginReadyChanged(bool pluginReady); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index cb62bdda3..ee0cf8687 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -29,6 +29,7 @@ #include "AutoPilotPluginManager.h" #include "UASMessageHandler.h" #include "UAS.h" +#include "JoystickManager.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") @@ -36,15 +37,18 @@ 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"; +const char* Vehicle::_settingsGroup = "Vehicle%1"; // %1 replaced with mavlink system id +const char* Vehicle::_joystickModeSettingsKey = "JoystickMode"; +const char* Vehicle::_joystickEnabledSettingsKey = "JoystickEnabled"; Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) : _id(vehicleId) + , _active(false) , _firmwareType(firmwareType) , _firmwarePlugin(NULL) , _autopilotPlugin(NULL) , _joystickMode(JoystickModeRC) + , _joystickEnabled(false) , _uas(NULL) , _mav(NULL) , _currentMessageCount(0) @@ -81,8 +85,6 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) , _wpm(NULL) , _updateCount(0) { - _loadSettings(); - _addLink(link); connect(MAVLinkProtocol::instance(), &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); @@ -96,9 +98,9 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude); connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude); - _firmwarePlugin = FirmwarePluginManager::instance()->firmwarePluginForAutopilot(firmwareType); + _firmwarePlugin = FirmwarePluginManager::instance()->firmwarePluginForAutopilot(firmwareType); _autopilotPlugin = AutoPilotPluginManager::instance()->newAutopilotPluginForVehicle(this); - + // Refresh timer connect(_refreshTimer, SIGNAL(timeout()), this, SLOT(_checkUpdate())); _refreshTimer->setInterval(UPDATE_TIMER); @@ -145,6 +147,8 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) } _setSystemType(_mav, _mav->getSystemType()); _updateArmingState(_mav->isArmed()); + + _loadSettings(); } Vehicle::~Vehicle() @@ -225,7 +229,7 @@ void Vehicle::_linkDisconnected(LinkInterface* link) } if (_links.count() == 0) { - emit allLinksDisconnected(); + emit allLinksDisconnected(this); } } @@ -836,6 +840,8 @@ void Vehicle::_loadSettings(void) if (!convertOk) { _joystickMode = JoystickModeRC; } + + _joystickEnabled = settings.value(_joystickEnabledSettingsKey, false).toBool(); } void Vehicle::_saveSettings(void) @@ -845,6 +851,7 @@ void Vehicle::_saveSettings(void) settings.beginGroup(QString(_settingsGroup).arg(_id)); settings.setValue(_joystickModeSettingsKey, _joystickMode); + settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled); } int Vehicle::joystickMode(void) @@ -868,7 +875,54 @@ QStringList Vehicle::joystickModes(void) { QStringList list; - list << "Simulate RC" << "Attitude" << "Position" << "Force" << "Velocity"; + list << "Normal" << "Attitude" << "Position" << "Force" << "Velocity"; return list; } + +bool Vehicle::joystickEnabled(void) +{ + return _joystickEnabled; +} + +void Vehicle::setJoystickEnabled(bool enabled) +{ + Fact* fact = _autopilotPlugin->getParameterFact(FactSystem::defaultComponentId, "COM_RC_IN_MODE"); + if (!fact) { + qCWarning(JoystickLog) << "Missing COM_RC_IN_MODE parameter"; + } + fact->setValue(enabled ? 1 : 0); + + _joystickEnabled = enabled; + _startJoystick(_joystickEnabled); + _saveSettings(); +} + +void Vehicle::_startJoystick(bool start) +{ + Joystick* joystick = JoystickManager::instance()->activeJoystick(); + + if (joystick) { +#ifndef __mobile__ + if (start) { + if (_joystickEnabled) { + joystick->startPolling(this); + } + } else { + joystick->stopPolling(); + } +#endif + } +} + +bool Vehicle::active(void) +{ + return _active; +} + +void Vehicle::setActive(bool active) +{ + _active = active; + + _startJoystick(_active); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index fc0271acd..424812891 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -102,7 +102,7 @@ public: Q_PROPERTY(int manualControlReservedButtonCount READ manualControlReservedButtonCount CONSTANT) typedef enum { - JoystickModeRC, ///< Joystick emulates am RC Transmitter + JoystickModeRC, ///< Joystick emulates an RC Transmitter JoystickModeAttitude, JoystickModePosition, JoystickModeForce, @@ -119,6 +119,16 @@ public: Q_PROPERTY(QStringList joystickModes READ joystickModes CONSTANT) QStringList joystickModes(void); + // Enable/Disable joystick for this vehicle + Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) + bool joystickEnabled(void); + void setJoystickEnabled(bool enabled); + + // Is vehicle active with respect to current active vehicle in QGC + Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) + bool active(void); + void setActive(bool active); + // Property accesors int id(void) { return _id; } MAV_AUTOPILOT firmwareType(void) { return _firmwareType; } @@ -199,9 +209,11 @@ public slots: void setLongitude(double longitude); signals: - void allLinksDisconnected(void); + void allLinksDisconnected(Vehicle* vehicle); void coordinateChanged(QGeoCoordinate coordinate); void joystickModeChanged(int mode); + void joystickEnabledChanged(bool enabled); + void activeChanged(bool active); /// Used internally to move sendMessage call to main thread void _sendMessageOnThread(mavlink_message_t message); @@ -274,15 +286,17 @@ private: void _addLink(LinkInterface* link); void _loadSettings(void); void _saveSettings(void); + void _startJoystick(bool start); bool _isAirplane (); void _addChange (int id); float _oneDecimal (float value); private: - int _id; ///< Mavlink system id - MAV_AUTOPILOT _firmwareType; + int _id; ///< Mavlink system id + bool _active; + MAV_AUTOPILOT _firmwareType; FirmwarePlugin* _firmwarePlugin; AutoPilotPlugin* _autopilotPlugin; @@ -292,6 +306,7 @@ private: QList _links; JoystickMode_t _joystickMode; + bool _joystickEnabled; UAS* _uas; @@ -341,5 +356,6 @@ private: static const char* _settingsGroup; static const char* _joystickModeSettingsKey; + static const char* _joystickEnabledSettingsKey; }; #endif diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml index f10491f1f..1fba08978 100644 --- a/src/VehicleSetup/JoystickConfig.qml +++ b/src/VehicleSetup/JoystickConfig.qml @@ -29,6 +29,7 @@ import QGroundControl.Palette 1.0 import QGroundControl.Controls 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Controllers 1.0 +import QGroundControl.FactSystem 1.0 /// Joystick Config QGCView { @@ -45,17 +46,6 @@ QGCView { 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 @@ -64,14 +54,11 @@ QGCView { nextButton: nextButton skipButton: skipButton - onAxisCountChanged: updateAxisCount() - Component.onCompleted: { controllerCompleted = true if (rootQGCView.completedSignalled) { controllerAndViewReady = true controller.start() - updateAxisCount() } } } @@ -88,14 +75,6 @@ QGCView { 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 @@ -375,6 +354,18 @@ QGCView { width: parent.width spacing: ScreenTools.defaultFontPixelHeight + + QGCCheckBox { + enabled: allowEnableDisable + text: _activeJoystick.calibrated ? (rcInMode.value < 2 ? "Enable joystick input, disable RC input" : "Enable/Disable not allowed (COM_RC_IN_MODE=2)") : "Enable/Disable not allowed (Calibrate First)" + checked: _activeVehicle.joystickEnabled + + property bool allowEnableDisable: _activeJoystick.calibrated && rcInMode.value < 2 + property Fact rcInMode: controller.getParameterFact(-1, "COM_RC_IN_MODE") + + onClicked: _activeVehicle.joystickEnabled = checked + } + Row { width: parent.width spacing: ScreenTools.defaultFontPixelWidth @@ -394,16 +385,6 @@ QGCView { } } - 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 diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc index a031c2af2..57148b851 100644 --- a/src/VehicleSetup/JoystickConfigController.cc +++ b/src/VehicleSetup/JoystickConfigController.cc @@ -56,21 +56,24 @@ const char* JoystickConfigController::_imagePitchDown = "joystickPitchDown.p 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(); +JoystickConfigController::JoystickConfigController(void) + : _activeJoystick(NULL) + , _currentStep(-1) + , _axisCount(0) + , _calState(calStateAxisWait) + , _statusText(NULL) + , _cancelButton(NULL) + , _nextButton(NULL) + , _skipButton(NULL) +{ + JoystickManager* joystickManager = JoystickManager::instance(); + + connect(joystickManager, &JoystickManager::activeJoystickChanged, this, &JoystickConfigController::_activeJoystickChanged); + _activeJoystickChanged(joystickManager->activeJoystick()); + _loadSettings(); _resetInternalCalibrationValues(); + _activeJoystick->startCalibration(); } void JoystickConfigController::start(void) @@ -81,6 +84,7 @@ void JoystickConfigController::start(void) JoystickConfigController::~JoystickConfigController() { + _activeJoystick->stopCalibration(); _storeSettings(); } @@ -551,8 +555,6 @@ void JoystickConfigController::_startCalibration(void) _currentStep = 0; _setupCurrentState(); - - JoystickManager::instance()->activeJoystick()->setCalibrating(true); } /// @brief Cancels the calibration process, setting things back to initial state. @@ -570,8 +572,6 @@ void JoystickConfigController::_stopCalibration(void) _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. @@ -737,3 +737,16 @@ void JoystickConfigController::_signalAllAttiudeValueChanges(void) emit yawAxisReversedChanged(yawAxisReversed()); emit throttleAxisReversedChanged(throttleAxisReversed()); } + +void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) +{ + if (_activeJoystick) { + disconnect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged); + _activeJoystick = NULL; + } + + if (joystick) { + _activeJoystick = joystick; + connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged); + } +} diff --git a/src/VehicleSetup/JoystickConfigController.h b/src/VehicleSetup/JoystickConfigController.h index caeacb26e..212bfc5a8 100644 --- a/src/VehicleSetup/JoystickConfigController.h +++ b/src/VehicleSetup/JoystickConfigController.h @@ -126,6 +126,7 @@ signals: void nextButtonMessageBoxDisplayed(void); private slots: + void _activeJoystickChanged(Joystick* joystick); void _axisValueChanged(int axis, int value); private: @@ -161,6 +162,8 @@ private: int axisTrim; ///< Trim position }; + Joystick* _activeJoystick; + int _currentStep; ///< Current step of state machine const struct stateMachineEntry* _getStateMachineEntry(int step); diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml index bdb22a292..f895818ea 100644 --- a/src/VehicleSetup/SetupView.qml +++ b/src/VehicleSetup/SetupView.qml @@ -198,6 +198,7 @@ Rectangle { SubMenuButton { width: buttonWidth imageResource: modelData.iconResource + setupIndicator: modelData.requiresSetup setupComplete: modelData.setupComplete exclusiveGroup: setupButtonGroup text: modelData.name.toUpperCase() -- 2.22.0