From 3987ea375636d7e9488982fb13ecd4740c1a2b91 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Sun, 1 May 2016 23:38:55 +0100 Subject: [PATCH 01/18] Android Joystick support --- qgroundcontrol.pro | 13 ++- src/Joystick/Joystick.cc | 41 ++------ src/Joystick/Joystick.h | 19 ++-- src/Joystick/JoystickAndroid.cc | 220 ++++++++++++++++++++++++++++++++++++++++ src/Joystick/JoystickAndroid.h | 49 +++++++++ src/Joystick/JoystickManager.cc | 63 ++++-------- src/Joystick/JoystickManager.h | 1 + src/Joystick/JoystickSDL.cc | 73 +++++++++++++ src/QGCApplication.cc | 5 +- src/Vehicle/Vehicle.cc | 4 - 10 files changed, 393 insertions(+), 95 deletions(-) create mode 100644 src/Joystick/JoystickAndroid.cc create mode 100644 src/Joystick/JoystickAndroid.h create mode 100644 src/Joystick/JoystickSDL.cc diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 68a3370..5998e7f 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -266,6 +266,7 @@ HEADERS += \ src/HomePositionManager.h \ src/Joystick/Joystick.h \ src/Joystick/JoystickManager.h \ + src/VehicleSetup/JoystickConfigController.h \ src/FollowMe/FollowMe.h \ src/PositionManager/SimulatedPosition.h \ src/JsonHelper.h \ @@ -314,6 +315,11 @@ HEADERS += \ src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h \ src/PositionManager/PositionManager.h +AndroidBuild { +HEADERS += \ + src/Joystick/JoystickAndroid.h \ +} + DebugBuild { HEADERS += \ src/comm/MockLink.h \ @@ -346,6 +352,7 @@ HEADERS += \ src/comm/QGCHilLink.h \ src/comm/QGCJSBSimLink.h \ src/comm/QGCXPlaneLink.h \ + src/Joystick/JoystickSDL.h \ src/QGCFileDialog.h \ src/QGCMessageBox.h \ src/uas/FileManager.h \ @@ -390,7 +397,6 @@ HEADERS += \ src/GPS/GPSManager.h \ src/GPS/GPSPositionMessage.h \ src/GPS/GPSProvider.h \ - src/VehicleSetup/JoystickConfigController.h \ src/ViewWidgets/CustomCommandWidget.h \ src/ViewWidgets/CustomCommandWidgetController.h \ src/ViewWidgets/LogDownload.h \ @@ -403,8 +409,10 @@ iOSBuild { src/audio/QGCAudioWorker_iOS.mm \ src/MobileScreenMgr.mm \ } + AndroidBuild { SOURCES += src/MobileScreenMgr.cc \ + src/Joystick/JoystickAndroid.cc \ } @@ -423,6 +431,7 @@ SOURCES += \ src/HomePositionManager.cc \ src/Joystick/Joystick.cc \ src/Joystick/JoystickManager.cc \ + src/VehicleSetup/JoystickConfigController.cc \ src/JsonHelper.cc \ src/FollowMe/FollowMe.cc \ src/LogCompressor.cc \ @@ -501,6 +510,7 @@ SOURCES += \ src/comm/QGCFlightGearLink.cc \ src/comm/QGCJSBSimLink.cc \ src/comm/QGCXPlaneLink.cc \ + src/Joystick/JoystickSDL.cc \ src/ui/HILDockWidget.cc \ src/ui/linechart/ChartPlot.cc \ src/ui/linechart/IncrementalPlot.cc \ @@ -531,7 +541,6 @@ SOURCES += \ src/GPS/RTCM/RTCMMavlink.cc \ src/GPS/GPSManager.cc \ src/GPS/GPSProvider.cc \ - src/VehicleSetup/JoystickConfigController.cc \ src/ViewWidgets/CustomCommandWidget.cc \ src/ViewWidgets/CustomCommandWidgetController.cc \ src/ViewWidgets/LogDownload.cc \ diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 8aea5b9..a8bb2e9 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -28,14 +28,6 @@ #include -#ifndef __mobile__ - #ifdef Q_OS_MAC - #include - #else - #include - #endif -#endif - QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") @@ -51,9 +43,7 @@ const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { "ThrottleAxis" }; -Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlIndex, MultiVehicleManager* multiVehicleManager) -#ifndef __mobile__ - : _sdlIndex(sdlIndex) +Joystick::Joystick(const QString& name, int axisCount, int buttonCount, MultiVehicleManager* multiVehicleManager) , _exitThread(false) , _name(name) , _axisCount(axisCount) @@ -68,15 +58,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlI , _activeVehicle(NULL) , _pollingStartedForCalibration(false) , _multiVehicleManager(multiVehicleManager) -#endif // __mobile__ { -#ifdef __mobile__ - Q_UNUSED(name) - Q_UNUSED(axisCount) - Q_UNUSED(buttonCount) - Q_UNUSED(sdlIndex) - Q_UNUSED(multiVehicleManager) -#else _rgAxisValues = new int[_axisCount]; _rgCalibration = new Calibration_t[_axisCount]; _rgButtonValues = new bool[_buttonCount]; @@ -90,21 +72,16 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlI } _loadSettings(); -#endif // __mobile__ } Joystick::~Joystick() { -#ifndef __mobile__ delete _rgAxisValues; delete _rgCalibration; delete _rgButtonValues; delete _rgButtonActions; -#endif } -#ifndef __mobile__ - void Joystick::_loadSettings(void) { QSettings settings; @@ -257,19 +234,14 @@ float Joystick::_adjustRange(int value, Calibration_t calibration) void Joystick::run(void) { - SDL_Joystick* sdlJoystick = SDL_JoystickOpen(_sdlIndex); - - if (!sdlJoystick) { - qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError(); - return; - } + open(); while (!_exitThread) { - SDL_JoystickUpdate(); + update(); // Update axes for (int axisIndex=0; axisIndex<_axisCount; axisIndex++) { - int newAxisValue = SDL_JoystickGetAxis(sdlJoystick, axisIndex); + int newAxisValue = getAxis(axisIndex); // Calibration code requires signal to be emitted even if value hasn't changed _rgAxisValues[axisIndex] = newAxisValue; emit rawAxisValueChanged(axisIndex, newAxisValue); @@ -277,7 +249,7 @@ void Joystick::run(void) // Update buttons for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) { - bool newButtonValue = !!SDL_JoystickGetButton(sdlJoystick, buttonIndex); + bool newButtonValue = getButton(buttonIndex); if (newButtonValue != _rgButtonValues[buttonIndex]) { _rgButtonValues[buttonIndex] = newButtonValue; emit rawButtonPressedChanged(buttonIndex, newButtonValue); @@ -362,7 +334,7 @@ void Joystick::run(void) QGC::SLEEP::msleep(40); } - SDL_JoystickClose(sdlJoystick); + close(); } void Joystick::startPolling(Vehicle* vehicle) @@ -579,4 +551,3 @@ bool Joystick::_validButton(int button) return button >= 0 && button < _buttonCount; } -#endif // __mobile__ diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index c26752f..389a737 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -39,9 +39,9 @@ class Joystick : public QThread Q_OBJECT public: - Joystick(const QString& name, int axisCount, int buttonCount, int sdlIndex, MultiVehicleManager* multiVehicleManager); + Joystick(const QString& name, int axisCount, int buttonCount, MultiVehicleManager* multiVehicleManager); ~Joystick(); - + typedef struct { int min; int max; @@ -63,7 +63,6 @@ public: ThrottleModeMax } ThrottleMode_t; -#ifndef __mobile__ Q_PROPERTY(QString name READ name CONSTANT) Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) @@ -137,7 +136,7 @@ signals: void buttonActionTriggered(int action); -private: +protected: void _saveSettings(void); void _loadSettings(void); float _adjustRange(int value, Calibration_t calibration); @@ -145,11 +144,18 @@ private: bool _validAxis(int axis); bool _validButton(int button); +private: + virtual bool open() = 0; + virtual void close() = 0; + virtual bool update() = 0; + + virtual bool getButton(int i) = 0; + virtual int getAxis(int i) = 0; + // Override from QThread virtual void run(void); -private: - int _sdlIndex; ///< Index for SDL_JoystickOpen +protected: bool _exitThread; ///< true: signal thread to exit @@ -174,7 +180,6 @@ private: bool _pollingStartedForCalibration; MultiVehicleManager* _multiVehicleManager; -#endif // __mobile__ private: static const char* _rgFunctionSettingsKey[maxFunction]; diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc new file mode 100644 index 0000000..f9eef36 --- /dev/null +++ b/src/Joystick/JoystickAndroid.cc @@ -0,0 +1,220 @@ +#include "JoystickAndroid.h" + +#include "QGCApplication.h" + +#include + +int JoystickAndroid::_androidBtnListCount; +int *JoystickAndroid::_androidBtnList; +int JoystickAndroid::ACTION_DOWN; +int JoystickAndroid::ACTION_UP; +QMutex JoystickAndroid::m_mutex; + +JoystickAndroid::JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager) + : Joystick(name,axisCount,buttonCount,multiVehicleManager) + , deviceId(id) +{ + int i; + + QAndroidJniEnvironment env; + QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", id); + + //set button mapping (number->code) + jintArray b = env->NewIntArray(_androidBtnListCount); + env->SetIntArrayRegion(b,0,_androidBtnListCount,_androidBtnList); + + QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", b); + jbooleanArray jSupportedButtons = btns.object(); + jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr); + //create a mapping table (btnCode) that maps button number with button code + btnValue = new bool[_buttonCount]; + btnCode = new int[_buttonCount]; + int c = 0; + for (i=0;i<_androidBtnListCount;i++) + if (supportedButtons[i]) { + btnValue[c] = false; + btnCode[c] = _androidBtnList[i]; + c++; + } + + env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0); + + //set axis mapping (number->code) + axisValue = new int[_axisCount]; + axisCode = new int[_axisCount]; + QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;"); + for (i=0;i<_axisCount;i++) { + QAndroidJniObject range = rangeListNative.callObjectMethod("get", "(I)Ljava/lang/Object;",i); + axisCode[i] = range.callMethod("getAxis"); + axisValue[i] = 0; + } + + + qCDebug(JoystickLog) << "axis:" <<_axisCount << "buttons:" <<_buttonCount; + QtAndroidPrivate::registerGenericMotionEventListener(this); + QtAndroidPrivate::registerKeyEventListener(this); +} + +JoystickAndroid::~JoystickAndroid() { + delete btnCode; + delete axisCode; + delete btnValue; + delete axisValue; + + QtAndroidPrivate::unregisterGenericMotionEventListener(this); + QtAndroidPrivate::unregisterKeyEventListener(this); +} + +QMap JoystickAndroid::discover(MultiVehicleManager* _multiVehicleManager) { + bool joystickFound = false; + static QMap ret; + + _initStatic(); //it's enough to run it once, should be in a static constructor + + QMutexLocker lock(&m_mutex); + + QAndroidJniEnvironment env; + QAndroidJniObject o = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDeviceIds"); + jintArray jarr = o.object(); + int sz = env->GetArrayLength(jarr); + jint *buff = env->GetIntArrayElements(jarr, nullptr); + + int SOURCE_GAMEPAD = QAndroidJniObject::getStaticField("android/view/InputDevice", "SOURCE_GAMEPAD"); + int SOURCE_JOYSTICK = QAndroidJniObject::getStaticField("android/view/InputDevice", "SOURCE_JOYSTICK"); + + for (int i = 0; i < sz; ++i) { + QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", buff[i]); + int sources = inputDevice.callMethod("getSources", "()I"); + if (((sources & SOURCE_GAMEPAD) != SOURCE_GAMEPAD) //check if the input device is interesting to us + && ((sources & SOURCE_JOYSTICK) != SOURCE_JOYSTICK)) continue; + + //get id and name + QString id = inputDevice.callObjectMethod("getDescriptor", "()Ljava/lang/String;").toString(); + QString name = inputDevice.callObjectMethod("getName", "()Ljava/lang/String;").toString(); + + + if (joystickFound) { //skipping { + qWarning() << "Skipping joystick:" << name; + continue; + } + + //get number of axis + QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;"); + int axisCount = rangeListNative.callMethod("size"); + + //get number of buttons + jintArray a = env->NewIntArray(_androidBtnListCount); + env->SetIntArrayRegion(a,0,_androidBtnListCount,_androidBtnList); + QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", a); + jbooleanArray jSupportedButtons = btns.object(); + jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr); + int buttonCount = 0; + for (int j=0;j<_androidBtnListCount;j++) + if (supportedButtons[j]) buttonCount++; + env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0); + + qCDebug(JoystickLog) << "\t" << name << "id:" << buff[i] << "axes:" << axisCount << "buttons:" << buttonCount; + + ret[name] = new JoystickAndroid(name, axisCount, buttonCount, buff[i], _multiVehicleManager); + joystickFound = true; + } + + env->ReleaseIntArrayElements(jarr, buff, 0); + + + return ret; +} + + +bool JoystickAndroid::handleKeyEvent(jobject event) { + QJNIObjectPrivate ev(event); + QMutexLocker lock(&m_mutex); + const int _deviceId = ev.callMethod("getDeviceId", "()I"); + if (_deviceId!=deviceId) return false; + + const int action = ev.callMethod("getAction", "()I"); + const int keyCode = ev.callMethod("getKeyCode", "()I"); + + for (int i=0;i<_buttonCount;i++) { + if (btnCode[i]==keyCode) { + if (action==ACTION_DOWN) btnValue[i] = true; + if (action==ACTION_UP) btnValue[i] = false; + return true; + } + } + return false; +} + +bool JoystickAndroid::handleGenericMotionEvent(jobject event) { + QJNIObjectPrivate ev(event); + QMutexLocker lock(&m_mutex); + const int _deviceId = ev.callMethod("getDeviceId", "()I"); + if (_deviceId!=deviceId) return false; + + for (int i=0;i<_axisCount;i++) { + const float v = ev.callMethod("getAxisValue", "(I)F",axisCode[i]); + axisValue[i] = (int)(v*32767.f); + } + return true; +} + + +bool JoystickAndroid::open(void) { + return true; +} + +void JoystickAndroid::close(void) { +} + +bool JoystickAndroid::update(void) +{ + return true; +} + +bool JoystickAndroid::getButton(int i) { + return btnValue[ i ]; +} + +int JoystickAndroid::getAxis(int i) { + return axisValue[ i ]; +} + + +//helper method +void JoystickAndroid::_initStatic() { + //this gets list of all possible buttons - this is needed to check how many buttons our gamepad supports + //instead of the whole logic below we could have just a simple array of hardcoded int values as these 'should' not change + + //int JoystickAndroid::_androidBtnListCount; + _androidBtnListCount = 31; + static int ret[31]; //there are 31 buttons in total accordingy to the API + int i; + //int *JoystickAndroid:: + _androidBtnList = ret; + + for (i=1;i<=16;i++) { + QString name = "KEYCODE_BUTTON_"+QString::number(i); + ret[i-1] = QAndroidJniObject::getStaticField("android/view/KeyEvent", name.toStdString().c_str()); + } + i--; + + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_A"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_B"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_C"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_L1"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_L2"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_R1"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_R2"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_MODE"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_SELECT"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_START"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBL"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBR"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_X"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_Y"); + ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_Z"); + + ACTION_DOWN = QAndroidJniObject::getStaticField("android/view/KeyEvent", "ACTION_DOWN"); + ACTION_UP = QAndroidJniObject::getStaticField("android/view/KeyEvent", "ACTION_UP"); +} + diff --git a/src/Joystick/JoystickAndroid.h b/src/Joystick/JoystickAndroid.h new file mode 100644 index 0000000..75ea813 --- /dev/null +++ b/src/Joystick/JoystickAndroid.h @@ -0,0 +1,49 @@ +#ifndef JOYSTICKANDROID_H +#define JOYSTICKANDROID_H + +#include "Joystick.h" +#include "Vehicle.h" +#include "MultiVehicleManager.h" + +#include +#include +#include +#include +#include + + +class JoystickAndroid : public Joystick, public QtAndroidPrivate::GenericMotionEventListener, public QtAndroidPrivate::KeyEventListener +{ +public: + JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager); + ~JoystickAndroid(); + + static QMap discover(MultiVehicleManager* _multiVehicleManager); + +private: + bool handleKeyEvent(jobject event); + bool handleGenericMotionEvent(jobject event); + + virtual bool open(); + virtual void close(); + virtual bool update(); + + virtual bool getButton(int i); + virtual int getAxis(int i); + + int *btnCode; + int *axisCode; + bool *btnValue; + int *axisValue; + + static void _initStatic(); + static int * _androidBtnList; //list of all possible android buttons + static int _androidBtnListCount; + + static int ACTION_DOWN, ACTION_UP; + static QMutex m_mutex; + + int deviceId; +}; + +#endif // JOYSTICKANDROID_H diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc index 70defba..3828c30 100644 --- a/src/Joystick/JoystickManager.cc +++ b/src/Joystick/JoystickManager.cc @@ -27,11 +27,12 @@ #include #ifndef __mobile__ - #ifdef Q_OS_MAC - #include - #else - #include - #endif + #include "JoystickSDL.h" + #define __sdljoystick__ +#endif + +#ifdef __android__ + #include "JoystickAndroid.h" #endif QGC_LOGGING_CATEGORY(JoystickManagerLog, "JoystickManagerLog") @@ -44,7 +45,15 @@ JoystickManager::JoystickManager(QGCApplication* app) , _activeJoystick(NULL) , _multiVehicleManager(NULL) { - +} + +JoystickManager::~JoystickManager() { + QMap::iterator i; + for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) { + qDebug() << "Releasing joystick:" << i.key(); + delete i.value(); + } + qDebug() << "Done"; } void JoystickManager::setToolbox(QGCToolbox *toolbox) @@ -55,33 +64,10 @@ void JoystickManager::setToolbox(QGCToolbox *toolbox) QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); -#ifndef __mobile__ - 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()); -#endif } Joystick* JoystickManager::activeJoystick(void) @@ -117,9 +100,6 @@ Joystick* JoystickManager::activeJoystick(void) void JoystickManager::setActiveJoystick(Joystick* joystick) { -#ifdef __mobile__ - Q_UNUSED(joystick) -#else QSettings settings; if (!_name2JoystickMap.contains(joystick->name())) { @@ -138,7 +118,6 @@ void JoystickManager::setActiveJoystick(Joystick* joystick) emit activeJoystickChanged(_activeJoystick); emit activeJoystickNameChanged(_activeJoystick->name()); -#endif } QVariantList JoystickManager::joysticks(void) @@ -159,11 +138,7 @@ QStringList JoystickManager::joystickNames(void) QString JoystickManager::activeJoystickName(void) { -#ifdef __mobile__ - return QString(); -#else return _activeJoystick ? _activeJoystick->name() : QString(); -#endif } void JoystickManager::setActiveJoystickName(const QString& name) diff --git a/src/Joystick/JoystickManager.h b/src/Joystick/JoystickManager.h index 76eca34..8837cfd 100644 --- a/src/Joystick/JoystickManager.h +++ b/src/Joystick/JoystickManager.h @@ -41,6 +41,7 @@ class JoystickManager : public QGCTool public: JoystickManager(QGCApplication* app); + ~JoystickManager(); /// List of available joysticks Q_PROPERTY(QVariantList joysticks READ joysticks CONSTANT) diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc new file mode 100644 index 0000000..db90c11 --- /dev/null +++ b/src/Joystick/JoystickSDL.cc @@ -0,0 +1,73 @@ +#include "JoystickSDL.h" + +#include "QGCApplication.h" + +#include + +JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, int index, MultiVehicleManager* multiVehicleManager) + : Joystick(name,axisCount,buttonCount,multiVehicleManager) + , _index(index) +{ +} + +QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) { + static QMap ret; + + if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) { + qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError(); + return ret; + } + + // Load available joysticks + + qCDebug(JoystickLog) << "Available joysticks"; + + for (int i=0; i ("QGroundControl.Controllers", 1, 0, "ValuesWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "QGCMobileFileDialogController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "RCChannelMonitorController"); - + qmlRegisterType ("QGroundControl.Controllers", 1, 0, "JoystickConfigController"); #ifndef __mobile__ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ViewWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "FirmwareUpgradeController"); - qmlRegisterType ("QGroundControl.Controllers", 1, 0, "JoystickConfigController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "LogDownloadController"); #endif diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 5410b92..cc984bc 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1124,7 +1124,6 @@ void Vehicle::setJoystickEnabled(bool enabled) void Vehicle::_startJoystick(bool start) { -#ifndef __mobile__ Joystick* joystick = _joystickManager->activeJoystick(); if (joystick) { if (start) { @@ -1135,9 +1134,6 @@ void Vehicle::_startJoystick(bool start) joystick->stopPolling(); } } -#else - Q_UNUSED(start); -#endif } bool Vehicle::active(void) From b025bec98b7f62d57a5a1b7e0aecf9d886df6e81 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Mon, 2 May 2016 00:10:45 +0100 Subject: [PATCH 02/18] Fixing syntax --- src/Joystick/Joystick.cc | 2 +- src/Joystick/JoystickAndroid.cc | 1 + src/Joystick/JoystickSDL.h | 34 ++++++++++++++++++++++++++++++++++ 3 files changed, 36 insertions(+), 1 deletion(-) create mode 100644 src/Joystick/JoystickSDL.h diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index a8bb2e9..0e7cf15 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -44,7 +44,7 @@ const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { }; Joystick::Joystick(const QString& name, int axisCount, int buttonCount, MultiVehicleManager* multiVehicleManager) - , _exitThread(false) + : _exitThread(false) , _name(name) , _axisCount(axisCount) , _buttonCount(buttonCount) diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc index f9eef36..777c0c3 100644 --- a/src/Joystick/JoystickAndroid.cc +++ b/src/Joystick/JoystickAndroid.cc @@ -139,6 +139,7 @@ bool JoystickAndroid::handleKeyEvent(jobject event) { if (btnCode[i]==keyCode) { if (action==ACTION_DOWN) btnValue[i] = true; if (action==ACTION_UP) btnValue[i] = false; + qWarning() << "Btn:"< +#else + #include +#endif + + +class JoystickSDL : public Joystick +{ +public: + JoystickSDL(const QString& name, int axisCount, int buttonCount, int index, MultiVehicleManager* multiVehicleManager); + + static QMap discover(MultiVehicleManager* _multiVehicleManager); + +private: + virtual bool open(); + virtual void close(); + virtual bool update(); + + virtual bool getButton(int i); + virtual int getAxis(int i); + + SDL_Joystick *sdlJoystick; + int _index; ///< Index for SDL_JoystickOpen +}; + +#endif // JOYSTICKSDL_H From cd7a000c40bd5c45ccf741fce2c54bcae21e6700 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Sun, 1 May 2016 23:38:55 +0100 Subject: [PATCH 03/18] Android Joystick support --- src/Joystick/Joystick.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 0e7cf15..a8bb2e9 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -44,7 +44,7 @@ const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { }; Joystick::Joystick(const QString& name, int axisCount, int buttonCount, MultiVehicleManager* multiVehicleManager) - : _exitThread(false) + , _exitThread(false) , _name(name) , _axisCount(axisCount) , _buttonCount(buttonCount) From 69ab420efcfe4554fc8f188ea31d2fe4050504bb Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Mon, 2 May 2016 00:10:45 +0100 Subject: [PATCH 04/18] Fixing syntax --- src/Joystick/Joystick.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index a8bb2e9..0e7cf15 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -44,7 +44,7 @@ const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { }; Joystick::Joystick(const QString& name, int axisCount, int buttonCount, MultiVehicleManager* multiVehicleManager) - , _exitThread(false) + : _exitThread(false) , _name(name) , _axisCount(axisCount) , _buttonCount(buttonCount) From b7c0259ee9fa4cfb8b9c475a360a4338aceb3ba0 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Sun, 1 May 2016 23:38:55 +0100 Subject: [PATCH 05/18] Android Joystick support --- src/Joystick/Joystick.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 0e7cf15..a8bb2e9 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -44,7 +44,7 @@ const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { }; Joystick::Joystick(const QString& name, int axisCount, int buttonCount, MultiVehicleManager* multiVehicleManager) - : _exitThread(false) + , _exitThread(false) , _name(name) , _axisCount(axisCount) , _buttonCount(buttonCount) From 40fee7d15038b609fad37d185b5ed1c0299299e9 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Mon, 2 May 2016 00:10:45 +0100 Subject: [PATCH 06/18] Fixing syntax --- src/Joystick/Joystick.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index a8bb2e9..0e7cf15 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -44,7 +44,7 @@ const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { }; Joystick::Joystick(const QString& name, int axisCount, int buttonCount, MultiVehicleManager* multiVehicleManager) - , _exitThread(false) + : _exitThread(false) , _name(name) , _axisCount(axisCount) , _buttonCount(buttonCount) From ac4ad3b776f4049bb8818c1d9ec4629d47567162 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Sun, 22 May 2016 12:21:15 +0100 Subject: [PATCH 07/18] Sync with official --- libs/mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/mavlink/include/mavlink/v1.0 b/libs/mavlink/include/mavlink/v1.0 index 9605f80..39c14e8 160000 --- a/libs/mavlink/include/mavlink/v1.0 +++ b/libs/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 9605f8087d3734ed89fa0aba6f343ced041fb3cb +Subproject commit 39c14e81edf2eac7b5bfe6771eb56614f46ef098 From 9d0b229519c6a34ea1f79d59ac33eec769dd13e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 12 May 2016 13:01:06 +0200 Subject: [PATCH 08/18] GPSProvider: add SIMULATE_RTCM_OUTPUT to enable simulated RTCM output (currently disabled) --- src/GPS/GPSProvider.cc | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/GPS/GPSProvider.cc b/src/GPS/GPSProvider.cc index 4412194..a991ad6 100644 --- a/src/GPS/GPSProvider.cc +++ b/src/GPS/GPSProvider.cc @@ -18,9 +18,25 @@ #include "Drivers/src/gps_helper.h" #include "definitions.h" +//#define SIMULATE_RTCM_OUTPUT //if defined, generate simulated RTCM messages + //additionally make sure to call connectGPS(""), eg. from QGCToolbox.cc + void GPSProvider::run() { +#ifdef SIMULATE_RTCM_OUTPUT + const int fakeMsgLengths[3] = { 30, 170, 240 }; + uint8_t* fakeData = new uint8_t[fakeMsgLengths[2]]; + while (!_requestStop) { + for (int i = 0; i < 3; ++i) { + gotRTCMData((uint8_t*) fakeData, fakeMsgLengths[i]); + msleep(4); + } + msleep(100); + } + delete[] fakeData; +#endif /* SIMULATE_RTCM_OUTPUT */ + if (_serial) delete _serial; _serial = new QSerialPort(); From 68e23bdc3bde3baf5e0fc029a0528d6983142969 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 12 May 2016 13:02:57 +0200 Subject: [PATCH 09/18] gps: use an inline function for usleep instead of #define This avoids conflicts with other code --- src/GPS/definitions.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h index b28351e..6845d3a 100644 --- a/src/GPS/definitions.h +++ b/src/GPS/definitions.h @@ -59,7 +59,9 @@ public: static void usleep(unsigned long usecs) { QThread::usleep(usecs); } }; -#define usleep Sleeper::usleep +static inline void usleep(unsigned long usecs) { + Sleeper::usleep(usecs); +} typedef uint64_t gps_abstime; From 3a71177db160003dbbbce497d351cd9f2b2eb1e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 12 May 2016 13:03:52 +0200 Subject: [PATCH 10/18] gps: update submodule --- src/GPS/Drivers | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/GPS/Drivers b/src/GPS/Drivers index 60739aa..d341ff5 160000 --- a/src/GPS/Drivers +++ b/src/GPS/Drivers @@ -1 +1 @@ -Subproject commit 60739aaace1723c700f23b22212696dc75169559 +Subproject commit d341ff59fafd51ebedd24337fceb8f631c4b0cd3 From 36abc43fc85a3b0413bf5feec49da621c0e5b13a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 24 May 2016 09:58:41 +0200 Subject: [PATCH 11/18] gps: use GPS_RTCM_DATA mavlink message for RTCM, use fragmentation if needed --- src/GPS/RTCM/RTCMMavlink.cc | 38 +++++++++++++++++++++++++++----------- src/GPS/RTCM/RTCMMavlink.h | 3 +++ 2 files changed, 30 insertions(+), 11 deletions(-) diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index 87b0d23..3b570d5 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -11,7 +11,6 @@ #include "RTCMMavlink.h" #include "MultiVehicleManager.h" -#include "MAVLinkProtocol.h" #include "Vehicle.h" #include @@ -24,8 +23,6 @@ RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) void RTCMMavlink::RTCMDataUpdate(QByteArray message) { - Q_ASSERT(message.size() <= 110); //mavlink message uses a fixed-size buffer - /* statistics */ _bandwidthByteCounter += message.size(); qint64 elapsed = _bandwidthTimer.elapsed(); @@ -34,19 +31,38 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message) _bandwidthTimer.restart(); _bandwidthByteCounter = 0; } - QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles(); - MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol(); - mavlink_gps_inject_data_t mavlinkRtcmData; - memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_inject_data_t)); - mavlinkRtcmData.len = message.size(); - memcpy(&mavlinkRtcmData.data, message.data(), message.size()); + const int maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN; + mavlink_gps_rtcm_data_t mavlinkRtcmData; + memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t)); + if (message.size() < maxMessageLength) { + mavlinkRtcmData.len = message.size(); + memcpy(&mavlinkRtcmData.data, message.data(), message.size()); + sendMessageToVehicle(mavlinkRtcmData); + } else { + //we need to fragment + int start = 0; + while (start < message.size()) { + int length = std::min(message.size() - start, maxMessageLength); + mavlinkRtcmData.flags = 1; //fragmented + mavlinkRtcmData.len = length; + memcpy(&mavlinkRtcmData.data, message.data() + start, length); + sendMessageToVehicle(mavlinkRtcmData); + start += length; + } + } +} + +void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg) +{ + QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles(); + MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol(); for (int i = 0; i < vehicles.count(); i++) { Vehicle* vehicle = qobject_cast(vehicles[i]); mavlink_message_t message; - mavlink_msg_gps_inject_data_encode(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), &message, &mavlinkRtcmData); + mavlink_msg_gps_rtcm_data_encode(mavlinkProtocol->getSystemId(), + mavlinkProtocol->getComponentId(), &message, &msg); vehicle->sendMessage(message); } } diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h index be780f4..6c59974 100644 --- a/src/GPS/RTCM/RTCMMavlink.h +++ b/src/GPS/RTCM/RTCMMavlink.h @@ -14,6 +14,7 @@ #include #include "QGCToolbox.h" +#include "MAVLinkProtocol.h" /** ** class RTCMMavlink @@ -30,6 +31,8 @@ public slots: void RTCMDataUpdate(QByteArray message); private: + void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg); + QGCToolbox& _toolbox; QElapsedTimer _bandwidthTimer; int _bandwidthByteCounter = 0; From 3d2725baefd21d60508e3c6ffd6c2857712d7a6f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 31 May 2016 20:06:30 +0200 Subject: [PATCH 12/18] gps read callback: avoid sleep, don't wait when data already available --- src/GPS/GPSProvider.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/GPS/GPSProvider.cc b/src/GPS/GPSProvider.cc index a991ad6..f8c836b 100644 --- a/src/GPS/GPSProvider.cc +++ b/src/GPS/GPSProvider.cc @@ -141,10 +141,11 @@ int GPSProvider::callback(GPSCallbackType type, void *data1, int data2) { switch (type) { case GPSCallbackType::readDeviceData: { - int timeout = *((int *) data1); - if (!_serial->waitForReadyRead(timeout)) - return 0; //timeout - msleep(10); //give some more time to buffer data + if (_serial->bytesAvailable() == 0) { + int timeout = *((int *) data1); + if (!_serial->waitForReadyRead(timeout)) + return 0; //timeout + } return (int)_serial->read((char*) data1, data2); } case GPSCallbackType::writeDeviceData: From e7efd8ac42940ca28a419ded345283bcf671186f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 1 Jun 2016 11:22:31 +0200 Subject: [PATCH 13/18] gps: update drivers submodule --- src/GPS/Drivers | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/GPS/Drivers b/src/GPS/Drivers index d341ff5..5c1ae95 160000 --- a/src/GPS/Drivers +++ b/src/GPS/Drivers @@ -1 +1 @@ -Subproject commit d341ff59fafd51ebedd24337fceb8f631c4b0cd3 +Subproject commit 5c1ae956552c3887c5fddd303a8f242efa715333 From 28c85e76588e4df93ed2c91532c67d9b101341cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 1 Jun 2016 11:23:49 +0200 Subject: [PATCH 14/18] gps: use a read buffer size of 1024 --- src/GPS/definitions.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h index 6845d3a..ca655d0 100644 --- a/src/GPS/definitions.h +++ b/src/GPS/definitions.h @@ -41,6 +41,8 @@ #include +#define GPS_READ_BUFFER_SIZE 1024 + #define GPS_INFO(...) qInfo(__VA_ARGS__) #define GPS_WARN(...) qWarning(__VA_ARGS__) #define GPS_ERR(...) qCritical(__VA_ARGS__) From d491300a7d53b9d25b02de234d4164c531853428 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 1 Jun 2016 11:24:38 +0200 Subject: [PATCH 15/18] GPSFact.json: add RTK float & fixed types --- src/Vehicle/GPSFact.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Vehicle/GPSFact.json b/src/Vehicle/GPSFact.json index 73046fb..826ae1d 100644 --- a/src/Vehicle/GPSFact.json +++ b/src/Vehicle/GPSFact.json @@ -25,8 +25,8 @@ "name": "lock", "shortDescription": "GPS Lock", "type": "uint32", - "enumStrings": "None,None,2D Lock,3D Lock,3D DGPS Lock,3D RTK GPS Lock", - "enumValues": "0,1,2,3,4,5", + "enumStrings": "None,None,2D Lock,3D Lock,3D DGPS Lock,3D RTK GPS Lock (float),3D RTK GPS Lock (fixed)", + "enumValues": "0,1,2,3,4,5,6", "decimalPlaces": 0 }, { From 907520b00d340bdbd9dacb859f60ac6ec6b17359 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Wed, 1 Jun 2016 21:58:18 +0100 Subject: [PATCH 16/18] Removing AndroidJoystick (requires Qt5.6) --- qgroundcontrol.pro | 2 - src/Joystick/JoystickAndroid.cc | 221 ---------------------------------------- src/Joystick/JoystickAndroid.h | 49 --------- src/Joystick/JoystickManager.cc | 10 +- 4 files changed, 8 insertions(+), 274 deletions(-) delete mode 100644 src/Joystick/JoystickAndroid.cc delete mode 100644 src/Joystick/JoystickAndroid.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0661c96..bb9a53c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -306,7 +306,6 @@ HEADERS += \ AndroidBuild { HEADERS += \ - src/Joystick/JoystickAndroid.h \ } DebugBuild { @@ -401,7 +400,6 @@ iOSBuild { AndroidBuild { SOURCES += src/MobileScreenMgr.cc \ - src/Joystick/JoystickAndroid.cc \ } diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc deleted file mode 100644 index 777c0c3..0000000 --- a/src/Joystick/JoystickAndroid.cc +++ /dev/null @@ -1,221 +0,0 @@ -#include "JoystickAndroid.h" - -#include "QGCApplication.h" - -#include - -int JoystickAndroid::_androidBtnListCount; -int *JoystickAndroid::_androidBtnList; -int JoystickAndroid::ACTION_DOWN; -int JoystickAndroid::ACTION_UP; -QMutex JoystickAndroid::m_mutex; - -JoystickAndroid::JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager) - : Joystick(name,axisCount,buttonCount,multiVehicleManager) - , deviceId(id) -{ - int i; - - QAndroidJniEnvironment env; - QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", id); - - //set button mapping (number->code) - jintArray b = env->NewIntArray(_androidBtnListCount); - env->SetIntArrayRegion(b,0,_androidBtnListCount,_androidBtnList); - - QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", b); - jbooleanArray jSupportedButtons = btns.object(); - jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr); - //create a mapping table (btnCode) that maps button number with button code - btnValue = new bool[_buttonCount]; - btnCode = new int[_buttonCount]; - int c = 0; - for (i=0;i<_androidBtnListCount;i++) - if (supportedButtons[i]) { - btnValue[c] = false; - btnCode[c] = _androidBtnList[i]; - c++; - } - - env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0); - - //set axis mapping (number->code) - axisValue = new int[_axisCount]; - axisCode = new int[_axisCount]; - QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;"); - for (i=0;i<_axisCount;i++) { - QAndroidJniObject range = rangeListNative.callObjectMethod("get", "(I)Ljava/lang/Object;",i); - axisCode[i] = range.callMethod("getAxis"); - axisValue[i] = 0; - } - - - qCDebug(JoystickLog) << "axis:" <<_axisCount << "buttons:" <<_buttonCount; - QtAndroidPrivate::registerGenericMotionEventListener(this); - QtAndroidPrivate::registerKeyEventListener(this); -} - -JoystickAndroid::~JoystickAndroid() { - delete btnCode; - delete axisCode; - delete btnValue; - delete axisValue; - - QtAndroidPrivate::unregisterGenericMotionEventListener(this); - QtAndroidPrivate::unregisterKeyEventListener(this); -} - -QMap JoystickAndroid::discover(MultiVehicleManager* _multiVehicleManager) { - bool joystickFound = false; - static QMap ret; - - _initStatic(); //it's enough to run it once, should be in a static constructor - - QMutexLocker lock(&m_mutex); - - QAndroidJniEnvironment env; - QAndroidJniObject o = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDeviceIds"); - jintArray jarr = o.object(); - int sz = env->GetArrayLength(jarr); - jint *buff = env->GetIntArrayElements(jarr, nullptr); - - int SOURCE_GAMEPAD = QAndroidJniObject::getStaticField("android/view/InputDevice", "SOURCE_GAMEPAD"); - int SOURCE_JOYSTICK = QAndroidJniObject::getStaticField("android/view/InputDevice", "SOURCE_JOYSTICK"); - - for (int i = 0; i < sz; ++i) { - QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", buff[i]); - int sources = inputDevice.callMethod("getSources", "()I"); - if (((sources & SOURCE_GAMEPAD) != SOURCE_GAMEPAD) //check if the input device is interesting to us - && ((sources & SOURCE_JOYSTICK) != SOURCE_JOYSTICK)) continue; - - //get id and name - QString id = inputDevice.callObjectMethod("getDescriptor", "()Ljava/lang/String;").toString(); - QString name = inputDevice.callObjectMethod("getName", "()Ljava/lang/String;").toString(); - - - if (joystickFound) { //skipping { - qWarning() << "Skipping joystick:" << name; - continue; - } - - //get number of axis - QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;"); - int axisCount = rangeListNative.callMethod("size"); - - //get number of buttons - jintArray a = env->NewIntArray(_androidBtnListCount); - env->SetIntArrayRegion(a,0,_androidBtnListCount,_androidBtnList); - QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", a); - jbooleanArray jSupportedButtons = btns.object(); - jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr); - int buttonCount = 0; - for (int j=0;j<_androidBtnListCount;j++) - if (supportedButtons[j]) buttonCount++; - env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0); - - qCDebug(JoystickLog) << "\t" << name << "id:" << buff[i] << "axes:" << axisCount << "buttons:" << buttonCount; - - ret[name] = new JoystickAndroid(name, axisCount, buttonCount, buff[i], _multiVehicleManager); - joystickFound = true; - } - - env->ReleaseIntArrayElements(jarr, buff, 0); - - - return ret; -} - - -bool JoystickAndroid::handleKeyEvent(jobject event) { - QJNIObjectPrivate ev(event); - QMutexLocker lock(&m_mutex); - const int _deviceId = ev.callMethod("getDeviceId", "()I"); - if (_deviceId!=deviceId) return false; - - const int action = ev.callMethod("getAction", "()I"); - const int keyCode = ev.callMethod("getKeyCode", "()I"); - - for (int i=0;i<_buttonCount;i++) { - if (btnCode[i]==keyCode) { - if (action==ACTION_DOWN) btnValue[i] = true; - if (action==ACTION_UP) btnValue[i] = false; - qWarning() << "Btn:"<("getDeviceId", "()I"); - if (_deviceId!=deviceId) return false; - - for (int i=0;i<_axisCount;i++) { - const float v = ev.callMethod("getAxisValue", "(I)F",axisCode[i]); - axisValue[i] = (int)(v*32767.f); - } - return true; -} - - -bool JoystickAndroid::open(void) { - return true; -} - -void JoystickAndroid::close(void) { -} - -bool JoystickAndroid::update(void) -{ - return true; -} - -bool JoystickAndroid::getButton(int i) { - return btnValue[ i ]; -} - -int JoystickAndroid::getAxis(int i) { - return axisValue[ i ]; -} - - -//helper method -void JoystickAndroid::_initStatic() { - //this gets list of all possible buttons - this is needed to check how many buttons our gamepad supports - //instead of the whole logic below we could have just a simple array of hardcoded int values as these 'should' not change - - //int JoystickAndroid::_androidBtnListCount; - _androidBtnListCount = 31; - static int ret[31]; //there are 31 buttons in total accordingy to the API - int i; - //int *JoystickAndroid:: - _androidBtnList = ret; - - for (i=1;i<=16;i++) { - QString name = "KEYCODE_BUTTON_"+QString::number(i); - ret[i-1] = QAndroidJniObject::getStaticField("android/view/KeyEvent", name.toStdString().c_str()); - } - i--; - - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_A"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_B"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_C"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_L1"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_L2"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_R1"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_R2"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_MODE"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_SELECT"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_START"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBL"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBR"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_X"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_Y"); - ret[i++] = QAndroidJniObject::getStaticField("android/view/KeyEvent", "KEYCODE_BUTTON_Z"); - - ACTION_DOWN = QAndroidJniObject::getStaticField("android/view/KeyEvent", "ACTION_DOWN"); - ACTION_UP = QAndroidJniObject::getStaticField("android/view/KeyEvent", "ACTION_UP"); -} - diff --git a/src/Joystick/JoystickAndroid.h b/src/Joystick/JoystickAndroid.h deleted file mode 100644 index 75ea813..0000000 --- a/src/Joystick/JoystickAndroid.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef JOYSTICKANDROID_H -#define JOYSTICKANDROID_H - -#include "Joystick.h" -#include "Vehicle.h" -#include "MultiVehicleManager.h" - -#include -#include -#include -#include -#include - - -class JoystickAndroid : public Joystick, public QtAndroidPrivate::GenericMotionEventListener, public QtAndroidPrivate::KeyEventListener -{ -public: - JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager); - ~JoystickAndroid(); - - static QMap discover(MultiVehicleManager* _multiVehicleManager); - -private: - bool handleKeyEvent(jobject event); - bool handleGenericMotionEvent(jobject event); - - virtual bool open(); - virtual void close(); - virtual bool update(); - - virtual bool getButton(int i); - virtual int getAxis(int i); - - int *btnCode; - int *axisCode; - bool *btnValue; - int *axisValue; - - static void _initStatic(); - static int * _androidBtnList; //list of all possible android buttons - static int _androidBtnListCount; - - static int ACTION_DOWN, ACTION_UP; - static QMutex m_mutex; - - int deviceId; -}; - -#endif // JOYSTICKANDROID_H diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc index e6e5c3d..1ef0fc6 100644 --- a/src/Joystick/JoystickManager.cc +++ b/src/Joystick/JoystickManager.cc @@ -19,7 +19,10 @@ #endif #ifdef __android__ - #include "JoystickAndroid.h" + /* + * Android Joystick not yet supported + * #include "JoystickAndroid.h" + */ #endif QGC_LOGGING_CATEGORY(JoystickManagerLog, "JoystickManagerLog") @@ -54,7 +57,10 @@ void JoystickManager::setToolbox(QGCToolbox *toolbox) #ifdef __sdljoystick__ _name2JoystickMap = JoystickSDL::discover(_multiVehicleManager); #elif defined(__android__) - _name2JoystickMap = JoystickAndroid::discover(_multiVehicleManager); + /* + * Android Joystick not yet supported + * _name2JoystickMap = JoystickAndroid::discover(_multiVehicleManager); + */ #endif if (!_name2JoystickMap.count()) { From 6e33df1aec5fa4f65dfc38820c8bfc679ed04507 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Wed, 1 Jun 2016 22:34:27 +0100 Subject: [PATCH 17/18] Updating mavlink v1.0 ref --- libs/mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/mavlink/include/mavlink/v1.0 b/libs/mavlink/include/mavlink/v1.0 index 39c14e8..9605f80 160000 --- a/libs/mavlink/include/mavlink/v1.0 +++ b/libs/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 39c14e81edf2eac7b5bfe6771eb56614f46ef098 +Subproject commit 9605f8087d3734ed89fa0aba6f343ced041fb3cb From 92422bd5621fb748ddbb91a6f6bf81e5ed0ab778 Mon Sep 17 00:00:00 2001 From: Gregory Dymarek Date: Wed, 1 Jun 2016 23:10:10 +0100 Subject: [PATCH 18/18] Improving coding style --- src/Joystick/Joystick.cc | 10 +++++----- src/Joystick/Joystick.h | 10 +++++----- src/Joystick/JoystickSDL.cc | 14 +++++++------- src/Joystick/JoystickSDL.h | 10 +++++----- 4 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index da1141e..9492dbe 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -221,14 +221,14 @@ float Joystick::_adjustRange(int value, Calibration_t calibration) void Joystick::run(void) { - open(); + _open(); while (!_exitThread) { - update(); + _update(); // Update axes for (int axisIndex=0; axisIndex<_axisCount; axisIndex++) { - int newAxisValue = getAxis(axisIndex); + int newAxisValue = _getAxis(axisIndex); // Calibration code requires signal to be emitted even if value hasn't changed _rgAxisValues[axisIndex] = newAxisValue; emit rawAxisValueChanged(axisIndex, newAxisValue); @@ -236,7 +236,7 @@ void Joystick::run(void) // Update buttons for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) { - bool newButtonValue = getButton(buttonIndex); + bool newButtonValue = _getButton(buttonIndex); if (newButtonValue != _rgButtonValues[buttonIndex]) { _rgButtonValues[buttonIndex] = newButtonValue; emit rawButtonPressedChanged(buttonIndex, newButtonValue); @@ -321,7 +321,7 @@ void Joystick::run(void) QGC::SLEEP::msleep(40); } - close(); + _close(); } void Joystick::startPolling(Vehicle* vehicle) diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index c6ab4ef..c4a1008 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -132,12 +132,12 @@ protected: bool _validButton(int button); private: - virtual bool open() = 0; - virtual void close() = 0; - virtual bool update() = 0; + virtual bool _open() = 0; + virtual void _close() = 0; + virtual bool _update() = 0; - virtual bool getButton(int i) = 0; - virtual int getAxis(int i) = 0; + virtual bool _getButton(int i) = 0; + virtual int _getAxis(int i) = 0; // Override from QThread virtual void run(void); diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index db90c11..14bdd0c 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -42,32 +42,32 @@ QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicl return ret; } -bool JoystickSDL::open(void) { +bool JoystickSDL::_open(void) { sdlJoystick = SDL_JoystickOpen(_index); if (!sdlJoystick) { - qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError(); - return false; + qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError(); + return false; } return true; } -void JoystickSDL::close(void) { +void JoystickSDL::_close(void) { SDL_JoystickClose(sdlJoystick); } -bool JoystickSDL::update(void) +bool JoystickSDL::_update(void) { SDL_JoystickUpdate(); return true; } -bool JoystickSDL::getButton(int i) { +bool JoystickSDL::_getButton(int i) { return !!SDL_JoystickGetButton(sdlJoystick, i); } -int JoystickSDL::getAxis(int i) { +int JoystickSDL::_getAxis(int i) { return SDL_JoystickGetAxis(sdlJoystick, i); } diff --git a/src/Joystick/JoystickSDL.h b/src/Joystick/JoystickSDL.h index d5e1a97..cea118d 100644 --- a/src/Joystick/JoystickSDL.h +++ b/src/Joystick/JoystickSDL.h @@ -20,12 +20,12 @@ public: static QMap discover(MultiVehicleManager* _multiVehicleManager); private: - virtual bool open(); - virtual void close(); - virtual bool update(); + bool _open() final; + void _close() final; + bool _update() final; - virtual bool getButton(int i); - virtual int getAxis(int i); + bool _getButton(int i) final; + int _getAxis(int i) final; SDL_Joystick *sdlJoystick; int _index; ///< Index for SDL_JoystickOpen