Skip to content

Q
qgroundcontrol
Commit d02b8cc9 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4477 from bluerobotics/hotplugging 

Joystick Hotplugging
parents afcf6615 4e7f33e3
Hide whitespace changes
Inline Side-by-side
Showing with 228 additions and 55 deletions
src/Joystick/Joystick.cc
View file @ d02b8cc9
......@@ -81,9 +81,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::_setDefaultCalibration(void) {
......@@ -410,7 +410,7 @@ void Joystick::run(void)
}
}
if (_calibrationMode != CalibrationModeCalibrating) {
if (_calibrationMode != CalibrationModeCalibrating && _calibrated) {
int axis = _rgFunctionAxis[rollFunction];
float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
......@@ -530,6 +530,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;
......@@ -554,14 +557,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)
......
src/Joystick/Joystick.h
View file @ d02b8cc9
......@@ -95,6 +95,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;
virtual bool requiresCalibration(void) { return true; }
......
src/Joystick/JoystickManager.cc
View file @ d02b8cc9
......@@ -48,17 +48,35 @@ JoystickManager::~JoystickManager() {
void JoystickManager::setToolbox(QGCToolbox *toolbox)
{
QGCTool::setToolbox(toolbox);
QGCTool::setToolbox(toolbox);
_multiVehicleManager = _toolbox->multiVehicleManager();
QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
}
void JoystickManager::discoverJoysticks()
void JoystickManager::init()
{
#ifdef __sdljoystick__
_name2JoystickMap = JoystickSDL::discover(_multiVehicleManager);
if (JoystickSDL::init()) {
_setActiveJoystickFromSettings();
connect(&_joystickCheckTimer, &QTimer::timeout, this, &JoystickManager::_updateAvailableJoysticks);
_joystickCheckTimer.start(250);
}
#elif defined(__android__)
/*
* Android Joystick not yet supported
*/
#endif
}
void JoystickManager::_setActiveJoystickFromSettings(void)
{
QMap<QString,Joystick*> newMap;
#ifdef __sdljoystick__
// Get the latest joystick mapping
newMap = JoystickSDL::discover(_multiVehicleManager);
#elif defined(__android__)
/*
* Android Joystick not yet supported
......@@ -66,16 +84,31 @@ void JoystickManager::discoverJoysticks()
*/
#endif
if (_activeJoystick && !newMap.contains(_activeJoystick->name())) {
qCDebug(JoystickManagerLog) << "Active joystick removed";
setActiveJoystick(NULL);
}
// 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();
i.value()->stopPolling();
i.value()->wait(1000);
i.value()->deleteLater();
}
}
_name2JoystickMap = newMap;
emit availableJoysticksChanged();
if (!_name2JoystickMap.count()) {
qCDebug(JoystickManagerLog) << "\tnone found";
setActiveJoystick(NULL);
return;
}
_setActiveJoystickFromSettings();
}
void JoystickManager::_setActiveJoystickFromSettings(void)
{
QSettings settings;
settings.beginGroup(_settingsGroup);
......@@ -97,23 +130,31 @@ 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());
if (_activeJoystick != NULL) {
qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name();
settings.beginGroup(_settingsGroup);
settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
}
emit activeJoystickChanged(_activeJoystick);
emit activeJoystickNameChanged(_activeJoystick->name());
emit activeJoystickNameChanged(_activeJoystick?_activeJoystick->name():"");
}
QVariantList JoystickManager::joysticks(void)
......@@ -146,3 +187,31 @@ void JoystickManager::setActiveJoystickName(const QString& name)
setActiveJoystick(_name2JoystickMap[name]);
}
void JoystickManager::_updateAvailableJoysticks(void)
{
#ifdef __sdljoystick__
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;
}
}
#elif defined(__android__)
/*
* Android Joystick not yet supported
*/
#endif
}
src/Joystick/JoystickManager.h
View file @ d02b8cc9
......@@ -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
src/Joystick/JoystickSDL.cc
View file @ d02b8cc9
......@@ -13,15 +13,21 @@ JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, in
if(_isGameController) _setDefaultCalibration();
}
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
......@@ -30,6 +36,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;
......@@ -58,11 +65,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;
}
......@@ -94,11 +114,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)
......@@ -132,4 +173,3 @@ uint8_t JoystickSDL::_getHat(int hat,int i) {
}
return 0;
}
src/Joystick/JoystickSDL.h
View file @ d02b8cc9
......@@ -12,7 +12,11 @@ class JoystickSDL : public Joystick
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 QMap<QString, Joystick*> discover(MultiVehicleManager* _multiVehicleManager);
static bool init(void);
int index(void) { return _index; }
void setIndex(int index) { _index = index; }
// This can be uncommented to hide the calibration buttons for gamecontrollers in the future
// bool requiresCalibration(void) final { return !_isGameController; }
......
src/QGCApplication.cc
View file @ d02b8cc9
......@@ -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();
......
src/Vehicle/Vehicle.cc
View file @ d02b8cc9
......@@ -143,6 +143,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);
......@@ -1298,6 +1300,12 @@ QStringList Vehicle::joystickModes(void)
return list;
}
void Vehicle::_activeJoystickChanged(void)
{
_loadSettings();
_startJoystick(true);
}
bool Vehicle::joystickEnabled(void)
{
return _joystickEnabled;
......
src/Vehicle/Vehicle.h
View file @ d02b8cc9
......@@ -701,6 +701,8 @@ private slots:
void _newGeoFenceAvailable(void);
void _sendMavCommandAgain(void);
void _activeJoystickChanged(void);
private:
bool _containsLink(LinkInterface* link);
void _addLink(LinkInterface* link);
......
src/VehicleSetup/JoystickConfig.qml
View file @ d02b8cc9
......@@ -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
......@@ -359,11 +369,22 @@ SetupPage {
QGCCheckBox {
enabled: checked || _activeJoystick.calibrated
text: _activeJoystick.calibrated ? qsTr("Enable joystick input") : qsTr("Enable not allowed (Calibrate First)")
id: enabledCheckBox
enabled: _activeJoystick ? _activeJoystick.calibrated : false
text: _activeJoystick ? _activeJoystick.calibrated ? qsTr("Enable joystick input") : qsTr("Enable not allowed (Calibrate First)") : ""
checked: _activeVehicle.joystickEnabled
onClicked: _activeVehicle.joystickEnabled = checked
Connections {
target: joystickManager
onActiveJoystickChanged: {
if(_activeJoystick) {
enabledCheckBox.checked = Qt.binding(function() { return _activeJoystick.calibrated && _activeVehicle.joystickEnabled })
}
}
}
}
Row {
......@@ -391,6 +412,16 @@ SetupPage {
joystickCombo.currentIndex = index
}
}
Connections {
target: joystickManager
onAvailableJoysticksChanged: {
var index = joystickCombo.find(joystickManager.activeJoystickName)
if (index >= 0) {
joystickCombo.currentIndex = index
}
}
}
}
}
......@@ -403,7 +434,7 @@ SetupPage {
QGCRadioButton {
exclusiveGroup: throttleModeExclusiveGroup
text: qsTr("Center stick is zero throttle")
checked: _activeJoystick.throttleMode == 0
checked: _activeJoystick ? _activeJoystick.throttleMode == 0 : false
onClicked: _activeJoystick.throttleMode = 0
}
......@@ -412,11 +443,11 @@ SetupPage {
x: 20
width: parent.width
spacing: ScreenTools.defaultFontPixelWidth
visible: _activeJoystick.throttleMode == 0
visible: _activeJoystick ? _activeJoystick.throttleMode == 0 : false
QGCCheckBox {
id: accumulator
checked: _activeJoystick.accumulator
checked: _activeJoystick ? _activeJoystick.accumulator : false
text: qsTr("Spring loaded throttle smoothing")
onClicked: _activeJoystick.accumulator = checked
......@@ -426,7 +457,7 @@ SetupPage {
QGCRadioButton {
exclusiveGroup: throttleModeExclusiveGroup
text: qsTr("Full down stick is zero throttle")
checked: _activeJoystick.throttleMode == 1
checked: _activeJoystick ? _activeJoystick.throttleMode == 1 : false
onClicked: _activeJoystick.throttleMode = 1
}
......@@ -437,7 +468,7 @@ SetupPage {
QGCCheckBox {
id: exponential
checked: _activeJoystick.exponential
checked: _activeJoystick ? _activeJoystick.exponential : false
text: qsTr("Use exponential curve on roll, pitch, yaw")
onClicked: _activeJoystick.exponential = checked
......@@ -526,7 +557,7 @@ SetupPage {
Repeater {
id: buttonActionRepeater
model: _activeJoystick.totalButtonCount
model: _activeJoystick ? _activeJoystick.totalButtonCount : 0
Row {
spacing: ScreenTools.defaultFontPixelWidth
......@@ -536,7 +567,7 @@ SetupPage {
QGCCheckBox {
anchors.verticalCenter: parent.verticalCenter
checked: _activeJoystick.buttonActions[modelData] != ""
checked: _activeJoystick ? _activeJoystick.buttonActions[modelData] != "" : false
onClicked: _activeJoystick.setButtonAction(modelData, checked ? buttonActionCombo.textAt(buttonActionCombo.currentIndex) : "")
}
......@@ -562,7 +593,7 @@ SetupPage {
QGCComboBox {
id: buttonActionCombo
width: ScreenTools.defaultFontPixelWidth * 20
model: _activeJoystick.actions
model: _activeJoystick ? _activeJoystick.actions : 0
onActivated: _activeJoystick.setButtonAction(modelData, textAt(index))
Component.onCompleted: currentIndex = find(_activeJoystick.buttonActions[modelData])
......@@ -593,7 +624,7 @@ SetupPage {
Repeater {
id: jsButtonActionRepeater
model: _activeJoystick.totalButtonCount
model: _activeJoystick ? _activeJoystick.totalButtonCount : 0
Row {
spacing: ScreenTools.defaultFontPixelWidth
......@@ -719,7 +750,7 @@ SetupPage {
Repeater {
id: axisMonitorRepeater
model: _activeJoystick.axisCount
model: _activeJoystick ? _activeJoystick.axisCount : 0
width: parent.width
Row {
......@@ -771,7 +802,7 @@ SetupPage {
Repeater {
id: buttonMonitorRepeater
model: _activeJoystick.totalButtonCount
model: _activeJoystick ? _activeJoystick.totalButtonCount : 0
Rectangle {
width: ScreenTools.defaultFontPixelHeight * 1.2
......
src/VehicleSetup/JoystickConfigController.cc
View file @ d02b8cc9
......@@ -73,7 +73,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.
......@@ -495,9 +497,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;
}
}
_transmitterMode = joystick->getTXMode();
......@@ -849,9 +852,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;
}
......@@ -866,6 +871,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);
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment