From 3b0f1f871867883d4175328fcfc726bc7bfd32e1 Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Tue, 11 Jun 2024 15:07:22 +0800 Subject: [PATCH] add --- src/Joystick/CMakeLists.txt | 40 + src/Joystick/Joystick.cc | 1171 ++++++++++++++++++++++ src/Joystick/Joystick.h | 370 +++++++ src/Joystick/JoystickAndroid.cc | 293 ++++++ src/Joystick/JoystickAndroid.h | 54 + src/Joystick/JoystickManager.cc | 226 +++++ src/Joystick/JoystickManager.h | 82 ++ src/Joystick/JoystickMavCommand.cc | 101 ++ src/Joystick/JoystickMavCommand.h | 40 + src/Joystick/JoystickMavCommands.json | 17 + src/Joystick/JoystickSDL.cc | 178 ++++ src/Joystick/JoystickSDL.h | 53 + src/PositionManager/CMakeLists.txt | 16 + src/PositionManager/PositionManager.cpp | 202 ++++ src/PositionManager/PositionManager.h | 74 ++ src/PositionManager/SimulatedPosition.cc | 96 ++ src/PositionManager/SimulatedPosition.h | 49 + 17 files changed, 3062 insertions(+) create mode 100644 src/Joystick/CMakeLists.txt create mode 100644 src/Joystick/Joystick.cc create mode 100644 src/Joystick/Joystick.h create mode 100644 src/Joystick/JoystickAndroid.cc create mode 100644 src/Joystick/JoystickAndroid.h create mode 100644 src/Joystick/JoystickManager.cc create mode 100644 src/Joystick/JoystickManager.h create mode 100644 src/Joystick/JoystickMavCommand.cc create mode 100644 src/Joystick/JoystickMavCommand.h create mode 100644 src/Joystick/JoystickMavCommands.json create mode 100644 src/Joystick/JoystickSDL.cc create mode 100644 src/Joystick/JoystickSDL.h create mode 100644 src/PositionManager/CMakeLists.txt create mode 100644 src/PositionManager/PositionManager.cpp create mode 100644 src/PositionManager/PositionManager.h create mode 100644 src/PositionManager/SimulatedPosition.cc create mode 100644 src/PositionManager/SimulatedPosition.h diff --git a/src/Joystick/CMakeLists.txt b/src/Joystick/CMakeLists.txt new file mode 100644 index 0000000..83b1065 --- /dev/null +++ b/src/Joystick/CMakeLists.txt @@ -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() diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc new file mode 100644 index 0000000..b9835c4 --- /dev/null +++ b/src/Joystick/Joystick.cc @@ -0,0 +1,1171 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include "Joystick.h" +#include "QGC.h" +#include "AutoPilotPlugin.h" +#include "UAS.h" +#include "QGCApplication.h" +#include "VideoManager.h" +#include "QGCCameraManager.h" +#include "QGCCameraControl.h" + +#include + +// JoystickLog Category declaration moved to QGCLoggingCategory.cc to allow access in Vehicle +QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") + +const char* Joystick::_settingsGroup = "Joysticks"; +const char* Joystick::_calibratedSettingsKey = "Calibrated4"; // Increment number to force recalibration +const char* Joystick::_buttonActionNameKey = "ButtonActionName%1"; +const char* Joystick::_buttonActionRepeatKey = "ButtonActionRepeat%1"; +const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; +const char* Joystick::_negativeThrustSettingsKey = "NegativeThrust"; +const char* Joystick::_exponentialSettingsKey = "Exponential"; +const char* Joystick::_accumulatorSettingsKey = "Accumulator"; +const char* Joystick::_deadbandSettingsKey = "Deadband"; +const char* Joystick::_circleCorrectionSettingsKey = "Circle_Correction"; +const char* Joystick::_axisFrequencySettingsKey = "AxisFrequency"; +const char* Joystick::_buttonFrequencySettingsKey = "ButtonFrequency"; +const char* Joystick::_txModeSettingsKey = nullptr; +const char* Joystick::_fixedWingTXModeSettingsKey = "TXMode_FixedWing"; +const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; +const char* Joystick::_roverTXModeSettingsKey = "TXMode_Rover"; +const char* Joystick::_vtolTXModeSettingsKey = "TXMode_VTOL"; +const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine"; + +const char* Joystick::_buttonActionNone = QT_TR_NOOP("No Action"); +const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm"); +const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm"); +const char* Joystick::_buttonActionToggleArm = QT_TR_NOOP("Toggle Arm"); +const char* Joystick::_buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing"); +const char* Joystick::_buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor"); +const char* Joystick::_buttonActionContinuousZoomIn = QT_TR_NOOP("Continuous Zoom In"); +const char* Joystick::_buttonActionContinuousZoomOut = QT_TR_NOOP("Continuous Zoom Out"); +const char* Joystick::_buttonActionStepZoomIn = QT_TR_NOOP("Step Zoom In"); +const char* Joystick::_buttonActionStepZoomOut = QT_TR_NOOP("Step Zoom Out"); +const char* Joystick::_buttonActionNextStream = QT_TR_NOOP("Next Video Stream"); +const char* Joystick::_buttonActionPreviousStream = QT_TR_NOOP("Previous Video Stream"); +const char* Joystick::_buttonActionNextCamera = QT_TR_NOOP("Next Camera"); +const char* Joystick::_buttonActionPreviousCamera = QT_TR_NOOP("Previous Camera"); +const char* Joystick::_buttonActionTriggerCamera = QT_TR_NOOP("Trigger Camera"); +const char* Joystick::_buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video"); +const char* Joystick::_buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video"); +const char* Joystick::_buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video"); +const char* Joystick::_buttonActionGimbalDown = QT_TR_NOOP("Gimbal Down"); +const char* Joystick::_buttonActionGimbalUp = QT_TR_NOOP("Gimbal Up"); +const char* Joystick::_buttonActionGimbalLeft = QT_TR_NOOP("Gimbal Left"); +const char* Joystick::_buttonActionGimbalRight = QT_TR_NOOP("Gimbal Right"); +const char* Joystick::_buttonActionGimbalCenter = QT_TR_NOOP("Gimbal Center"); +const char* Joystick::_buttonActionEmergencyStop = QT_TR_NOOP("Emergency Stop"); +const char* Joystick::_buttonActionGripperGrab = QT_TR_NOOP("Gripper Close"); +const char* Joystick::_buttonActionGripperRelease = QT_TR_NOOP("Gripper Open"); + +const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { + "RollAxis", + "PitchAxis", + "YawAxis", + "ThrottleAxis", + "GimbalPitchAxis", + "GimbalYawAxis" +}; + +int Joystick::_transmitterMode = 2; + +const float Joystick::_defaultAxisFrequencyHz = 25.0f; +const float Joystick::_defaultButtonFrequencyHz = 5.0f; +const float Joystick::_minAxisFrequencyHz = 0.25f; +const float Joystick::_maxAxisFrequencyHz = 200.0f; +const float Joystick::_minButtonFrequencyHz = 0.25f; +const float Joystick::_maxButtonFrequencyHz = 50.0f; + +AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString name) + : QObject(parent) + , action(name) +{ +} + +AssignableButtonAction::AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_) + : QObject(parent) + , _action(action_) + , _repeat(canRepeat_) +{ +} + +Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager) + : _name(name) + , _axisCount(axisCount) + , _buttonCount(buttonCount) + , _hatCount(hatCount) + , _hatButtonCount(4 * hatCount) + , _totalButtonCount(_buttonCount+_hatButtonCount) + , _multiVehicleManager(multiVehicleManager) +{ + qRegisterMetaType(); + + _rgAxisValues = new int[static_cast(_axisCount)]; + _rgCalibration = new Calibration_t[static_cast(_axisCount)]; + _rgButtonValues = new uint8_t[static_cast(_totalButtonCount)]; + for (int i = 0; i < _axisCount; i++) { + _rgAxisValues[i] = 0; + } + for (int i = 0; i < _totalButtonCount; i++) { + _rgButtonValues[i] = BUTTON_UP; + _buttonActionArray.append(nullptr); + } + _buildActionList(_multiVehicleManager->activeVehicle()); + _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle()); + _loadSettings(); + connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged); + connect(qgcApp()->toolbox()->multiVehicleManager()->vehicles(), &QmlObjectListModel::countChanged, this, &Joystick::_vehicleCountChanged); + + _customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json"); +} + +void Joystick::stop() +{ + _exitThread = true; + wait(); +} + +Joystick::~Joystick() +{ + if (!_exitThread) { + qWarning() << "Joystick thread still running!"; + } + delete[] _rgAxisValues; + delete[] _rgCalibration; + delete[] _rgButtonValues; + _assignableButtonActions.clearAndDeleteContents(); + for (int button = 0; button < _totalButtonCount; button++) { + if(_buttonActionArray[button]) { + _buttonActionArray[button]->deleteLater(); + } + } +} + +void Joystick::_setDefaultCalibration(void) { + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); + + // Only set default calibrations if we do not have a calibration for this gamecontroller + if(_calibrated) return; + + for(int axis = 0; axis < _axisCount; axis++) { + Joystick::Calibration_t calibration; + _rgCalibration[axis] = calibration; + } + + _rgCalibration[1].reversed = true; + _rgCalibration[3].reversed = true; + + // Default TX Mode 2 axis assignments for gamecontrollers + _rgFunctionAxis[rollFunction] = 2; + _rgFunctionAxis[pitchFunction] = 3; + _rgFunctionAxis[yawFunction] = 0; + _rgFunctionAxis[throttleFunction] = 1; + + _rgFunctionAxis[gimbalPitchFunction]= 4; + _rgFunctionAxis[gimbalYawFunction] = 5; + + _exponential = 0; + _accumulator = false; + _deadband = false; + _axisFrequencyHz = _defaultAxisFrequencyHz; + _buttonFrequencyHz = _defaultButtonFrequencyHz; + _throttleMode = ThrottleModeDownZero; + _calibrated = true; + _circleCorrection = false; + + _saveSettings(); +} + +void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle) +{ + if(activeVehicle) { + if(activeVehicle->fixedWing()) { + _txModeSettingsKey = _fixedWingTXModeSettingsKey; + } else if(activeVehicle->multiRotor()) { + _txModeSettingsKey = _multiRotorTXModeSettingsKey; + } else if(activeVehicle->rover()) { + _txModeSettingsKey = _roverTXModeSettingsKey; + } else if(activeVehicle->vtol()) { + _txModeSettingsKey = _vtolTXModeSettingsKey; + } else if(activeVehicle->sub()) { + _txModeSettingsKey = _submarineTXModeSettingsKey; + } else { + _txModeSettingsKey = nullptr; + qWarning() << "No valid joystick TXmode settings key for selected vehicle"; + return; + } + } else { + _txModeSettingsKey = nullptr; + } +} + +void Joystick::_activeVehicleChanged(Vehicle* activeVehicle) +{ + _updateTXModeSettingsKey(activeVehicle); + if(activeVehicle) { + QSettings settings; + settings.beginGroup(_settingsGroup); + int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); + setTXMode(mode); + } +} +void Joystick::_flightModesChanged() +{ + _buildActionList(_activeVehicle); +} + +void Joystick::_vehicleCountChanged(int count) +{ + if(count == 0) + { + // then the last vehicle has been deleted + qCDebug(JoystickLog) << "Stopping joystick thread due to last active vehicle deletion"; + this->stopPolling(); + } +} + +void Joystick::_loadSettings() +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + Vehicle* activeVehicle = _multiVehicleManager->activeVehicle(); + + if(_txModeSettingsKey && activeVehicle) + _transmitterMode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); + + settings.beginGroup(_name); + + bool badSettings = false; + bool convertOk; + + qCDebug(JoystickLog) << "_loadSettings " << _name; + + _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); + _exponential = settings.value(_exponentialSettingsKey, 0).toFloat(); + _accumulator = settings.value(_accumulatorSettingsKey, false).toBool(); + _deadband = settings.value(_deadbandSettingsKey, false).toBool(); + _axisFrequencyHz = settings.value(_axisFrequencySettingsKey, _defaultAxisFrequencyHz).toFloat(); + _buttonFrequencyHz = settings.value(_buttonFrequencySettingsKey, _defaultButtonFrequencyHz).toFloat(); + _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); + _negativeThrust = settings.value(_negativeThrustSettingsKey, false).toBool(); + + + _throttleMode = static_cast(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk)); + badSettings |= !convertOk; + + qCDebug(JoystickLog) << "_loadSettings calibrated:txmode:throttlemode:exponential:deadband:badsettings" << _calibrated << _transmitterMode << _throttleMode << _exponential << _deadband << badSettings; + + QString minTpl ("Axis%1Min"); + QString maxTpl ("Axis%1Max"); + QString trimTpl ("Axis%1Trim"); + QString revTpl ("Axis%1Rev"); + QString deadbndTpl ("Axis%1Deadbnd"); + + for (int axis = 0; axis < _axisCount; axis++) { + Calibration_t* calibration = &_rgCalibration[axis]; + + calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->min = settings.value(minTpl.arg(axis), -32768).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->max = settings.value(maxTpl.arg(axis), 32767).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->deadband = settings.value(deadbndTpl.arg(axis), 0).toInt(&convertOk); + badSettings |= !convertOk; + + calibration->reversed = settings.value(revTpl.arg(axis), false).toBool(); + + qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings; + } + + int workingAxis = 0; + for (int function = 0; function < maxFunction; function++) { + int functionAxis; + functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk); + badSettings |= !convertOk || (functionAxis >= _axisCount); + if(functionAxis >= 0) { + workingAxis++; + } + if(functionAxis < _axisCount) { + _rgFunctionAxis[function] = functionAxis; + } + qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings; + } + badSettings |= workingAxis < 4; + + // FunctionAxis mappings are always stored in TX mode 2 + // Remap to stored TX mode in settings + _remapAxes(2, _transmitterMode, _rgFunctionAxis); + + for (int button = 0; button < _totalButtonCount; button++) { + QString a = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); + if(!a.isEmpty() && a != _buttonActionNone) { + if(_buttonActionArray[button]) { + _buttonActionArray[button]->deleteLater(); + } + AssignedButtonAction* ap = new AssignedButtonAction(this, a); + ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); + _buttonActionArray[button] = ap; + _buttonActionArray[button]->buttonTime.start(); + qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + } + } + + if (badSettings) { + _calibrated = false; + settings.setValue(_calibratedSettingsKey, false); + } +} + +void Joystick::_saveButtonSettings() +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + for (int button = 0; button < _totalButtonCount; button++) { + if(_buttonActionArray[button]) { + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + } + } +} + +void Joystick::_saveSettings() +{ + QSettings settings; + settings.beginGroup(_settingsGroup); + + // Transmitter mode is static + // Save the mode we are using + if(_txModeSettingsKey) + settings.setValue(_txModeSettingsKey, _transmitterMode); + + settings.beginGroup(_name); + settings.setValue(_calibratedSettingsKey, _calibrated); + settings.setValue(_exponentialSettingsKey, _exponential); + settings.setValue(_accumulatorSettingsKey, _accumulator); + settings.setValue(_deadbandSettingsKey, _deadband); + settings.setValue(_axisFrequencySettingsKey, _axisFrequencyHz); + settings.setValue(_buttonFrequencySettingsKey, _buttonFrequencyHz); + settings.setValue(_throttleModeSettingsKey, _throttleMode); + settings.setValue(_negativeThrustSettingsKey, _negativeThrust); + settings.setValue(_circleCorrectionSettingsKey, _circleCorrection); + + qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; + + QString minTpl ("Axis%1Min"); + QString maxTpl ("Axis%1Max"); + QString trimTpl ("Axis%1Trim"); + QString revTpl ("Axis%1Rev"); + QString deadbndTpl ("Axis%1Deadbnd"); + + for (int axis = 0; axis < _axisCount; axis++) { + Calibration_t* calibration = &_rgCalibration[axis]; + settings.setValue(trimTpl.arg(axis), calibration->center); + settings.setValue(minTpl.arg(axis), calibration->min); + settings.setValue(maxTpl.arg(axis), calibration->max); + settings.setValue(revTpl.arg(axis), calibration->reversed); + settings.setValue(deadbndTpl.arg(axis), calibration->deadband); + qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed:deadband" + << _name + << axis + << calibration->min + << calibration->max + << calibration->center + << calibration->reversed + << calibration->deadband; + } + + // Always save function Axis mappings in TX Mode 2 + // Write mode 2 mappings without changing mapping currently in use + int temp[maxFunction]; + _remapAxes(_transmitterMode, 2, temp); + for (int function = 0; function < maxFunction; function++) { + settings.setValue(_rgFunctionSettingsKey[function], temp[function]); + qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function]; + } + _saveButtonSettings(); +} + +// Relative mappings of axis functions between different TX modes +int Joystick::_mapFunctionMode(int mode, int function) { + static const int mapping[][6] = { + { yawFunction, pitchFunction, rollFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction }, + { yawFunction, throttleFunction, rollFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction }, + { rollFunction, pitchFunction, yawFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction }, + { rollFunction, throttleFunction, yawFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction }}; + return mapping[mode-1][function]; +} + +// Remap current axis functions from current TX mode to new TX mode +void Joystick::_remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]) { + int temp[maxFunction]; + for(int function = 0; function < maxFunction; function++) { + temp[_mapFunctionMode(newMode, function)] = _rgFunctionAxis[_mapFunctionMode(currentMode, function)]; + } + for(int function = 0; function < maxFunction; function++) { + newMapping[function] = temp[function]; + } +} + +void Joystick::setTXMode(int mode) { + if(mode > 0 && mode <= 4) { + _remapAxes(_transmitterMode, mode, _rgFunctionAxis); + _transmitterMode = mode; + _saveSettings(); + } else { + qCWarning(JoystickLog) << "Invalid mode:" << mode; + } +} + +/// Adjust the raw axis value to the -1:1 range given calibration information +float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDeadbands) +{ + float valueNormalized; + float axisLength; + float axisBasis; + + if (value > calibration.center) { + axisBasis = 1.0f; + valueNormalized = value - calibration.center; + axisLength = calibration.max - calibration.center; + } else { + axisBasis = -1.0f; + valueNormalized = calibration.center - value; + axisLength = calibration.center - calibration.min; + } + + float axisPercent; + + if (withDeadbands) { + if (valueNormalized>calibration.deadband) { + axisPercent = (valueNormalized - calibration.deadband) / (axisLength - calibration.deadband); + } else if (valueNormalized<-calibration.deadband) { + axisPercent = (valueNormalized + calibration.deadband) / (axisLength - calibration.deadband); + } else { + axisPercent = 0.f; + } + } + else { + axisPercent = valueNormalized / axisLength; + } + + float correctedValue = axisBasis * axisPercent; + + if (calibration.reversed) { + correctedValue *= -1.0f; + } + +#if 0 + qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:deadband:basis:normalized:length" + << correctedValue + << value + << calibration.min + << calibration.max + << calibration.center + << calibration.reversed + << calibration.deadband + << axisBasis + << valueNormalized + << axisLength; +#endif + + return std::max(-1.0f, std::min(correctedValue, 1.0f)); +} + + +void Joystick::run() +{ + //-- Joystick thread + _open(); + //-- Reset timers + _axisTime.start(); + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { + if(_buttonActionArray[buttonIndex]) { + _buttonActionArray[buttonIndex]->buttonTime.start(); + } + } + while (!_exitThread) { + _update(); + _handleButtons(); + if (axisCount() != 0) { + _handleAxis(); + } + QGC::SLEEP::msleep(qMin(static_cast(1000.0f / _maxAxisFrequencyHz), static_cast(1000.0f / _maxButtonFrequencyHz)) / 2); + } + _close(); +} + +void Joystick::_handleButtons() +{ + int lastBbuttonValues[256]; + //-- Update button states + for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) { + bool newButtonValue = _getButton(buttonIndex); + if(buttonIndex < 256) + lastBbuttonValues[buttonIndex] = _rgButtonValues[buttonIndex]; + if (newButtonValue && _rgButtonValues[buttonIndex] == BUTTON_UP) { + _rgButtonValues[buttonIndex] = BUTTON_DOWN; + emit rawButtonPressedChanged(buttonIndex, newButtonValue); + } else if (!newButtonValue && _rgButtonValues[buttonIndex] != BUTTON_UP) { + _rgButtonValues[buttonIndex] = BUTTON_UP; + emit rawButtonPressedChanged(buttonIndex, newButtonValue); + } + } + //-- Update hat - append hat buttons to the end of the normal button list + int numHatButtons = 4; + for (int hatIndex = 0; hatIndex < _hatCount; hatIndex++) { + for (int hatButtonIndex = 0; hatButtonIndexaction; + if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) + continue; + if(!_buttonActionArray[buttonIndex]->repeat) { + //-- This button just went down + if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) { + // Check for a multi-button action + QList rgButtons = { buttonIndex }; + bool executeButtonAction = true; + for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) { + if (multiIndex != buttonIndex) { + if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action == buttonAction) { + // We found a multi-button action + if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) { + // So far so good + rgButtons.append(multiIndex); + continue; + } else { + // We are missing a press we need + executeButtonAction = false; + break; + } + } + } + } + if (executeButtonAction) { + qCDebug(JoystickLog) << "Action triggered" << rgButtons << buttonAction; + _executeButtonAction(buttonAction, true); + } + } + } else { + //-- Process repeat buttons + int buttonDelay = static_cast(1000.0f / _buttonFrequencyHz); + if(_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) { + _buttonActionArray[buttonIndex]->buttonTime.start(); + qCDebug(JoystickLog) << "Repeat button triggered" << buttonIndex << buttonAction; + _executeButtonAction(buttonAction, true); + } + } + } + //-- Flag it as processed + _rgButtonValues[buttonIndex] = BUTTON_REPEAT; + } else if(_rgButtonValues[buttonIndex] == BUTTON_UP) { + //-- Button up transition + if(buttonIndex < 256) { + if(lastBbuttonValues[buttonIndex] == BUTTON_DOWN || lastBbuttonValues[buttonIndex] == BUTTON_REPEAT) { + if(_buttonActionArray[buttonIndex]) { + QString buttonAction = _buttonActionArray[buttonIndex]->action; + if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) + continue; + qCDebug(JoystickLog) << "Button up" << buttonIndex << buttonAction; + _executeButtonAction(buttonAction, false); + } + } + } + } + } +} + +void Joystick::_handleAxis() +{ + //-- Get frequency + int axisDelay = static_cast(1000.0f / _axisFrequencyHz); + //-- Check elapsed time since last run + if(_axisTime.elapsed() > axisDelay) { + _axisTime.start(); + //-- Update axis + for (int axisIndex = 0; axisIndex < _axisCount; 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); + } + if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) { + int axis = _rgFunctionAxis[rollFunction]; + float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + + axis = _rgFunctionAxis[pitchFunction]; + float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + + axis = _rgFunctionAxis[yawFunction]; + float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + + axis = _rgFunctionAxis[throttleFunction]; + float throttle = _adjustRange(_rgAxisValues[axis],_rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband); + + float gimbalPitch = 0.0f; + float gimbalYaw = 0.0f; + + if(_axisCount > 4) { + axis = _rgFunctionAxis[gimbalPitchFunction]; + gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } + + if(_axisCount > 5) { + axis = _rgFunctionAxis[gimbalYawFunction]; + gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } + + if (_accumulator) { + static float throttle_accu = 0.f; + throttle_accu += throttle * (40 / 1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time) + throttle_accu = std::max(static_cast(-1.f), std::min(throttle_accu, static_cast(1.f))); + throttle = throttle_accu; + } + + if (_circleCorrection) { + float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); + float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); + float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); + float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); + + // Map from unit circle to linear range and limit + roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); + pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); + yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); + throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); + } + + if ( _exponential < -0.01f) { + // Exponential (0% to -50% range like most RC radios) + // _exponential is set by a slider in joystickConfigAdvanced.qml + // Calculate new RPY with exponential applied + roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll; + pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch; + yaw = -_exponential*powf(yaw, 3) + (1+_exponential)*yaw; + } + + // Adjust throttle to 0:1 range + if (_throttleMode == ThrottleModeCenterZero && _activeVehicle->supportsThrottleModeCenterZero()) { + if (!_activeVehicle->supportsNegativeThrust() || !_negativeThrust) { + throttle = std::max(0.0f, throttle); + } + } else { + throttle = (throttle + 1.0f) / 2.0f; + } + qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw; + // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits) + // Set up button bitmap + quint64 buttonPressedBits = 0; // Buttons pressed for manualControl signal + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { + quint64 buttonBit = static_cast(1LL << buttonIndex); + if (_rgButtonValues[buttonIndex] != BUTTON_UP) { + // Mark the button as pressed as long as its pressed + buttonPressedBits |= buttonBit; + } + } + emit axisValues(roll, pitch, yaw, throttle); + + uint16_t shortButtons = static_cast(buttonPressedBits & 0xFFFF); + _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, shortButtons); + } + } +} + +void Joystick::startPolling(Vehicle* vehicle) +{ + if (vehicle) { + // If a vehicle is connected, disconnect it + if (_activeVehicle) { + disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); + disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + disconnect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); + disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + } + // Always set up the new vehicle + _activeVehicle = vehicle; + // If joystick is not calibrated, disable it + if ( axisCount() != 0 && !_calibrated ) { + vehicle->setJoystickEnabled(false); + } + // Update qml in case of joystick transition + emit calibratedChanged(_calibrated); + // Build action list + _buildActionList(vehicle); + // Only connect the new vehicle if it wants joystick data + if (vehicle->joystickEnabled()) { + _pollingStartedForCalibration = false; + connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + connect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + connect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + connect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); + connect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + connect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); + connect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + connect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + } + } + if (!isRunning()) { + _exitThread = false; + start(); + } +} + +void Joystick::stopPolling(void) +{ + if (isRunning()) { + if (_activeVehicle && _activeVehicle->joystickEnabled()) { + disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); + disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + } + _exitThread = true; + } +} + +void Joystick::setCalibration(int axis, Calibration_t& calibration) +{ + if (!_validAxis(axis)) { + return; + } + _calibrated = true; + _rgCalibration[axis] = calibration; + _saveSettings(); + emit calibratedChanged(_calibrated); +} + +Joystick::Calibration_t Joystick::getCalibration(int axis) +{ + if (!_validAxis(axis)) { + return Calibration_t(); + } + return _rgCalibration[axis]; +} + +void Joystick::setFunctionAxis(AxisFunction_t function, int axis) +{ + if (!_validAxis(axis)) { + return; + } + _calibrated = true; + _rgFunctionAxis[function] = axis; + _saveSettings(); + emit calibratedChanged(_calibrated); +} + +int Joystick::getFunctionAxis(AxisFunction_t function) +{ + if (static_cast(function) < 0 || function >= maxFunction) { + qCWarning(JoystickLog) << "Invalid function" << function; + } + return _rgFunctionAxis[function]; +} + +void Joystick::setButtonRepeat(int button, bool repeat) +{ + if (!_validButton(button) || !_buttonActionArray[button]) { + return; + } + _buttonActionArray[button]->repeat = repeat; + _buttonActionArray[button]->buttonTime.start(); + //-- Save to settings + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); +} + +bool Joystick::getButtonRepeat(int button) +{ + if (!_validButton(button) || !_buttonActionArray[button]) { + return false; + } + return _buttonActionArray[button]->repeat; +} + +void Joystick::setButtonAction(int button, const QString& action) +{ + if (!_validButton(button)) { + return; + } + qCWarning(JoystickLog) << "setButtonAction:" << button << action; + QSettings settings; + settings.beginGroup(_settingsGroup); + settings.beginGroup(_name); + if(action.isEmpty() || action == _buttonActionNone) { + if(_buttonActionArray[button]) { + _buttonActionArray[button]->deleteLater(); + _buttonActionArray[button] = nullptr; + //-- Clear from settings + settings.remove(QString(_buttonActionNameKey).arg(button)); + settings.remove(QString(_buttonActionRepeatKey).arg(button)); + } + } else { + if(!_buttonActionArray[button]) { + _buttonActionArray[button] = new AssignedButtonAction(this, action); + } else { + _buttonActionArray[button]->action = action; + //-- Make sure repeat is off if this action doesn't support repeats + int idx = _findAssignableButtonAction(action); + if(idx >= 0) { + AssignableButtonAction* p = qobject_cast(_assignableButtonActions[idx]); + if(!p->canRepeat()) { + _buttonActionArray[button]->repeat = false; + } + } + } + //-- Save to settings + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + } + emit buttonActionsChanged(); +} + +QString Joystick::getButtonAction(int button) +{ + if (_validButton(button)) { + if(_buttonActionArray[button]) { + return _buttonActionArray[button]->action; + } + } + return QString(_buttonActionNone); +} + +QStringList Joystick::buttonActions() +{ + QStringList list; + for (int button = 0; button < _totalButtonCount; button++) { + list << getButtonAction(button); + } + return list; +} + +int Joystick::throttleMode() +{ + return _throttleMode; +} + +void Joystick::setThrottleMode(int mode) +{ + if (mode < 0 || mode >= ThrottleModeMax) { + qCWarning(JoystickLog) << "Invalid throttle mode" << mode; + return; + } + _throttleMode = static_cast(mode); + if (_throttleMode == ThrottleModeDownZero) { + setAccumulator(false); + } + _saveSettings(); + emit throttleModeChanged(_throttleMode); +} + +bool Joystick::negativeThrust() const +{ + return _negativeThrust; +} + +void Joystick::setNegativeThrust(bool allowNegative) +{ + if (_negativeThrust == allowNegative) { + return; + } + _negativeThrust = allowNegative; + _saveSettings(); + emit negativeThrustChanged(_negativeThrust); +} + +float Joystick::exponential() const +{ + return _exponential; +} + +void Joystick::setExponential(float expo) +{ + _exponential = expo; + _saveSettings(); + emit exponentialChanged(_exponential); +} + +bool Joystick::accumulator() const +{ + return _accumulator; +} + +void Joystick::setAccumulator(bool accu) +{ + _accumulator = accu; + _saveSettings(); + emit accumulatorChanged(_accumulator); +} + +bool Joystick::deadband() const +{ + return _deadband; +} + +void Joystick::setDeadband(bool deadband) +{ + _deadband = deadband; + _saveSettings(); +} + +bool Joystick::circleCorrection() const +{ + return _circleCorrection; +} + +void Joystick::setCircleCorrection(bool circleCorrection) +{ + _circleCorrection = circleCorrection; + _saveSettings(); + emit circleCorrectionChanged(_circleCorrection); +} + +void Joystick::setAxisFrequency(float val) +{ + //-- Arbitrary limits + val = qMax(_minAxisFrequencyHz, val); + val = qMin(_maxAxisFrequencyHz, val); + _axisFrequencyHz = val; + _saveSettings(); + emit axisFrequencyHzChanged(); +} + +void Joystick::setButtonFrequency(float val) +{ + //-- Arbitrary limits + val = qMax(_minButtonFrequencyHz, val); + val = qMin(_maxButtonFrequencyHz, val); + _buttonFrequencyHz = val; + _saveSettings(); + emit buttonFrequencyHzChanged(); +} + +void Joystick::setCalibrationMode(bool calibrating) +{ + _calibrationMode = calibrating; + if (calibrating && !isRunning()) { + _pollingStartedForCalibration = true; + startPolling(_multiVehicleManager->activeVehicle()); + } + else if (_pollingStartedForCalibration) { + stopPolling(); + } +} + + +void Joystick::_executeButtonAction(const QString& action, bool buttonDown) +{ + if (!_activeVehicle || !_activeVehicle->joystickEnabled() || action == _buttonActionNone) { + return; + } + if (action == _buttonActionArm) { + if (buttonDown) emit setArmed(true); + } else if (action == _buttonActionDisarm) { + if (buttonDown) emit setArmed(false); + } else if (action == _buttonActionToggleArm) { + if (buttonDown) emit setArmed(!_activeVehicle->armed()); + } else if (action == _buttonActionVTOLFixedWing) { + if (buttonDown) emit setVtolInFwdFlight(true); + } else if (action == _buttonActionVTOLMultiRotor) { + if (buttonDown) emit setVtolInFwdFlight(false); + } else if (_activeVehicle->flightModes().contains(action)) { + if (buttonDown) emit setFlightMode(action); + } else if(action == _buttonActionContinuousZoomIn || action == _buttonActionContinuousZoomOut) { + if (buttonDown) { + emit startContinuousZoom(action == _buttonActionContinuousZoomIn ? 1 : -1); + } else { + emit stopContinuousZoom(); + } + } else if(action == _buttonActionStepZoomIn || action == _buttonActionStepZoomOut) { + if (buttonDown) emit stepZoom(action == _buttonActionStepZoomIn ? 1 : -1); + } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) { + if (buttonDown) emit stepStream(action == _buttonActionNextStream ? 1 : -1); + } else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) { + if (buttonDown) emit stepCamera(action == _buttonActionNextCamera ? 1 : -1); + } else if(action == _buttonActionTriggerCamera) { + if (buttonDown) emit triggerCamera(); + } else if(action == _buttonActionStartVideoRecord) { + if (buttonDown) emit startVideoRecord(); + } else if(action == _buttonActionStopVideoRecord) { + if (buttonDown) emit stopVideoRecord(); + } else if(action == _buttonActionToggleVideoRecord) { + if (buttonDown) emit toggleVideoRecord(); + } else if(action == _buttonActionGimbalUp) { + if (buttonDown) _pitchStep(1); + } else if(action == _buttonActionGimbalDown) { + if (buttonDown) _pitchStep(-1); + } else if(action == _buttonActionGimbalLeft) { + if (buttonDown) _yawStep(-1); + } else if(action == _buttonActionGimbalRight) { + if (buttonDown) _yawStep(1); + } else if(action == _buttonActionGimbalCenter) { + if (buttonDown) { + _localPitch = 0.0; + _localYaw = 0.0; + emit gimbalControlValue(0.0, 0.0); + } + } else if(action == _buttonActionEmergencyStop) { + if(buttonDown) emit emergencyStop(); + } else if(action == _buttonActionGripperGrab) { + if(buttonDown) { + emit gripperAction(GRIPPER_ACTION_GRAB); + } + } else if(action == _buttonActionGripperRelease) { + if(buttonDown) { + emit gripperAction(GRIPPER_ACTION_RELEASE); + } + } else { + if (buttonDown && _activeVehicle) { + for (auto& item : _customMavCommands) { + if (action == item.name()) { + item.send(_activeVehicle); + return; + } + } + } + } +} + +void Joystick::_pitchStep(int direction) +{ + _localPitch += static_cast(direction); + //-- Arbitrary range + if(_localPitch < -90.0) _localPitch = -90.0; + if(_localPitch > 35.0) _localPitch = 35.0; + emit gimbalControlValue(_localPitch, _localYaw); +} + +void Joystick::_yawStep(int direction) +{ + _localYaw += static_cast(direction); + if(_localYaw < -180.0) _localYaw = -180.0; + if(_localYaw > 180.0) _localYaw = 180.0; + emit gimbalControlValue(_localPitch, _localYaw); +} + +bool Joystick::_validAxis(int axis) const +{ + if(axis >= 0 && axis < _axisCount) { + return true; + } + qCWarning(JoystickLog) << "Invalid axis index" << axis; + return false; +} + +bool Joystick::_validButton(int button) const +{ + if(button >= 0 && button < _totalButtonCount) + return true; + qCWarning(JoystickLog) << "Invalid button index" << button; + return false; +} + +int Joystick::_findAssignableButtonAction(const QString& action) +{ + for(int i = 0; i < _assignableButtonActions.count(); i++) { + AssignableButtonAction* p = qobject_cast(_assignableButtonActions[i]); + if(p->action() == action) + return i; + } + return -1; +} + +void Joystick::_buildActionList(Vehicle* activeVehicle) +{ + if(_assignableButtonActions.count()) + _assignableButtonActions.clearAndDeleteContents(); + _availableActionTitles.clear(); + //-- Available Actions + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNone)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionArm)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionDisarm)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleArm)); + if (activeVehicle) { + QStringList list = activeVehicle->flightModes(); + foreach(auto mode, list) { + _assignableButtonActions.append(new AssignableButtonAction(this, mode)); + } + } + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLFixedWing)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLMultiRotor)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomIn, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomOut, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomIn, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomOut, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextStream)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousStream)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextCamera)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousCamera)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionTriggerCamera)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStartVideoRecord)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStopVideoRecord)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleVideoRecord)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalDown, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalUp, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalLeft, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalRight, true)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalCenter)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease)); + + for (auto& item : _customMavCommands) { + _assignableButtonActions.append(new AssignableButtonAction(this, item.name())); + } + + for(int i = 0; i < _assignableButtonActions.count(); i++) { + AssignableButtonAction* p = qobject_cast(_assignableButtonActions[i]); + _availableActionTitles << p->action(); + } + emit assignableActionsChanged(); +} diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h new file mode 100644 index 0000000..7405630 --- /dev/null +++ b/src/Joystick/Joystick.h @@ -0,0 +1,370 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 +#include +#include + +#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 _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 _buttonActionArray; + QStringList _availableActionTitles; + MultiVehicleManager* _multiVehicleManager = nullptr; + + QList _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(); +}; diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc new file mode 100644 index 0000000..3994864 --- /dev/null +++ b/src/Joystick/JoystickAndroid.cc @@ -0,0 +1,293 @@ +#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; + +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(); + 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"); + // 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 JoystickAndroid::discover(MultiVehicleManager* _multiVehicleManager) { + static QMap ret; + + 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"); + + QList 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("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("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); + } + + 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("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] = static_cast((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("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"); + + 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(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(); +} diff --git a/src/Joystick/JoystickAndroid.h b/src/Joystick/JoystickAndroid.h new file mode 100644 index 0000000..2ff473c --- /dev/null +++ b/src/Joystick/JoystickAndroid.h @@ -0,0 +1,54 @@ +#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 bool init(JoystickManager *manager); + + static void setNativeMethods(); + + 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); + 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 diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc new file mode 100644 index 0000000..bd03f15 --- /dev/null +++ b/src/Joystick/JoystickManager.cc @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 + +#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::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 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::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); +} diff --git a/src/Joystick/JoystickManager.h b/src/Joystick/JoystickManager.h new file mode 100644 index 0000000..6614501 --- /dev/null +++ b/src/Joystick/JoystickManager.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 + +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 _name2JoystickMap; + MultiVehicleManager* _multiVehicleManager; + + static const char * _settingsGroup; + static const char * _settingsKeyActiveJoystick; + + int _joystickCheckTimerCounter; + QTimer _joystickCheckTimer; +}; diff --git a/src/Joystick/JoystickMavCommand.cc b/src/Joystick/JoystickMavCommand.cc new file mode 100644 index 0000000..66e72f4 --- /dev/null +++ b/src/Joystick/JoystickMavCommand.cc @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 +#include +#include + +QGC_LOGGING_CATEGORY(JoystickMavCommandLog, "JoystickMavCommandLog") + +static void parseJsonValue(const QJsonObject& jsonObject, const QString& key, float& param) +{ + if (jsonObject.contains(key)) + param = static_cast(jsonObject.value(key).toDouble()); +} + +QList JoystickMavCommand::load(const QString& jsonFilename) +{ + qCDebug(JoystickMavCommandLog) << "Loading" << jsonFilename; + QList 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(_id), + _showError, + _param1, _param2, _param3, _param4, _param5, _param6, _param7); +} diff --git a/src/Joystick/JoystickMavCommand.h b/src/Joystick/JoystickMavCommand.h new file mode 100644 index 0000000..96cf9f4 --- /dev/null +++ b/src/Joystick/JoystickMavCommand.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 +#include + +class Vehicle; + +/// Custom MAV command +class JoystickMavCommand +{ +public: + static QList 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; +}; + diff --git a/src/Joystick/JoystickMavCommands.json b/src/Joystick/JoystickMavCommands.json new file mode 100644 index 0000000..ba28ed4 --- /dev/null +++ b/src/Joystick/JoystickMavCommands.json @@ -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 + } + ] +} diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc new file mode 100644 index 0000000..4743e38 --- /dev/null +++ b/src/Joystick/JoystickSDL.cc @@ -0,0 +1,178 @@ +#include "JoystickSDL.h" + +#include "QGCApplication.h" + +#include +#include + +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 JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) { + static QMap ret; + + QMap newRet; + + // Load available joysticks + + qCDebug(JoystickLog) << "Available joysticks"; + + for (int i=0; i(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; +} diff --git a/src/Joystick/JoystickSDL.h b/src/Joystick/JoystickSDL.h new file mode 100644 index 0000000..b03b988 --- /dev/null +++ b/src/Joystick/JoystickSDL.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 + +/// @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 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 + +}; diff --git a/src/PositionManager/CMakeLists.txt b/src/PositionManager/CMakeLists.txt new file mode 100644 index 0000000..b693aa5 --- /dev/null +++ b/src/PositionManager/CMakeLists.txt @@ -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} + ) + diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp new file mode 100644 index 0000000..c668534 --- /dev/null +++ b/src/PositionManager/PositionManager.cpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 +#endif + +#include + +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 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::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; +} diff --git a/src/PositionManager/PositionManager.h b/src/PositionManager/PositionManager.h new file mode 100644 index 0000000..5654640 --- /dev/null +++ b/src/PositionManager/PositionManager.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +#include "QGCToolbox.h" +#include "SimulatedPosition.h" + +class QGCPositionManager : public QGCTool { + Q_OBJECT + +public: + static constexpr size_t MinHorizonalAccuracyMeters = 100; + QGCPositionManager(QGCApplication* app, QGCToolbox* toolbox); + ~QGCPositionManager(); + + Q_PROPERTY(QGeoCoordinate gcsPosition READ gcsPosition NOTIFY gcsPositionChanged) + Q_PROPERTY(qreal gcsHeading READ gcsHeading NOTIFY gcsHeadingChanged) + Q_PROPERTY(qreal gcsPositionHorizontalAccuracy READ gcsPositionHorizontalAccuracy + NOTIFY gcsPositionHorizontalAccuracyChanged) + + enum QGCPositionSource { + Simulated, + InternalGPS, + Log, + NmeaGPS + }; + + QGeoCoordinate gcsPosition (void) { return _gcsPosition; } + qreal gcsHeading (void) const{ return _gcsHeading; } + qreal gcsPositionHorizontalAccuracy(void) const { return _gcsPositionHorizontalAccuracy; } + QGeoPositionInfo geoPositionInfo (void) const { return _geoPositionInfo; } + void setPositionSource (QGCPositionSource source); + int updateInterval (void) const; + void setNmeaSourceDevice (QIODevice* device); + + // Overrides from QGCTool + void setToolbox(QGCToolbox* toolbox) override; + + +private slots: + void _positionUpdated(const QGeoPositionInfo &update); + void _error(QGeoPositionInfoSource::Error positioningError); + +signals: + void gcsPositionChanged(QGeoCoordinate gcsPosition); + void gcsHeadingChanged(qreal gcsHeading); + void positionInfoUpdated(QGeoPositionInfo update); + void gcsPositionHorizontalAccuracyChanged(); + +private: + int _updateInterval = 0; + QGeoPositionInfo _geoPositionInfo; + QGeoCoordinate _gcsPosition; + qreal _gcsHeading = qQNaN(); + qreal _gcsPositionHorizontalAccuracy = std::numeric_limits::infinity(); + + QGeoPositionInfoSource* _currentSource = nullptr; + QGeoPositionInfoSource* _defaultSource = nullptr; + QNmeaPositionInfoSource* _nmeaSource = nullptr; + QGeoPositionInfoSource* _simulatedSource = nullptr; + bool _usingPluginSource = false; +}; diff --git a/src/PositionManager/SimulatedPosition.cc b/src/PositionManager/SimulatedPosition.cc new file mode 100644 index 0000000..3ff6e0d --- /dev/null +++ b/src/PositionManager/SimulatedPosition.cc @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include +#include +#include + +#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(intervalMsecs)); + double verticalDistance = _verticalVelocityMetersPerSec * (1000.0 / static_cast(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(sender()); + + if (homePosition.isValid()) { + _lastPosition.setCoordinate(homePosition); + disconnect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged); + } +} diff --git a/src/PositionManager/SimulatedPosition.h b/src/PositionManager/SimulatedPosition.h new file mode 100644 index 0000000..357dd69 --- /dev/null +++ b/src/PositionManager/SimulatedPosition.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include "QGCToolbox.h" +#include + +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; +};