parent
4083b4a6f7
commit
3b0f1f8718
@ -0,0 +1,40 @@
|
||||
|
||||
set(EXTRA_SRC)
|
||||
|
||||
if (ANDROID)
|
||||
list(APPEND EXTRA_SRC
|
||||
JoystickAndroid.cc
|
||||
)
|
||||
endif()
|
||||
|
||||
add_library(Joystick
|
||||
Joystick.cc
|
||||
JoystickManager.cc
|
||||
JoystickSDL.cc
|
||||
JoystickMavCommand.cc
|
||||
${EXTRA_SRC}
|
||||
)
|
||||
|
||||
target_link_libraries(Joystick
|
||||
PRIVATE
|
||||
ui
|
||||
PUBLIC
|
||||
qgc
|
||||
ui
|
||||
)
|
||||
|
||||
target_include_directories(Joystick PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
if(WIN32)
|
||||
target_link_libraries(Joystick PUBLIC sdl2)
|
||||
else()
|
||||
find_package(SDL2 REQUIRED)
|
||||
if (IS_DIRECTORY "${SDL2_INCLUDE_DIRS}")
|
||||
include_directories(${SDL2_INCLUDE_DIRS})
|
||||
string(STRIP "${SDL2_LIBRARIES}" SDL2_LIBRARIES)
|
||||
target_link_libraries(Joystick PRIVATE ${SDL2_LIBRARIES})
|
||||
else()
|
||||
include_directories(${SDL2_DIR})
|
||||
target_link_libraries(Joystick PRIVATE SDL2::SDL2)
|
||||
endif()
|
||||
endif()
|
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,370 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file
|
||||
/// @brief Joystick Controller
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QObject>
|
||||
#include <QThread>
|
||||
#include <atomic>
|
||||
|
||||
#include "QGCLoggingCategory.h"
|
||||
#include "Vehicle.h"
|
||||
#include "MultiVehicleManager.h"
|
||||
#include "JoystickMavCommand.h"
|
||||
|
||||
// JoystickLog Category declaration moved to QGCLoggingCategory.cc to allow access in Vehicle
|
||||
Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog)
|
||||
Q_DECLARE_METATYPE(GRIPPER_ACTIONS)
|
||||
|
||||
/// Action assigned to button
|
||||
class AssignedButtonAction : public QObject {
|
||||
Q_OBJECT
|
||||
public:
|
||||
AssignedButtonAction(QObject* parent, const QString name);
|
||||
QString action;
|
||||
QElapsedTimer buttonTime;
|
||||
bool repeat = false;
|
||||
};
|
||||
|
||||
/// Assignable Button Action
|
||||
class AssignableButtonAction : public QObject {
|
||||
Q_OBJECT
|
||||
public:
|
||||
AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_ = false);
|
||||
Q_PROPERTY(QString action READ action CONSTANT)
|
||||
Q_PROPERTY(bool canRepeat READ canRepeat CONSTANT)
|
||||
QString action () { return _action; }
|
||||
bool canRepeat () const{ return _repeat; }
|
||||
private:
|
||||
QString _action;
|
||||
bool _repeat = false;
|
||||
};
|
||||
|
||||
/// Joystick Controller
|
||||
class Joystick : public QThread
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager);
|
||||
|
||||
virtual ~Joystick();
|
||||
|
||||
typedef struct Calibration_t {
|
||||
int min;
|
||||
int max;
|
||||
int center;
|
||||
int deadband;
|
||||
bool reversed;
|
||||
Calibration_t()
|
||||
: min(-32767)
|
||||
, max(32767)
|
||||
, center(0)
|
||||
, deadband(0)
|
||||
, reversed(false) {}
|
||||
} Calibration_t;
|
||||
|
||||
typedef enum {
|
||||
rollFunction,
|
||||
pitchFunction,
|
||||
yawFunction,
|
||||
throttleFunction,
|
||||
gimbalPitchFunction,
|
||||
gimbalYawFunction,
|
||||
maxFunction
|
||||
} AxisFunction_t;
|
||||
|
||||
typedef enum {
|
||||
ThrottleModeCenterZero,
|
||||
ThrottleModeDownZero,
|
||||
ThrottleModeMax
|
||||
} ThrottleMode_t;
|
||||
|
||||
Q_PROPERTY(QString name READ name CONSTANT)
|
||||
Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged)
|
||||
Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT)
|
||||
Q_PROPERTY(int axisCount READ axisCount CONSTANT)
|
||||
Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT)
|
||||
|
||||
//-- Actions assigned to buttons
|
||||
Q_PROPERTY(QStringList buttonActions READ buttonActions NOTIFY buttonActionsChanged)
|
||||
|
||||
//-- Actions that can be assigned to buttons
|
||||
Q_PROPERTY(QmlObjectListModel* assignableActions READ assignableActions NOTIFY assignableActionsChanged)
|
||||
Q_PROPERTY(QStringList assignableActionTitles READ assignableActionTitles NOTIFY assignableActionsChanged)
|
||||
Q_PROPERTY(QString disabledActionName READ disabledActionName CONSTANT)
|
||||
|
||||
Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged)
|
||||
Q_PROPERTY(float axisFrequencyHz READ axisFrequencyHz WRITE setAxisFrequency NOTIFY axisFrequencyHzChanged)
|
||||
Q_PROPERTY(float minAxisFrequencyHz MEMBER _minAxisFrequencyHz CONSTANT)
|
||||
Q_PROPERTY(float maxAxisFrequencyHz MEMBER _minAxisFrequencyHz CONSTANT)
|
||||
Q_PROPERTY(float buttonFrequencyHz READ buttonFrequencyHz WRITE setButtonFrequency NOTIFY buttonFrequencyHzChanged)
|
||||
Q_PROPERTY(float minButtonFrequencyHz MEMBER _minButtonFrequencyHz CONSTANT)
|
||||
Q_PROPERTY(float maxButtonFrequencyHz MEMBER _maxButtonFrequencyHz CONSTANT)
|
||||
Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged)
|
||||
Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
|
||||
Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
|
||||
Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged)
|
||||
|
||||
Q_INVOKABLE void setButtonRepeat (int button, bool repeat);
|
||||
Q_INVOKABLE bool getButtonRepeat (int button);
|
||||
Q_INVOKABLE void setButtonAction (int button, const QString& action);
|
||||
Q_INVOKABLE QString getButtonAction (int button);
|
||||
|
||||
// Property accessors
|
||||
|
||||
QString name () { return _name; }
|
||||
int totalButtonCount () const{ return _totalButtonCount; }
|
||||
int axisCount () const{ return _axisCount; }
|
||||
QStringList buttonActions ();
|
||||
|
||||
QmlObjectListModel* assignableActions () { return &_assignableButtonActions; }
|
||||
QStringList assignableActionTitles () { return _availableActionTitles; }
|
||||
QString disabledActionName () { return _buttonActionNone; }
|
||||
|
||||
/// Start the polling thread which will in turn emit joystick signals
|
||||
void startPolling(Vehicle* vehicle);
|
||||
void stopPolling(void);
|
||||
|
||||
void setCalibration(int axis, Calibration_t& calibration);
|
||||
Calibration_t getCalibration(int axis);
|
||||
|
||||
void setFunctionAxis(AxisFunction_t function, int axis);
|
||||
int getFunctionAxis(AxisFunction_t function);
|
||||
|
||||
void stop();
|
||||
|
||||
/*
|
||||
// Joystick index used by sdl library
|
||||
// Settable because sdl library remaps indices after certain events
|
||||
virtual int index(void) = 0;
|
||||
virtual void setIndex(int index) = 0;
|
||||
*/
|
||||
virtual bool requiresCalibration(void) { return true; }
|
||||
|
||||
int throttleMode ();
|
||||
void setThrottleMode (int mode);
|
||||
|
||||
bool negativeThrust () const;
|
||||
void setNegativeThrust (bool allowNegative);
|
||||
|
||||
float exponential () const;
|
||||
void setExponential (float expo);
|
||||
|
||||
bool accumulator () const;
|
||||
void setAccumulator (bool accu);
|
||||
|
||||
bool deadband () const;
|
||||
void setDeadband (bool accu);
|
||||
|
||||
bool circleCorrection () const;
|
||||
void setCircleCorrection(bool circleCorrection);
|
||||
|
||||
void setTXMode (int mode);
|
||||
int getTXMode () { return _transmitterMode; }
|
||||
|
||||
/// Set the current calibration mode
|
||||
void setCalibrationMode (bool calibrating);
|
||||
|
||||
/// Get joystick message rate (in Hz)
|
||||
float axisFrequencyHz () const{ return _axisFrequencyHz; }
|
||||
/// Set joystick message rate (in Hz)
|
||||
void setAxisFrequency (float val);
|
||||
|
||||
/// Get joystick button repeat rate (in Hz)
|
||||
float buttonFrequencyHz () const{ return _buttonFrequencyHz; }
|
||||
/// Set joystick button repeat rate (in Hz)
|
||||
void setButtonFrequency(float val);
|
||||
|
||||
signals:
|
||||
// The raw signals are only meant for use by calibration
|
||||
void rawAxisValueChanged (int index, int value);
|
||||
void rawButtonPressedChanged (int index, int pressed);
|
||||
void calibratedChanged (bool calibrated);
|
||||
void buttonActionsChanged ();
|
||||
void assignableActionsChanged ();
|
||||
void throttleModeChanged (int mode);
|
||||
void negativeThrustChanged (bool allowNegative);
|
||||
void exponentialChanged (float exponential);
|
||||
void accumulatorChanged (bool accumulator);
|
||||
void enabledChanged (bool enabled);
|
||||
void circleCorrectionChanged (bool circleCorrection);
|
||||
void axisValues (float roll, float pitch, float yaw, float throttle);
|
||||
|
||||
void axisFrequencyHzChanged ();
|
||||
void buttonFrequencyHzChanged ();
|
||||
void startContinuousZoom (int direction);
|
||||
void stopContinuousZoom ();
|
||||
void stepZoom (int direction);
|
||||
void stepCamera (int direction);
|
||||
void stepStream (int direction);
|
||||
void triggerCamera ();
|
||||
void startVideoRecord ();
|
||||
void stopVideoRecord ();
|
||||
void toggleVideoRecord ();
|
||||
void gimbalPitchStep (int direction);
|
||||
void gimbalYawStep (int direction);
|
||||
void centerGimbal ();
|
||||
void gimbalControlValue (double pitch, double yaw);
|
||||
void setArmed (bool arm);
|
||||
void setVtolInFwdFlight (bool set);
|
||||
void setFlightMode (const QString& flightMode);
|
||||
void emergencyStop ();
|
||||
/**
|
||||
* @brief Send MAV_CMD_DO_GRIPPER command to the vehicle
|
||||
*
|
||||
* @param gripperAction (Open / Close) Gripper action to command
|
||||
*/
|
||||
void gripperAction (GRIPPER_ACTIONS gripperAction);
|
||||
|
||||
protected:
|
||||
void _setDefaultCalibration ();
|
||||
void _saveSettings ();
|
||||
void _saveButtonSettings ();
|
||||
void _loadSettings ();
|
||||
float _adjustRange (int value, Calibration_t calibration, bool withDeadbands);
|
||||
void _executeButtonAction (const QString& action, bool buttonDown);
|
||||
int _findAssignableButtonAction(const QString& action);
|
||||
bool _validAxis (int axis) const;
|
||||
bool _validButton (int button) const;
|
||||
void _handleAxis ();
|
||||
void _handleButtons ();
|
||||
void _buildActionList (Vehicle* activeVehicle);
|
||||
|
||||
void _pitchStep (int direction);
|
||||
void _yawStep (int direction);
|
||||
double _localYaw = 0.0;
|
||||
double _localPitch = 0.0;
|
||||
|
||||
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;
|
||||
virtual bool _getHat (int hat,int i) = 0;
|
||||
|
||||
void _updateTXModeSettingsKey(Vehicle* activeVehicle);
|
||||
int _mapFunctionMode(int mode, int function);
|
||||
void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]);
|
||||
|
||||
// Override from QThread
|
||||
virtual void run();
|
||||
|
||||
protected:
|
||||
|
||||
enum {
|
||||
BUTTON_UP,
|
||||
BUTTON_DOWN,
|
||||
BUTTON_REPEAT
|
||||
};
|
||||
|
||||
static const float _defaultAxisFrequencyHz;
|
||||
static const float _defaultButtonFrequencyHz;
|
||||
|
||||
uint8_t*_rgButtonValues = nullptr;
|
||||
|
||||
std::atomic<bool> _exitThread{false}; ///< true: signal thread to exit
|
||||
bool _calibrationMode = false;
|
||||
int* _rgAxisValues = nullptr;
|
||||
Calibration_t* _rgCalibration = nullptr;
|
||||
ThrottleMode_t _throttleMode = ThrottleModeDownZero;
|
||||
bool _negativeThrust = false;
|
||||
float _exponential = 0;
|
||||
bool _accumulator = false;
|
||||
bool _deadband = false;
|
||||
bool _circleCorrection = true;
|
||||
float _axisFrequencyHz = _defaultAxisFrequencyHz;
|
||||
float _buttonFrequencyHz = _defaultButtonFrequencyHz;
|
||||
Vehicle* _activeVehicle = nullptr;
|
||||
|
||||
bool _pollingStartedForCalibration = false;
|
||||
|
||||
QString _name;
|
||||
bool _calibrated;
|
||||
int _axisCount;
|
||||
int _buttonCount;
|
||||
int _hatCount;
|
||||
int _hatButtonCount;
|
||||
int _totalButtonCount;
|
||||
|
||||
static int _transmitterMode;
|
||||
int _rgFunctionAxis[maxFunction] = {};
|
||||
QElapsedTimer _axisTime;
|
||||
|
||||
QmlObjectListModel _assignableButtonActions;
|
||||
QList<AssignedButtonAction*> _buttonActionArray;
|
||||
QStringList _availableActionTitles;
|
||||
MultiVehicleManager* _multiVehicleManager = nullptr;
|
||||
|
||||
QList<JoystickMavCommand> _customMavCommands;
|
||||
|
||||
static const float _minAxisFrequencyHz;
|
||||
static const float _maxAxisFrequencyHz;
|
||||
static const float _minButtonFrequencyHz;
|
||||
static const float _maxButtonFrequencyHz;
|
||||
|
||||
private:
|
||||
static const char* _rgFunctionSettingsKey[maxFunction];
|
||||
|
||||
static const char* _settingsGroup;
|
||||
static const char* _calibratedSettingsKey;
|
||||
static const char* _buttonActionNameKey;
|
||||
static const char* _buttonActionRepeatKey;
|
||||
static const char* _throttleModeSettingsKey;
|
||||
static const char* _negativeThrustSettingsKey;
|
||||
static const char* _exponentialSettingsKey;
|
||||
static const char* _accumulatorSettingsKey;
|
||||
static const char* _deadbandSettingsKey;
|
||||
static const char* _circleCorrectionSettingsKey;
|
||||
static const char* _axisFrequencySettingsKey;
|
||||
static const char* _buttonFrequencySettingsKey;
|
||||
static const char* _txModeSettingsKey;
|
||||
static const char* _fixedWingTXModeSettingsKey;
|
||||
static const char* _multiRotorTXModeSettingsKey;
|
||||
static const char* _roverTXModeSettingsKey;
|
||||
static const char* _vtolTXModeSettingsKey;
|
||||
static const char* _submarineTXModeSettingsKey;
|
||||
|
||||
static const char* _buttonActionNone;
|
||||
static const char* _buttonActionArm;
|
||||
static const char* _buttonActionDisarm;
|
||||
static const char* _buttonActionToggleArm;
|
||||
static const char* _buttonActionVTOLFixedWing;
|
||||
static const char* _buttonActionVTOLMultiRotor;
|
||||
static const char* _buttonActionStepZoomIn;
|
||||
static const char* _buttonActionStepZoomOut;
|
||||
static const char* _buttonActionContinuousZoomIn;
|
||||
static const char* _buttonActionContinuousZoomOut;
|
||||
static const char* _buttonActionNextStream;
|
||||
static const char* _buttonActionPreviousStream;
|
||||
static const char* _buttonActionNextCamera;
|
||||
static const char* _buttonActionPreviousCamera;
|
||||
static const char* _buttonActionTriggerCamera;
|
||||
static const char* _buttonActionStartVideoRecord;
|
||||
static const char* _buttonActionStopVideoRecord;
|
||||
static const char* _buttonActionToggleVideoRecord;
|
||||
static const char* _buttonActionGimbalDown;
|
||||
static const char* _buttonActionGimbalUp;
|
||||
static const char* _buttonActionGimbalLeft;
|
||||
static const char* _buttonActionGimbalRight;
|
||||
static const char* _buttonActionGimbalCenter;
|
||||
static const char* _buttonActionEmergencyStop;
|
||||
static const char* _buttonActionGripperGrab;
|
||||
static const char* _buttonActionGripperRelease;
|
||||
|
||||
|
||||
private slots:
|
||||
void _activeVehicleChanged(Vehicle* activeVehicle);
|
||||
void _vehicleCountChanged(int count);
|
||||
void _flightModesChanged();
|
||||
};
|
||||
@ -0,0 +1,293 @@
|
||||
#include "JoystickAndroid.h"
|
||||
|
||||
#include "QGCApplication.h"
|
||||
|
||||
#include <QQmlEngine>
|
||||
|
||||
int JoystickAndroid::_androidBtnListCount;
|
||||
int *JoystickAndroid::_androidBtnList;
|
||||
int JoystickAndroid::ACTION_DOWN;
|
||||
int JoystickAndroid::ACTION_UP;
|
||||
QMutex JoystickAndroid::m_mutex;
|
||||
|
||||
static void clear_jni_exception()
|
||||
{
|
||||
QAndroidJniEnvironment jniEnv;
|
||||
if (jniEnv->ExceptionCheck()) {
|
||||
jniEnv->ExceptionDescribe();
|
||||
jniEnv->ExceptionClear();
|
||||
}
|
||||
}
|
||||
|
||||
JoystickAndroid::JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager)
|
||||
: Joystick(name,axisCount,buttonCount,0,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<jbooleanArray>();
|
||||
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<jint>("getAxis");
|
||||
// Don't allow two axis with the same code
|
||||
for (int j = 0; j < i; j++) {
|
||||
if (axisCode[i] == axisCode[j]) {
|
||||
axisCode[i] = -1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
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<QString, Joystick*> JoystickAndroid::discover(MultiVehicleManager* _multiVehicleManager) {
|
||||
static QMap<QString, Joystick*> ret;
|
||||
|
||||
QMutexLocker lock(&m_mutex);
|
||||
|
||||
QAndroidJniEnvironment env;
|
||||
QAndroidJniObject o = QAndroidJniObject::callStaticObjectMethod<jintArray>("android/view/InputDevice", "getDeviceIds");
|
||||
jintArray jarr = o.object<jintArray>();
|
||||
int sz = env->GetArrayLength(jarr);
|
||||
jint *buff = env->GetIntArrayElements(jarr, nullptr);
|
||||
|
||||
int SOURCE_GAMEPAD = QAndroidJniObject::getStaticField<jint>("android/view/InputDevice", "SOURCE_GAMEPAD");
|
||||
int SOURCE_JOYSTICK = QAndroidJniObject::getStaticField<jint>("android/view/InputDevice", "SOURCE_JOYSTICK");
|
||||
|
||||
QList<QString> names;
|
||||
|
||||
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<jint>("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();
|
||||
|
||||
names.push_back(name);
|
||||
|
||||
if (ret.contains(name)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// get number of axis
|
||||
QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;");
|
||||
int axisCount = rangeListNative.callMethod<jint>("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<jbooleanArray>();
|
||||
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);
|
||||
}
|
||||
|
||||
for (auto i = ret.begin(); i != ret.end();) {
|
||||
if (!names.contains(i.key())) {
|
||||
i = ret.erase(i);
|
||||
} else {
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
env->ReleaseIntArrayElements(jarr, buff, 0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
bool JoystickAndroid::handleKeyEvent(jobject event) {
|
||||
QJNIObjectPrivate ev(event);
|
||||
QMutexLocker lock(&m_mutex);
|
||||
const int _deviceId = ev.callMethod<jint>("getDeviceId", "()I");
|
||||
if (_deviceId!=deviceId) return false;
|
||||
|
||||
const int action = ev.callMethod<jint>("getAction", "()I");
|
||||
const int keyCode = ev.callMethod<jint>("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<jint>("getDeviceId", "()I");
|
||||
if (_deviceId!=deviceId) return false;
|
||||
|
||||
for (int i = 0; i <_axisCount; i++) {
|
||||
const float v = ev.callMethod<jfloat>("getAxisValue", "(I)F",axisCode[i]);
|
||||
axisValue[i] = static_cast<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 ];
|
||||
}
|
||||
|
||||
bool JoystickAndroid::_getHat(int hat,int i) {
|
||||
Q_UNUSED(hat);
|
||||
Q_UNUSED(i);
|
||||
return false;
|
||||
}
|
||||
|
||||
static JoystickManager *_manager = nullptr;
|
||||
|
||||
//helper method
|
||||
bool JoystickAndroid::init(JoystickManager *manager) {
|
||||
_manager = manager;
|
||||
|
||||
//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;
|
||||
|
||||
clear_jni_exception();
|
||||
for (i = 1; i <= 16; i++) {
|
||||
QString name = "KEYCODE_BUTTON_"+QString::number(i);
|
||||
ret[i-1] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", name.toStdString().c_str());
|
||||
}
|
||||
i--;
|
||||
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_A");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_B");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_C");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_L1");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_L2");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_R1");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_R2");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_MODE");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_SELECT");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_START");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBL");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBR");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_X");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_Y");
|
||||
ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_Z");
|
||||
|
||||
ACTION_DOWN = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "ACTION_DOWN");
|
||||
ACTION_UP = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "ACTION_UP");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static const char kJniClassName[] {"org/mavlink/qgroundcontrol/QGCActivity"};
|
||||
|
||||
static void jniUpdateAvailableJoysticks(JNIEnv *envA, jobject thizA)
|
||||
{
|
||||
Q_UNUSED(envA);
|
||||
Q_UNUSED(thizA);
|
||||
|
||||
if (_manager != nullptr) {
|
||||
qCDebug(JoystickLog) << "jniUpdateAvailableJoysticks triggered";
|
||||
emit _manager->updateAvailableJoysticksSignal();
|
||||
}
|
||||
}
|
||||
|
||||
void JoystickAndroid::setNativeMethods()
|
||||
{
|
||||
qCDebug(JoystickLog) << "Registering Native Functions";
|
||||
|
||||
// REGISTER THE C++ FUNCTION WITH JNI
|
||||
JNINativeMethod javaMethods[] {
|
||||
{"nativeUpdateAvailableJoysticks", "()V", reinterpret_cast<void *>(jniUpdateAvailableJoysticks)}
|
||||
};
|
||||
|
||||
clear_jni_exception();
|
||||
QAndroidJniEnvironment jniEnv;
|
||||
jclass objectClass = jniEnv->FindClass(kJniClassName);
|
||||
if(!objectClass) {
|
||||
clear_jni_exception();
|
||||
qWarning() << "Couldn't find class:" << kJniClassName;
|
||||
return;
|
||||
}
|
||||
|
||||
jint val = jniEnv->RegisterNatives(objectClass, javaMethods, sizeof(javaMethods) / sizeof(javaMethods[0]));
|
||||
|
||||
if (val < 0) {
|
||||
qWarning() << "Error registering methods: " << val;
|
||||
} else {
|
||||
qCDebug(JoystickLog) << "Native Functions Registered";
|
||||
}
|
||||
clear_jni_exception();
|
||||
}
|
||||
@ -0,0 +1,54 @@
|
||||
#ifndef JOYSTICKANDROID_H
|
||||
#define JOYSTICKANDROID_H
|
||||
|
||||
#include "Joystick.h"
|
||||
#include "Vehicle.h"
|
||||
#include "MultiVehicleManager.h"
|
||||
|
||||
#include <jni.h>
|
||||
#include <QtCore/private/qjni_p.h>
|
||||
#include <QtCore/private/qjnihelpers_p.h>
|
||||
#include <QtAndroidExtras/QtAndroidExtras>
|
||||
#include <QtAndroidExtras/QAndroidJniObject>
|
||||
|
||||
|
||||
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 bool init(JoystickManager *manager);
|
||||
|
||||
static void setNativeMethods();
|
||||
|
||||
static QMap<QString, Joystick*> 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);
|
||||
virtual bool _getHat (int hat,int i);
|
||||
|
||||
int *btnCode;
|
||||
int *axisCode;
|
||||
bool *btnValue;
|
||||
int *axisValue;
|
||||
|
||||
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
|
||||
@ -0,0 +1,226 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include "JoystickManager.h"
|
||||
#include "QGCApplication.h"
|
||||
|
||||
#include <QQmlEngine>
|
||||
|
||||
#ifndef __mobile__
|
||||
#include "JoystickSDL.h"
|
||||
#define __sdljoystick__
|
||||
#endif
|
||||
|
||||
#ifdef __android__
|
||||
#include "JoystickAndroid.h"
|
||||
#endif
|
||||
|
||||
QGC_LOGGING_CATEGORY(JoystickManagerLog, "JoystickManagerLog")
|
||||
|
||||
const char * JoystickManager::_settingsGroup = "JoystickManager";
|
||||
const char * JoystickManager::_settingsKeyActiveJoystick = "ActiveJoystick";
|
||||
|
||||
JoystickManager::JoystickManager(QGCApplication* app, QGCToolbox* toolbox)
|
||||
: QGCTool(app, toolbox)
|
||||
, _activeJoystick(nullptr)
|
||||
, _multiVehicleManager(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
JoystickManager::~JoystickManager() {
|
||||
QMap<QString, Joystick*>::iterator i;
|
||||
for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) {
|
||||
qCDebug(JoystickManagerLog) << "Releasing joystick:" << i.key();
|
||||
i.value()->stop();
|
||||
delete i.value();
|
||||
}
|
||||
qDebug() << "Done";
|
||||
}
|
||||
|
||||
void JoystickManager::setToolbox(QGCToolbox *toolbox)
|
||||
{
|
||||
QGCTool::setToolbox(toolbox);
|
||||
|
||||
_multiVehicleManager = _toolbox->multiVehicleManager();
|
||||
|
||||
QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
|
||||
}
|
||||
|
||||
void JoystickManager::init() {
|
||||
#ifdef __sdljoystick__
|
||||
if (!JoystickSDL::init()) {
|
||||
return;
|
||||
}
|
||||
_setActiveJoystickFromSettings();
|
||||
#elif defined(__android__)
|
||||
if (!JoystickAndroid::init(this)) {
|
||||
return;
|
||||
}
|
||||
connect(this, &JoystickManager::updateAvailableJoysticksSignal, this, &JoystickManager::restartJoystickCheckTimer);
|
||||
#endif
|
||||
connect(&_joystickCheckTimer, &QTimer::timeout, this, &JoystickManager::_updateAvailableJoysticks);
|
||||
_joystickCheckTimerCounter = 5;
|
||||
_joystickCheckTimer.start(1000);
|
||||
}
|
||||
|
||||
void JoystickManager::_setActiveJoystickFromSettings(void)
|
||||
{
|
||||
QMap<QString,Joystick*> newMap;
|
||||
|
||||
#ifdef __sdljoystick__
|
||||
// Get the latest joystick mapping
|
||||
newMap = JoystickSDL::discover(_multiVehicleManager);
|
||||
#elif defined(__android__)
|
||||
newMap = JoystickAndroid::discover(_multiVehicleManager);
|
||||
#endif
|
||||
|
||||
if (_activeJoystick && !newMap.contains(_activeJoystick->name())) {
|
||||
qCDebug(JoystickManagerLog) << "Active joystick removed";
|
||||
setActiveJoystick(nullptr);
|
||||
}
|
||||
|
||||
// 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()) {
|
||||
setActiveJoystick(nullptr);
|
||||
return;
|
||||
}
|
||||
|
||||
QSettings settings;
|
||||
|
||||
settings.beginGroup(_settingsGroup);
|
||||
QString name = settings.value(_settingsKeyActiveJoystick).toString();
|
||||
|
||||
if (name.isEmpty()) {
|
||||
name = _name2JoystickMap.first()->name();
|
||||
}
|
||||
|
||||
setActiveJoystick(_name2JoystickMap.value(name, _name2JoystickMap.first()));
|
||||
settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
|
||||
}
|
||||
|
||||
Joystick* JoystickManager::activeJoystick(void)
|
||||
{
|
||||
return _activeJoystick;
|
||||
}
|
||||
|
||||
void JoystickManager::setActiveJoystick(Joystick* joystick)
|
||||
{
|
||||
QSettings settings;
|
||||
|
||||
if (joystick != nullptr && !_name2JoystickMap.contains(joystick->name())) {
|
||||
qCWarning(JoystickManagerLog) << "Set active not in map" << joystick->name();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_activeJoystick == joystick) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_activeJoystick) {
|
||||
_activeJoystick->stopPolling();
|
||||
}
|
||||
|
||||
_activeJoystick = joystick;
|
||||
|
||||
if (_activeJoystick != nullptr) {
|
||||
qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name();
|
||||
|
||||
settings.beginGroup(_settingsGroup);
|
||||
settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
|
||||
}
|
||||
|
||||
emit activeJoystickChanged(_activeJoystick);
|
||||
emit activeJoystickNameChanged(_activeJoystick?_activeJoystick->name():"");
|
||||
}
|
||||
|
||||
QVariantList JoystickManager::joysticks(void)
|
||||
{
|
||||
QVariantList list;
|
||||
|
||||
for (const QString &name: _name2JoystickMap.keys()) {
|
||||
list += QVariant::fromValue(_name2JoystickMap[name]);
|
||||
}
|
||||
|
||||
return list;
|
||||
}
|
||||
|
||||
QStringList JoystickManager::joystickNames(void)
|
||||
{
|
||||
return _name2JoystickMap.keys();
|
||||
}
|
||||
|
||||
QString JoystickManager::activeJoystickName(void)
|
||||
{
|
||||
return _activeJoystick ? _activeJoystick->name() : QString();
|
||||
}
|
||||
|
||||
bool JoystickManager::setActiveJoystickName(const QString& name)
|
||||
{
|
||||
if (_name2JoystickMap.contains(name)) {
|
||||
setActiveJoystick(_name2JoystickMap[name]);
|
||||
return true;
|
||||
} else {
|
||||
qCWarning(JoystickManagerLog) << "Set active not in map" << name;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: move this to the right place: JoystickSDL.cc and JoystickAndroid.cc respectively and call through Joystick.cc
|
||||
*/
|
||||
void JoystickManager::_updateAvailableJoysticks()
|
||||
{
|
||||
#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__)
|
||||
_joystickCheckTimerCounter--;
|
||||
_setActiveJoystickFromSettings();
|
||||
if (_joystickCheckTimerCounter <= 0) {
|
||||
_joystickCheckTimer.stop();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void JoystickManager::restartJoystickCheckTimer()
|
||||
{
|
||||
_joystickCheckTimerCounter = 5;
|
||||
_joystickCheckTimer.start(1000);
|
||||
}
|
||||
@ -0,0 +1,82 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file
|
||||
/// @brief Joystick Manager
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "QGCLoggingCategory.h"
|
||||
#include "Joystick.h"
|
||||
#include "MultiVehicleManager.h"
|
||||
#include "QGCToolbox.h"
|
||||
|
||||
#include <QVariantList>
|
||||
|
||||
Q_DECLARE_LOGGING_CATEGORY(JoystickManagerLog)
|
||||
|
||||
/// Joystick Manager
|
||||
class JoystickManager : public QGCTool
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
JoystickManager(QGCApplication* app, QGCToolbox* toolbox);
|
||||
~JoystickManager();
|
||||
|
||||
Q_PROPERTY(QVariantList joysticks READ joysticks NOTIFY availableJoysticksChanged)
|
||||
Q_PROPERTY(QStringList joystickNames READ joystickNames NOTIFY availableJoysticksChanged)
|
||||
|
||||
Q_PROPERTY(Joystick* activeJoystick READ activeJoystick WRITE setActiveJoystick NOTIFY activeJoystickChanged)
|
||||
Q_PROPERTY(QString activeJoystickName READ activeJoystickName WRITE setActiveJoystickName NOTIFY activeJoystickNameChanged)
|
||||
|
||||
/// List of available joysticks
|
||||
QVariantList joysticks();
|
||||
/// List of available joystick names
|
||||
QStringList joystickNames(void);
|
||||
|
||||
/// Get active joystick
|
||||
Joystick* activeJoystick(void);
|
||||
/// Set active joystick
|
||||
void setActiveJoystick(Joystick* joystick);
|
||||
|
||||
QString activeJoystickName(void);
|
||||
bool setActiveJoystickName(const QString& name);
|
||||
|
||||
void restartJoystickCheckTimer(void);
|
||||
|
||||
// Override from QGCTool
|
||||
virtual void setToolbox(QGCToolbox *toolbox);
|
||||
|
||||
public slots:
|
||||
void init();
|
||||
|
||||
signals:
|
||||
void activeJoystickChanged(Joystick* joystick);
|
||||
void activeJoystickNameChanged(const QString& name);
|
||||
void availableJoysticksChanged(void);
|
||||
void updateAvailableJoysticksSignal();
|
||||
|
||||
private slots:
|
||||
void _updateAvailableJoysticks(void);
|
||||
|
||||
private:
|
||||
void _setActiveJoystickFromSettings(void);
|
||||
|
||||
private:
|
||||
Joystick* _activeJoystick;
|
||||
QMap<QString, Joystick*> _name2JoystickMap;
|
||||
MultiVehicleManager* _multiVehicleManager;
|
||||
|
||||
static const char * _settingsGroup;
|
||||
static const char * _settingsKeyActiveJoystick;
|
||||
|
||||
int _joystickCheckTimerCounter;
|
||||
QTimer _joystickCheckTimer;
|
||||
};
|
||||
@ -0,0 +1,101 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "JoystickMavCommand.h"
|
||||
#include "QGCLoggingCategory.h"
|
||||
#include "Vehicle.h"
|
||||
#include <QJsonDocument>
|
||||
#include <QJsonParseError>
|
||||
#include <QJsonArray>
|
||||
|
||||
QGC_LOGGING_CATEGORY(JoystickMavCommandLog, "JoystickMavCommandLog")
|
||||
|
||||
static void parseJsonValue(const QJsonObject& jsonObject, const QString& key, float& param)
|
||||
{
|
||||
if (jsonObject.contains(key))
|
||||
param = static_cast<float>(jsonObject.value(key).toDouble());
|
||||
}
|
||||
|
||||
QList<JoystickMavCommand> JoystickMavCommand::load(const QString& jsonFilename)
|
||||
{
|
||||
qCDebug(JoystickMavCommandLog) << "Loading" << jsonFilename;
|
||||
QList<JoystickMavCommand> result;
|
||||
|
||||
QFile jsonFile(jsonFilename);
|
||||
if (!jsonFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||||
qCDebug(JoystickMavCommandLog) << "Could not open" << jsonFilename;
|
||||
return result;
|
||||
}
|
||||
|
||||
QByteArray bytes = jsonFile.readAll();
|
||||
jsonFile.close();
|
||||
QJsonParseError jsonParseError;
|
||||
QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError);
|
||||
if (jsonParseError.error != QJsonParseError::NoError) {
|
||||
qWarning() << jsonFilename << "Unable to open json document" << jsonParseError.errorString();
|
||||
return result;
|
||||
}
|
||||
|
||||
QJsonObject json = doc.object();
|
||||
|
||||
const int version = json.value("version").toInt();
|
||||
if (version != 1) {
|
||||
qWarning() << jsonFilename << ": invalid version" << version;
|
||||
return result;
|
||||
}
|
||||
|
||||
QJsonValue jsonValue = json.value("commands");
|
||||
if (!jsonValue.isArray()) {
|
||||
qWarning() << jsonFilename << ": 'commands' is not an array";
|
||||
return result;
|
||||
}
|
||||
|
||||
QJsonArray jsonArray = jsonValue.toArray();
|
||||
for (QJsonValue info: jsonArray) {
|
||||
if (!info.isObject()) {
|
||||
qWarning() << jsonFilename << ": 'commands' should contain objects";
|
||||
return result;
|
||||
}
|
||||
|
||||
auto jsonObject = info.toObject();
|
||||
JoystickMavCommand item;
|
||||
if (!jsonObject.contains("id")) {
|
||||
qWarning() << jsonFilename << ": 'id' is required";
|
||||
continue;
|
||||
}
|
||||
item._id = jsonObject.value("id").toInt();
|
||||
if (!jsonObject.contains("name")) {
|
||||
qWarning() << jsonFilename << ": 'name' is required";
|
||||
continue;
|
||||
}
|
||||
item._name = jsonObject.value("name").toString();
|
||||
item._showError = jsonObject.value("showError").toBool();
|
||||
parseJsonValue(jsonObject, "param1", item._param1);
|
||||
parseJsonValue(jsonObject, "param2", item._param2);
|
||||
parseJsonValue(jsonObject, "param3", item._param3);
|
||||
parseJsonValue(jsonObject, "param4", item._param4);
|
||||
parseJsonValue(jsonObject, "param5", item._param5);
|
||||
parseJsonValue(jsonObject, "param6", item._param6);
|
||||
parseJsonValue(jsonObject, "param7", item._param7);
|
||||
|
||||
qCDebug(JoystickMavCommandLog) << jsonObject;
|
||||
|
||||
result.append(item);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void JoystickMavCommand::send(Vehicle* vehicle)
|
||||
{
|
||||
vehicle->sendMavCommand(vehicle->defaultComponentId(),
|
||||
static_cast<MAV_CMD>(_id),
|
||||
_showError,
|
||||
_param1, _param2, _param3, _param4, _param5, _param6, _param7);
|
||||
}
|
||||
@ -0,0 +1,40 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file
|
||||
/// @brief Custom Joystick MAV command
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QString>
|
||||
#include <QList>
|
||||
|
||||
class Vehicle;
|
||||
|
||||
/// Custom MAV command
|
||||
class JoystickMavCommand
|
||||
{
|
||||
public:
|
||||
static QList<JoystickMavCommand> load(const QString& jsonFilename);
|
||||
QString name() const { return _name; }
|
||||
|
||||
void send(Vehicle* vehicle);
|
||||
private:
|
||||
QString _name;
|
||||
int _id = 0;
|
||||
bool _showError = false;
|
||||
float _param1 = 0.0f;
|
||||
float _param2 = 0.0f;
|
||||
float _param3 = 0.0f;
|
||||
float _param4 = 0.0f;
|
||||
float _param5 = 0.0f;
|
||||
float _param6 = 0.0f;
|
||||
float _param7 = 0.0f;
|
||||
};
|
||||
|
||||
@ -0,0 +1,17 @@
|
||||
{
|
||||
"comment": "Joystick MAV commands",
|
||||
"version": 1,
|
||||
|
||||
"commands": [
|
||||
{
|
||||
"id": 31010,
|
||||
"name": "MAV_CMD_USER_1",
|
||||
"param1": 1.0
|
||||
},
|
||||
{
|
||||
"id": 31011,
|
||||
"name": "MAV_CMD_USER_2",
|
||||
"param1": 0.0
|
||||
}
|
||||
]
|
||||
}
|
||||
@ -0,0 +1,178 @@
|
||||
#include "JoystickSDL.h"
|
||||
|
||||
#include "QGCApplication.h"
|
||||
|
||||
#include <QQmlEngine>
|
||||
#include <QTextStream>
|
||||
|
||||
JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager)
|
||||
: Joystick(name,axisCount,buttonCount,hatCount,multiVehicleManager)
|
||||
, _isGameController(isGameController)
|
||||
, _index(index)
|
||||
{
|
||||
if(_isGameController) _setDefaultCalibration();
|
||||
}
|
||||
|
||||
bool JoystickSDL::init(void) {
|
||||
if (SDL_InitSubSystem(SDL_INIT_GAMECONTROLLER | SDL_INIT_JOYSTICK) < 0) {
|
||||
SDL_JoystickEventState(SDL_ENABLE);
|
||||
qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError();
|
||||
return false;
|
||||
}
|
||||
_loadGameControllerMappings();
|
||||
return true;
|
||||
}
|
||||
|
||||
QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) {
|
||||
static QMap<QString, Joystick*> ret;
|
||||
|
||||
QMap<QString,Joystick*> newRet;
|
||||
|
||||
// Load available joysticks
|
||||
|
||||
qCDebug(JoystickLog) << "Available joysticks";
|
||||
|
||||
for (int i=0; i<SDL_NumJoysticks(); i++) {
|
||||
QString name = SDL_JoystickNameForIndex(i);
|
||||
|
||||
if (!ret.contains(name)) {
|
||||
int axisCount, buttonCount, hatCount;
|
||||
bool isGameController = SDL_IsGameController(i);
|
||||
|
||||
if (SDL_Joystick* sdlJoystick = SDL_JoystickOpen(i)) {
|
||||
SDL_ClearError();
|
||||
axisCount = SDL_JoystickNumAxes(sdlJoystick);
|
||||
buttonCount = SDL_JoystickNumButtons(sdlJoystick);
|
||||
hatCount = SDL_JoystickNumHats(sdlJoystick);
|
||||
if (axisCount < 0 || buttonCount < 0 || hatCount < 0) {
|
||||
qCWarning(JoystickLog) << "\t libsdl error parsing joystick features:" << SDL_GetError();
|
||||
}
|
||||
SDL_JoystickClose(sdlJoystick);
|
||||
} else {
|
||||
qCWarning(JoystickLog) << "\t libsdl failed opening joystick" << qPrintable(name) << "error:" << SDL_GetError();
|
||||
continue;
|
||||
}
|
||||
|
||||
qCDebug(JoystickLog) << "\t" << name << "axes:" << axisCount << "buttons:" << buttonCount << "hats:" << hatCount << "isGC:" << isGameController;
|
||||
|
||||
// Check for joysticks with duplicate names and differentiate the keys when necessary.
|
||||
// This is required when using an Xbox 360 wireless receiver that always identifies as
|
||||
// 4 individual joysticks, regardless of how many joysticks are actually connected to the
|
||||
// receiver. Using GUID does not help, all of these devices present the same GUID.
|
||||
QString originalName = name;
|
||||
uint8_t duplicateIdx = 1;
|
||||
while (newRet[name]) {
|
||||
name = QString("%1 %2").arg(originalName).arg(duplicateIdx++);
|
||||
}
|
||||
|
||||
newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager);
|
||||
} else {
|
||||
newRet[name] = ret[name];
|
||||
JoystickSDL *j = static_cast<JoystickSDL*>(newRet[name]);
|
||||
if (j->index() != i) {
|
||||
j->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;
|
||||
}
|
||||
|
||||
void JoystickSDL::_loadGameControllerMappings(void) {
|
||||
QFile file(":/db/mapping/joystick/gamecontrollerdb.txt");
|
||||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
|
||||
{
|
||||
qWarning() << "Couldn't load GameController mapping database.";
|
||||
return;
|
||||
}
|
||||
|
||||
QTextStream s(&file);
|
||||
|
||||
while (!s.atEnd()) {
|
||||
SDL_GameControllerAddMapping(s.readLine().toStdString().c_str());
|
||||
}
|
||||
}
|
||||
|
||||
bool JoystickSDL::_open(void) {
|
||||
if ( _isGameController ) {
|
||||
sdlController = SDL_GameControllerOpen(_index);
|
||||
sdlJoystick = SDL_GameControllerGetJoystick(sdlController);
|
||||
} else {
|
||||
sdlJoystick = SDL_JoystickOpen(_index);
|
||||
}
|
||||
|
||||
if (!sdlJoystick) {
|
||||
qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError();
|
||||
return false;
|
||||
}
|
||||
|
||||
qCDebug(JoystickLog) << "Opened joystick at" << sdlJoystick;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void JoystickSDL::_close(void) {
|
||||
if (sdlJoystick == nullptr) {
|
||||
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);
|
||||
// This segfaults so often, and I've spent so much time trying to find the cause and fix it
|
||||
// I think this might be an SDL bug
|
||||
// We are much more stable just commenting this out
|
||||
//SDL_JoystickClose(sdlJoystick);
|
||||
}
|
||||
}
|
||||
|
||||
sdlJoystick = nullptr;
|
||||
sdlController = nullptr;
|
||||
}
|
||||
|
||||
bool JoystickSDL::_update(void)
|
||||
{
|
||||
SDL_JoystickUpdate();
|
||||
SDL_GameControllerUpdate();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool JoystickSDL::_getButton(int i) {
|
||||
if (_isGameController) {
|
||||
return SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)) == 1;
|
||||
} else {
|
||||
return SDL_JoystickGetButton(sdlJoystick, i) == 1;
|
||||
}
|
||||
}
|
||||
|
||||
int JoystickSDL::_getAxis(int i) {
|
||||
if (_isGameController) {
|
||||
return SDL_GameControllerGetAxis(sdlController, SDL_GameControllerAxis(i));
|
||||
} else {
|
||||
return SDL_JoystickGetAxis(sdlJoystick, i);
|
||||
}
|
||||
}
|
||||
|
||||
bool JoystickSDL::_getHat(int hat, int i) {
|
||||
uint8_t hatButtons[] = {SDL_HAT_UP,SDL_HAT_DOWN,SDL_HAT_LEFT,SDL_HAT_RIGHT};
|
||||
if (i < int(sizeof(hatButtons))) {
|
||||
return (SDL_JoystickGetHat(sdlJoystick, hat) & hatButtons[i]) != 0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@ -0,0 +1,53 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file
|
||||
/// @brief SDL Joystick Interface
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Joystick.h"
|
||||
#include "Vehicle.h"
|
||||
#include "MultiVehicleManager.h"
|
||||
|
||||
#include <SDL.h>
|
||||
|
||||
/// @brief SDL Joystick Interface
|
||||
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 bool init(void);
|
||||
|
||||
int index(void) const { 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; }
|
||||
|
||||
private:
|
||||
static void _loadGameControllerMappings();
|
||||
|
||||
bool _open () final;
|
||||
void _close () final;
|
||||
bool _update () final;
|
||||
|
||||
bool _getButton (int i) final;
|
||||
int _getAxis (int i) final;
|
||||
bool _getHat (int hat,int i) final;
|
||||
|
||||
SDL_Joystick* sdlJoystick;
|
||||
SDL_GameController* sdlController;
|
||||
|
||||
bool _isGameController;
|
||||
int _index; ///< Index for SDL_JoystickOpen
|
||||
|
||||
};
|
||||
@ -0,0 +1,16 @@
|
||||
|
||||
add_library(PositionManager
|
||||
PositionManager.cpp
|
||||
SimulatedPosition.cc
|
||||
)
|
||||
|
||||
target_link_libraries(PositionManager
|
||||
PUBLIC
|
||||
qgc
|
||||
)
|
||||
|
||||
target_include_directories(PositionManager
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
|
||||
@ -0,0 +1,202 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "PositionManager.h"
|
||||
#include "QGCApplication.h"
|
||||
#include "QGCCorePlugin.h"
|
||||
|
||||
#if !defined(NO_SERIAL_LINK) && !defined(__android__)
|
||||
#include <QSerialPortInfo>
|
||||
#endif
|
||||
|
||||
#include <QtPositioning/private/qgeopositioninfosource_p.h>
|
||||
|
||||
QGCPositionManager::QGCPositionManager(QGCApplication* app, QGCToolbox* toolbox)
|
||||
: QGCTool (app, toolbox)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
QGCPositionManager::~QGCPositionManager()
|
||||
{
|
||||
delete(_simulatedSource);
|
||||
delete(_nmeaSource);
|
||||
}
|
||||
|
||||
void QGCPositionManager::setToolbox(QGCToolbox *toolbox)
|
||||
{
|
||||
QGCTool::setToolbox(toolbox);
|
||||
//-- First see if plugin provides a position source
|
||||
_defaultSource = toolbox->corePlugin()->createPositionSource(this);
|
||||
if (_defaultSource) {
|
||||
_usingPluginSource = true;
|
||||
}
|
||||
|
||||
if (qgcApp()->runningUnitTests()) {
|
||||
// Units test on travis fail due to lack of position source
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_defaultSource) {
|
||||
//-- Otherwise, create a default one
|
||||
#if 1
|
||||
// Calling this can end up falling through a path which tries to instantiate a serialnmea source.
|
||||
// The Qt code for this will pop a qWarning if there are no serial ports available. This in turn
|
||||
// causes you to be unable to run with QT_FATAL_WARNINGS=1 to debug stuff.
|
||||
_defaultSource = QGeoPositionInfoSource::createDefaultSource(this);
|
||||
#else
|
||||
// So instead we create our own version of QGeoPositionInfoSource::createDefaultSource which isn't as stupid.
|
||||
QList<QCborMap> plugins = QGeoPositionInfoSourcePrivate::pluginsSorted();
|
||||
foreach (const auto &obj, plugins) {
|
||||
if (obj.value("Position").isBool() && obj.value("Position").toBool()) {
|
||||
QString pluginName = obj.value("Keys").toArray()[0].toString();
|
||||
if (pluginName == "serialnmea") {
|
||||
#if !defined(NO_SERIAL_LINK) && !defined(__android__)
|
||||
if (QSerialPortInfo::availablePorts().isEmpty()) {
|
||||
// This prevents the qWarning from popping
|
||||
continue;
|
||||
}
|
||||
#else
|
||||
continue;
|
||||
#endif
|
||||
}
|
||||
_defaultSource = QGeoPositionInfoSource::createSource(pluginName, this);
|
||||
if (_defaultSource) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
_simulatedSource = new SimulatedPosition();
|
||||
|
||||
#if 1
|
||||
setPositionSource(QGCPositionSource::InternalGPS);
|
||||
#else
|
||||
setPositionSource(QGCPositionManager::Simulated);
|
||||
#endif
|
||||
}
|
||||
|
||||
void QGCPositionManager::setNmeaSourceDevice(QIODevice* device)
|
||||
{
|
||||
// stop and release _nmeaSource
|
||||
if (_nmeaSource) {
|
||||
_nmeaSource->stopUpdates();
|
||||
disconnect(_nmeaSource);
|
||||
|
||||
// if _currentSource is pointing there, point to null
|
||||
if (_currentSource == _nmeaSource){
|
||||
_currentSource = nullptr;
|
||||
}
|
||||
|
||||
delete _nmeaSource;
|
||||
_nmeaSource = nullptr;
|
||||
|
||||
}
|
||||
_nmeaSource = new QNmeaPositionInfoSource(QNmeaPositionInfoSource::RealTimeMode, this);
|
||||
_nmeaSource->setDevice(device);
|
||||
// set equivalent range error to enable position accuracy reporting
|
||||
_nmeaSource->setUserEquivalentRangeError(5.1);
|
||||
setPositionSource(QGCPositionManager::NmeaGPS);
|
||||
}
|
||||
|
||||
void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update)
|
||||
{
|
||||
_geoPositionInfo = update;
|
||||
|
||||
QGeoCoordinate newGCSPosition = QGeoCoordinate();
|
||||
qreal newGCSHeading = update.attribute(QGeoPositionInfo::Direction);
|
||||
|
||||
if (update.isValid() && update.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
|
||||
// Note that gcsPosition filters out possible crap values
|
||||
if (qAbs(update.coordinate().latitude()) > 0.001 &&
|
||||
qAbs(update.coordinate().longitude()) > 0.001 ) {
|
||||
_gcsPositionHorizontalAccuracy = update.attribute(QGeoPositionInfo::HorizontalAccuracy);
|
||||
if (_gcsPositionHorizontalAccuracy <= MinHorizonalAccuracyMeters) {
|
||||
newGCSPosition = update.coordinate();
|
||||
}
|
||||
emit gcsPositionHorizontalAccuracyChanged();
|
||||
}
|
||||
}
|
||||
if (newGCSPosition != _gcsPosition) {
|
||||
_gcsPosition = newGCSPosition;
|
||||
emit gcsPositionChanged(_gcsPosition);
|
||||
}
|
||||
|
||||
// At this point only plugins support gcs heading. The reason is that the quality of heading information from a local
|
||||
// position device (not a compass) is unknown. In many cases it can only be trusted if the GCS location is moving above
|
||||
// a certain rate of speed. When it is not, or the gcs is standing still the heading is just random. We don't want these
|
||||
// random heading to be shown on the fly view. So until we can get a true compass based heading or some smarted heading quality
|
||||
// information which takes into account the speed of movement we normally don't set a heading. We do use the heading though
|
||||
// if the plugin overrides the position source. In that case we assume that it hopefully know what it is doing.
|
||||
if (_usingPluginSource && newGCSHeading != _gcsHeading) {
|
||||
_gcsHeading = newGCSHeading;
|
||||
emit gcsHeadingChanged(_gcsHeading);
|
||||
}
|
||||
|
||||
emit positionInfoUpdated(update);
|
||||
}
|
||||
|
||||
int QGCPositionManager::updateInterval() const
|
||||
{
|
||||
return _updateInterval;
|
||||
}
|
||||
|
||||
void QGCPositionManager::setPositionSource(QGCPositionManager::QGCPositionSource source)
|
||||
{
|
||||
if (_currentSource != nullptr) {
|
||||
_currentSource->stopUpdates();
|
||||
disconnect(_currentSource);
|
||||
|
||||
// Reset all values so we dont get stuck on old values
|
||||
_geoPositionInfo = QGeoPositionInfo();
|
||||
_gcsPosition = QGeoCoordinate();
|
||||
_gcsHeading = qQNaN();
|
||||
_gcsPositionHorizontalAccuracy = std::numeric_limits<qreal>::infinity();
|
||||
|
||||
emit gcsPositionChanged(_gcsPosition);
|
||||
emit gcsHeadingChanged(_gcsHeading);
|
||||
emit positionInfoUpdated(_geoPositionInfo);
|
||||
emit gcsPositionHorizontalAccuracyChanged();
|
||||
}
|
||||
|
||||
if (qgcApp()->runningUnitTests()) {
|
||||
// Units test on travis fail due to lack of position source
|
||||
return;
|
||||
}
|
||||
|
||||
switch(source) {
|
||||
case QGCPositionManager::Log:
|
||||
break;
|
||||
case QGCPositionManager::Simulated:
|
||||
_currentSource = _simulatedSource;
|
||||
break;
|
||||
case QGCPositionManager::NmeaGPS:
|
||||
_currentSource = _nmeaSource;
|
||||
break;
|
||||
case QGCPositionManager::InternalGPS:
|
||||
default:
|
||||
_currentSource = _defaultSource;
|
||||
break;
|
||||
}
|
||||
|
||||
if (_currentSource != nullptr) {
|
||||
_updateInterval = _currentSource->minimumUpdateInterval();
|
||||
_currentSource->setPreferredPositioningMethods(QGeoPositionInfoSource::SatellitePositioningMethods);
|
||||
_currentSource->setUpdateInterval(_updateInterval);
|
||||
connect(_currentSource, &QGeoPositionInfoSource::positionUpdated, this, &QGCPositionManager::_positionUpdated);
|
||||
connect(_currentSource, &QGeoPositionInfoSource::errorOccurred, this, &QGCPositionManager::_error);
|
||||
_currentSource->startUpdates();
|
||||
}
|
||||
}
|
||||
|
||||
void QGCPositionManager::_error(QGeoPositionInfoSource::Error positioningError)
|
||||
{
|
||||
qWarning() << "QGCPositionManager error" << positioningError;
|
||||
}
|
||||
@ -0,0 +1,96 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <QtCore>
|
||||
#include <QDateTime>
|
||||
#include <QDate>
|
||||
|
||||
#include "SimulatedPosition.h"
|
||||
#include "QGCApplication.h"
|
||||
#include "MultiVehicleManager.h"
|
||||
|
||||
SimulatedPosition::SimulatedPosition()
|
||||
: QGeoPositionInfoSource(nullptr)
|
||||
{
|
||||
_updateTimer.setSingleShot(false);
|
||||
|
||||
// Initialize position to normal PX4 Gazebo home position
|
||||
_lastPosition.setTimestamp(QDateTime::currentDateTime());
|
||||
_lastPosition.setCoordinate(QGeoCoordinate(47.3977420, 8.5455941, 488));
|
||||
_lastPosition.setAttribute(QGeoPositionInfo::Attribute::Direction, _heading);
|
||||
_lastPosition.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, _horizontalVelocityMetersPerSec);
|
||||
_lastPosition.setAttribute(QGeoPositionInfo::Attribute::VerticalSpeed, _verticalVelocityMetersPerSec);
|
||||
|
||||
// When a vehicle shows up we switch location to the vehicle home position
|
||||
connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &SimulatedPosition::_vehicleAdded);
|
||||
|
||||
connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::_updatePosition);
|
||||
}
|
||||
|
||||
QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const
|
||||
{
|
||||
return _lastPosition;
|
||||
}
|
||||
|
||||
SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMethods() const
|
||||
{
|
||||
return AllPositioningMethods;
|
||||
}
|
||||
|
||||
void SimulatedPosition::startUpdates(void)
|
||||
{
|
||||
_updateTimer.start(qMax(updateInterval(), minimumUpdateInterval()));
|
||||
}
|
||||
|
||||
void SimulatedPosition::stopUpdates(void)
|
||||
{
|
||||
_updateTimer.stop();
|
||||
}
|
||||
|
||||
void SimulatedPosition::requestUpdate(int /*timeout*/)
|
||||
{
|
||||
emit errorOccurred(QGeoPositionInfoSource::UpdateTimeoutError);
|
||||
}
|
||||
|
||||
void SimulatedPosition::_updatePosition(void)
|
||||
{
|
||||
int intervalMsecs = _updateTimer.interval();
|
||||
|
||||
QGeoCoordinate coord = _lastPosition.coordinate();
|
||||
double horizontalDistance = _horizontalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
|
||||
double verticalDistance = _verticalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
|
||||
|
||||
_lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance));
|
||||
|
||||
emit positionUpdated(_lastPosition);
|
||||
}
|
||||
|
||||
QGeoPositionInfoSource::Error SimulatedPosition::error() const
|
||||
{
|
||||
return QGeoPositionInfoSource::NoError;
|
||||
}
|
||||
|
||||
void SimulatedPosition::_vehicleAdded(Vehicle* vehicle)
|
||||
{
|
||||
if (vehicle->homePosition().isValid()) {
|
||||
_lastPosition.setCoordinate(vehicle->homePosition());
|
||||
} else {
|
||||
connect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
|
||||
}
|
||||
}
|
||||
|
||||
void SimulatedPosition::_vehicleHomePositionChanged(QGeoCoordinate homePosition)
|
||||
{
|
||||
Vehicle* vehicle = qobject_cast<Vehicle*>(sender());
|
||||
|
||||
if (homePosition.isValid()) {
|
||||
_lastPosition.setCoordinate(homePosition);
|
||||
disconnect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,49 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QtPositioning/qgeopositioninfosource.h>
|
||||
#include "QGCToolbox.h"
|
||||
#include <QTimer>
|
||||
|
||||
class Vehicle;
|
||||
|
||||
class SimulatedPosition : public QGeoPositionInfoSource
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
SimulatedPosition();
|
||||
|
||||
QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const override;
|
||||
|
||||
PositioningMethods supportedPositioningMethods (void) const override;
|
||||
int minimumUpdateInterval (void) const override { return _updateIntervalMsecs; }
|
||||
Error error (void) const override;
|
||||
|
||||
public slots:
|
||||
void startUpdates (void) override;
|
||||
void stopUpdates (void) override;
|
||||
void requestUpdate (int timeout = 5000) override;
|
||||
|
||||
private slots:
|
||||
void _updatePosition (void);
|
||||
void _vehicleAdded (Vehicle* vehicle);
|
||||
void _vehicleHomePositionChanged (QGeoCoordinate homePosition);
|
||||
|
||||
private:
|
||||
QTimer _updateTimer;
|
||||
QGeoPositionInfo _lastPosition;
|
||||
|
||||
static constexpr int _updateIntervalMsecs = 1000;
|
||||
static constexpr double _horizontalVelocityMetersPerSec = 0.5;
|
||||
static constexpr double _verticalVelocityMetersPerSec = 0.1;
|
||||
static constexpr double _heading = 45;
|
||||
};
|
||||
Loading…
Reference in new issue