diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc
index 17ce67b..f86dbab 100644
--- a/src/Joystick/Joystick.cc
+++ b/src/Joystick/Joystick.cc
@@ -71,9 +71,9 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
Joystick::~Joystick()
{
- delete _rgAxisValues;
- delete _rgCalibration;
- delete _rgButtonValues;
+ delete[] _rgAxisValues;
+ delete[] _rgCalibration;
+ delete[] _rgButtonValues;
}
void Joystick::_loadSettings(void)
@@ -286,7 +286,7 @@ void Joystick::run(void)
}
}
- if (_calibrationMode != CalibrationModeCalibrating) {
+ if (_calibrationMode != CalibrationModeCalibrating && _calibrated) {
int axis = _rgFunctionAxis[rollFunction];
float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
@@ -406,6 +406,9 @@ void Joystick::startPolling(Vehicle* vehicle)
vehicle->setJoystickEnabled(false);
}
+ // Update qml in case of joystick transition
+ emit calibratedChanged(_calibrated);
+
// Only connect the new vehicle if it wants joystick data
if (vehicle->joystickEnabled()) {
_pollingStartedForCalibration = false;
@@ -430,14 +433,15 @@ void Joystick::stopPolling(void)
if (_activeVehicle && _activeVehicle->joystickEnabled()) {
UAS* uas = _activeVehicle->uas();
-
+ // Neutral attitude controls
+ // emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode());
disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
}
// FIXME: ****
//disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
_exitThread = true;
- }
+ }
}
void Joystick::setCalibration(int axis, Calibration_t& calibration)
diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h
index c9915ea..8b8b524 100644
--- a/src/Joystick/Joystick.h
+++ b/src/Joystick/Joystick.h
@@ -88,6 +88,11 @@ public:
QVariantList buttonActions(void);
QString name(void) { return _name; }
+
+ // Joystick index used by sdl library
+ // Settable because sdl library remaps indicies after certain events
+ virtual int index(void) = 0;
+ virtual void setIndex(int index) = 0;
int throttleMode(void);
void setThrottleMode(int mode);
diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc
index a8d3f6f..0e3fa6a 100644
--- a/src/Joystick/JoystickManager.cc
+++ b/src/Joystick/JoystickManager.cc
@@ -55,10 +55,22 @@ void JoystickManager::setToolbox(QGCToolbox *toolbox)
QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
}
-void JoystickManager::discoverJoysticks()
+void JoystickManager::init()
{
+ if (JoystickSDL::init()) {
+ _setActiveJoystickFromSettings();
+ connect(&_joystickCheckTimer, &QTimer::timeout, this, &JoystickManager::_updateAvailableJoysticks);
+ _joystickCheckTimer.start(250);
+ }
+}
+
+void JoystickManager::_setActiveJoystickFromSettings(void)
+{
+ QMap<QString,Joystick*> newMap;
+
#ifdef __sdljoystick__
- _name2JoystickMap = JoystickSDL::discover(_multiVehicleManager);
+ // Get the latest joystick mapping
+ newMap = JoystickSDL::discover(_multiVehicleManager);
#elif defined(__android__)
/*
* Android Joystick not yet supported
@@ -66,16 +78,30 @@ void JoystickManager::discoverJoysticks()
*/
#endif
- if (!_name2JoystickMap.count()) {
- qCDebug(JoystickManagerLog) << "\tnone found";
- return;
+ // Check to see if our current mapping contains any joysticks that are not in the new mapping
+ // If so, those joysticks have been unplugged, and need to be cleaned up
+ QMap<QString, Joystick*>::iterator i;
+ for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) {
+ if (!newMap.contains(i.key())) {
+ qCDebug(JoystickManagerLog) << "Releasing joystick:" << i.key();
+ if (_activeJoystick && !newMap.contains(_activeJoystick->name())) {
+ qCDebug(JoystickManagerLog) << "\twas active";
+ _activeJoystick->stopPolling();
+ _activeJoystick = NULL;
+ }
+ // FIXME: Don't leave this hanging! We need to free the object.
+ //i.value()->deleteLater();
+ }
}
- _setActiveJoystickFromSettings();
-}
+ _name2JoystickMap = newMap;
+ emit availableJoysticksChanged();
-void JoystickManager::_setActiveJoystickFromSettings(void)
-{
+ if (!_name2JoystickMap.count()) {
+ setActiveJoystick(NULL);
+ return;
+ }
+
QSettings settings;
settings.beginGroup(_settingsGroup);
@@ -97,23 +123,30 @@ Joystick* JoystickManager::activeJoystick(void)
void JoystickManager::setActiveJoystick(Joystick* joystick)
{
QSettings settings;
-
- if (!_name2JoystickMap.contains(joystick->name())) {
+
+ if (joystick != NULL && !_name2JoystickMap.contains(joystick->name())) {
qCWarning(JoystickManagerLog) << "Set active not in map" << joystick->name();
return;
}
-
+
+ if (_activeJoystick == joystick) {
+ return;
+ }
+
if (_activeJoystick) {
_activeJoystick->stopPolling();
}
_activeJoystick = joystick;
- settings.beginGroup(_settingsGroup);
- settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
-
- emit activeJoystickChanged(_activeJoystick);
- emit activeJoystickNameChanged(_activeJoystick->name());
+ if (_activeJoystick != NULL) {
+ qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name();
+
+ settings.beginGroup(_settingsGroup);
+ settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
+ emit activeJoystickChanged(_activeJoystick);
+ emit activeJoystickNameChanged(_activeJoystick->name());
+ }
}
QVariantList JoystickManager::joysticks(void)
@@ -146,3 +179,25 @@ void JoystickManager::setActiveJoystickName(const QString& name)
setActiveJoystick(_name2JoystickMap[name]);
}
+
+void JoystickManager::_updateAvailableJoysticks(void)
+{
+ SDL_Event event;
+ while (SDL_PollEvent(&event)) {
+ switch(event.type) {
+ case SDL_QUIT:
+ qCDebug(JoystickManagerLog) << "SDL ERROR:" << SDL_GetError();
+ break;
+ case SDL_JOYDEVICEADDED:
+ qCDebug(JoystickManagerLog) << "Joystick added:" << event.jdevice.which;
+ _setActiveJoystickFromSettings();
+ break;
+ case SDL_JOYDEVICEREMOVED:
+ qCDebug(JoystickManagerLog) << "Joystick removed:" << event.jdevice.which;
+ _setActiveJoystickFromSettings();
+ break;
+ default:
+ break;
+ }
+ }
+}
diff --git a/src/Joystick/JoystickManager.h b/src/Joystick/JoystickManager.h
index 8460b75..98a3d09 100644
--- a/src/Joystick/JoystickManager.h
+++ b/src/Joystick/JoystickManager.h
@@ -29,8 +29,8 @@ public:
~JoystickManager();
/// List of available joysticks
- Q_PROPERTY(QVariantList joysticks READ joysticks CONSTANT)
- Q_PROPERTY(QStringList joystickNames READ joystickNames CONSTANT)
+ Q_PROPERTY(QVariantList joysticks READ joysticks NOTIFY availableJoysticksChanged)
+ Q_PROPERTY(QStringList joystickNames READ joystickNames NOTIFY availableJoysticksChanged)
/// Active joystick
Q_PROPERTY(Joystick* activeJoystick READ activeJoystick WRITE setActiveJoystick NOTIFY activeJoystickChanged)
@@ -49,13 +49,15 @@ public:
virtual void setToolbox(QGCToolbox *toolbox);
public slots:
- void discoverJoysticks();
+ void init();
signals:
void activeJoystickChanged(Joystick* joystick);
void activeJoystickNameChanged(const QString& name);
+ void availableJoysticksChanged(void);
private slots:
+ void _updateAvailableJoysticks(void);
private:
void _setActiveJoystickFromSettings(void);
@@ -67,6 +69,8 @@ private:
static const char * _settingsGroup;
static const char * _settingsKeyActiveJoystick;
+
+ QTimer _joystickCheckTimer;
};
#endif
diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc
index 303c300..4e074e9 100644
--- a/src/Joystick/JoystickSDL.cc
+++ b/src/Joystick/JoystickSDL.cc
@@ -12,15 +12,21 @@ JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, in
{
}
-QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) {
- static QMap<QString, Joystick*> ret;
-
+bool JoystickSDL::init(void) {
if (SDL_InitSubSystem(SDL_INIT_GAMECONTROLLER | SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
+ SDL_JoystickEventState(SDL_ENABLE);
qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError();
- return ret;
+ return false;
}
_loadGameControllerMappings();
+ return true;
+}
+
+QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) {
+ static QMap<QString, Joystick*> ret;
+
+ QMap<QString,Joystick*> newRet;
// Load available joysticks
@@ -29,6 +35,7 @@ QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicl
for (int i=0; i<SDL_NumJoysticks(); i++) {
QString name = SDL_JoystickNameForIndex(i);
+ // TODO use GUID instead of name in case of two joysticks with same name
if (!ret.contains(name)) {
int axisCount, buttonCount, hatCount;
bool isGameController;
@@ -57,11 +64,24 @@ QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicl
}
qCDebug(JoystickLog) << "\t" << name << "axes:" << axisCount << "buttons:" << buttonCount << "hats:" << hatCount << "isGC:" << isGameController;
- ret[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager);
+ newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager);
} else {
+ newRet[name] = ret[name];
+ if (newRet[name]->index() != i) {
+ newRet[name]->setIndex(i); // This joystick index has been remapped by SDL
+ }
+ // Anything left in ret after we exit the loop has been removed (unplugged) and needs to be cleaned up.
+ // We will handle that in JoystickManager in case the removed joystick was in use.
+ ret.remove(name);
qCDebug(JoystickLog) << "\tSkipping duplicate" << name;
}
}
+
+ if (!newRet.count()) {
+ qCDebug(JoystickLog) << "\tnone found";
+ }
+
+ ret = newRet;
return ret;
}
@@ -93,11 +113,32 @@ bool JoystickSDL::_open(void) {
return false;
}
+ qCDebug(JoystickLog) << "Opened joystick at" << sdlJoystick;
+
return true;
}
void JoystickSDL::_close(void) {
- SDL_JoystickClose(sdlJoystick);
+ if (sdlJoystick == NULL) {
+ qCDebug(JoystickLog) << "Attempt to close null joystick!";
+ return;
+ }
+
+ qCDebug(JoystickLog) << "Closing" << SDL_JoystickName(sdlJoystick) << "at" << sdlJoystick;
+
+ // We get a segfault if we try to close a joystick that has been detached
+ if (SDL_JoystickGetAttached(sdlJoystick) == SDL_FALSE) {
+ qCDebug(JoystickLog) << "\tJoystick is not attached!";
+ } else {
+
+ if (SDL_JoystickInstanceID(sdlJoystick) != -1) {
+ qCDebug(JoystickLog) << "\tID:" << SDL_JoystickInstanceID(sdlJoystick);
+ SDL_JoystickClose(sdlJoystick);
+ }
+ }
+
+ sdlJoystick = NULL;
+ sdlController = NULL;
}
bool JoystickSDL::_update(void)
@@ -131,4 +172,3 @@ uint8_t JoystickSDL::_getHat(int hat,int i) {
}
return 0;
}
-
diff --git a/src/Joystick/JoystickSDL.h b/src/Joystick/JoystickSDL.h
index 2c4a9ca..e8111a7 100644
--- a/src/Joystick/JoystickSDL.h
+++ b/src/Joystick/JoystickSDL.h
@@ -13,6 +13,10 @@ public:
JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager);
static QMap<QString, Joystick*> discover(MultiVehicleManager* _multiVehicleManager);
+ static bool init(void);
+
+ int index(void) { return _index; }
+ void setIndex(int index) { _index = index; }
private:
static void _loadGameControllerMappings();
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index d436f68..6a7de14 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -429,8 +429,8 @@ bool QGCApplication::_initForNormalAppBoot(void)
// Load known link configurations
toolbox()->linkManager()->loadLinkConfigurationList();
- // Probe for joysticks - TODO: manage on a timer or use events to deal with hotplug
- toolbox()->joystickManager()->discoverJoysticks();
+ // Probe for joysticks
+ toolbox()->joystickManager()->init();
if (_settingsUpgraded) {
settings.clear();
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 10c7dee..6c7df9c 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -141,6 +141,8 @@ Vehicle::Vehicle(LinkInterface* link,
{
_addLink(link);
+ connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_activeJoystickChanged);
+
_mavlink = qgcApp()->toolbox()->mavlinkProtocol();
connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived);
@@ -1295,6 +1297,12 @@ QStringList Vehicle::joystickModes(void)
return list;
}
+void Vehicle::_activeJoystickChanged(void)
+{
+ _loadSettings();
+ _startJoystick(true);
+}
+
bool Vehicle::joystickEnabled(void)
{
return _joystickEnabled;
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index c9d2c01..c291ea2 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -690,6 +690,8 @@ private slots:
void _newGeoFenceAvailable(void);
void _sendMavCommandAgain(void);
+ void _activeJoystickChanged(void);
+
private:
bool _containsLink(LinkInterface* link);
void _addLink(LinkInterface* link);
diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml
index 503c6be..58069ff 100644
--- a/src/VehicleSetup/JoystickConfig.qml
+++ b/src/VehicleSetup/JoystickConfig.qml
@@ -27,6 +27,16 @@ SetupPage {
pageName: qsTr("Joystick")
pageDescription: qsTr("Joystick Setup is used to configure a calibrate joysticks.")
+ Connections {
+ target: joystickManager
+ onAvailableJoysticksChanged: {
+ if( joystickManager.joysticks.length == 0 ) {
+ summaryButton.checked = true
+ setupView.showSummaryPanel()
+ }
+ }
+ }
+
Component {
id: pageComponent
@@ -358,11 +368,20 @@ SetupPage {
QGCCheckBox {
- enabled: checked || _activeJoystick.calibrated
+ id: enabledCheckBox
+ enabled: _activeJoystick.calibrated
text: _activeJoystick.calibrated ? qsTr("Enable joystick input") : qsTr("Enable not allowed (Calibrate First)")
checked: _activeVehicle.joystickEnabled
onClicked: _activeVehicle.joystickEnabled = checked
+
+ Connections {
+ target: joystickManager
+
+ onActiveJoystickChanged: {
+ enabledCheckBox.checked = Qt.binding(function() { return _activeJoystick.calibrated && _activeVehicle.joystickEnabled })
+ }
+ }
}
Row {
@@ -390,6 +409,18 @@ SetupPage {
joystickCombo.currentIndex = index
}
}
+
+ Connections {
+ target: joystickManager
+ onAvailableJoysticksChanged: {
+ var index = joystickCombo.find(joystickManager.activeJoystickName)
+ if (index == -1) {
+ console.warn(qsTr("Active joystick name not in combo"), joystickManager.activeJoystickName)
+ } else {
+ joystickCombo.currentIndex = index
+ }
+ }
+ }
}
}
diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc
index 505478b..8111896 100644
--- a/src/VehicleSetup/JoystickConfigController.cc
+++ b/src/VehicleSetup/JoystickConfigController.cc
@@ -69,7 +69,9 @@ void JoystickConfigController::start(void)
JoystickConfigController::~JoystickConfigController()
{
- _activeJoystick->stopCalibrationMode(Joystick::CalibrationModeMonitor);
+ if(_activeJoystick) {
+ _activeJoystick->stopCalibrationMode(Joystick::CalibrationModeMonitor);
+ }
}
/// @brief Returns the state machine entry for the specified state.
@@ -491,9 +493,10 @@ void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
int paramAxis;
paramAxis = joystick->getFunctionAxis((Joystick::AxisFunction_t)function);
-
- _rgFunctionAxisMapping[function] = paramAxis;
- _rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
+ if(paramAxis >= 0) {
+ _rgFunctionAxisMapping[function] = paramAxis;
+ _rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
+ }
}
_signalAllAttiudeValueChanges();
@@ -809,9 +812,11 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
if (_activeJoystick) {
joystickTransition = true;
disconnect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
- delete _rgAxisInfo;
- delete _axisValueSave;
- delete _axisRawValue;
+ // This will reset _rgFunctionAxis values to -1 to prevent out-of-bounds accesses
+ _resetInternalCalibrationValues();
+ delete[] _rgAxisInfo;
+ delete[] _axisValueSave;
+ delete[] _axisRawValue;
_axisCount = 0;
_activeJoystick = NULL;
}
@@ -826,6 +831,7 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
_rgAxisInfo = new struct AxisInfo[_axisCount];
_axisValueSave = new int[_axisCount];
_axisRawValue = new int[_axisCount];
+ _setInternalCalibrationValuesFromSettings();
connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
}
}