From 40c465b17f29a3bbfa6a5a4f7bef9b5171b96015 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 21 Oct 2015 09:42:46 -0700 Subject: [PATCH] Dynamic axis and button count support --- src/Joystick/Joystick.cc | 47 +++++++--- src/Joystick/Joystick.h | 23 +++-- src/VehicleSetup/JoystickConfig.qml | 3 +- src/VehicleSetup/JoystickConfigController.cc | 91 +++++++++++++------- src/VehicleSetup/JoystickConfigController.h | 25 +++--- 5 files changed, 121 insertions(+), 68 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 2fbd239b6..e64e20350 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -60,6 +60,10 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlI , _axisCount(axisCount) , _buttonCount(buttonCount) , _calibrationMode(CalibrationModeOff) + , _rgAxisValues(NULL) + , _rgCalibration(NULL) + , _rgButtonValues(NULL) + , _rgButtonActions(NULL) , _lastButtonBits(0) , _throttleMode(ThrottleModeCenterZero) , _activeVehicle(NULL) @@ -72,12 +76,16 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlI Q_UNUSED(buttonCount) Q_UNUSED(sdlIndex) #else - for (int i=0; i<_cAxes; i++) { + _rgAxisValues = new int[_axisCount]; + _rgCalibration = new Calibration_t[_axisCount]; + _rgButtonValues = new bool[_buttonCount]; + _rgButtonActions = new QString[_buttonCount]; + + for (int i=0; i<_axisCount; i++) { _rgAxisValues[i] = 0; } - for (int i=0; i<_cButtons; i++) { + for (int i=0; i<_buttonCount; i++) { _rgButtonValues[i] = false; - _rgButtonActions[i] = -1; } _loadSettings(); @@ -86,7 +94,10 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlI Joystick::~Joystick() { - + delete _rgAxisValues; + delete _rgCalibration; + delete _rgButtonValues; + delete _rgButtonActions; } #ifndef __mobile__ @@ -115,7 +126,7 @@ void Joystick::_loadSettings(void) QString trimTpl ("Axis%1Trim"); QString revTpl ("Axis%1Rev"); - for (int axis=0; axis<_cAxes; axis++) { + for (int axis=0; axis<_axisCount; axis++) { Calibration_t* calibration = &_rgCalibration[axis]; calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk); @@ -143,7 +154,7 @@ void Joystick::_loadSettings(void) qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings; } - for (int button=0; button<_cButtons; button++) { + for (int button=0; button<_buttonCount; button++) { _rgButtonActions[button] = settings.value(QString(_buttonActionSettingsKey).arg(button), QString()).toString(); qCDebug(JoystickLog) << "_loadSettings button:action" << button << _rgButtonActions[button]; } @@ -171,7 +182,7 @@ void Joystick::_saveSettings(void) QString trimTpl ("Axis%1Trim"); QString revTpl ("Axis%1Rev"); - for (int axis=0; axis<_cAxes; axis++) { + for (int axis=0; axis<_axisCount; axis++) { Calibration_t* calibration = &_rgCalibration[axis]; settings.setValue(trimTpl.arg(axis), calibration->center); @@ -193,7 +204,7 @@ void Joystick::_saveSettings(void) qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function]; } - for (int button=0; button<_cButtons; button++) { + for (int button=0; button<_buttonCount; button++) { settings.setValue(QString(_buttonActionSettingsKey).arg(button), _rgButtonActions[button]); qCDebug(JoystickLog) << "_saveSettings button:action" << button << _rgButtonActions[button]; } @@ -398,7 +409,7 @@ void Joystick::stopPolling(void) void Joystick::setCalibration(int axis, Calibration_t& calibration) { - if (axis < 0 || axis > _cAxes) { + if (!_validAxis(axis)) { qCWarning(JoystickLog) << "Invalid axis index" << axis; return; } @@ -411,7 +422,7 @@ void Joystick::setCalibration(int axis, Calibration_t& calibration) Joystick::Calibration_t Joystick::getCalibration(int axis) { - if (axis < 0 || axis > _cAxes) { + if (!_validAxis(axis)) { qCWarning(JoystickLog) << "Invalid axis index" << axis; } @@ -420,7 +431,7 @@ Joystick::Calibration_t Joystick::getCalibration(int axis) void Joystick::setFunctionAxis(AxisFunction_t function, int axis) { - if (axis < 0 || axis > _cAxes) { + if (!_validAxis(axis)) { qCWarning(JoystickLog) << "Invalid axis index" << axis; return; } @@ -451,7 +462,7 @@ QStringList Joystick::actions(void) void Joystick::setButtonAction(int button, const QString& action) { - if (button < 0 || button > _cButtons) { + if (!_validButton(button)) { qCWarning(JoystickLog) << "Invalid button index" << button; return; } @@ -465,7 +476,7 @@ void Joystick::setButtonAction(int button, const QString& action) QString Joystick::getButtonAction(int button) { - if (button < 0 || button > _cButtons) { + if (!_validButton(button)) { qCWarning(JoystickLog) << "Invalid button index" << button; } @@ -543,4 +554,14 @@ void Joystick::_buttonAction(const QString& action) } } +bool Joystick::_validAxis(int axis) +{ + return axis >= 0 && axis < _axisCount; +} + +bool Joystick::_validButton(int button) +{ + return button >= 0 && button < _buttonCount; +} + #endif // __mobile__ diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 669d46e95..db125b4c7 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -67,8 +67,8 @@ public: Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) - Q_PROPERTY(int buttonCount MEMBER _buttonCount CONSTANT) - Q_PROPERTY(int axisCount MEMBER _axisCount CONSTANT) + Q_PROPERTY(int buttonCount READ buttonCount CONSTANT) + Q_PROPERTY(int axisCount READ axisCount CONSTANT) Q_PROPERTY(QStringList actions READ actions CONSTANT) @@ -77,6 +77,11 @@ public: Q_INVOKABLE QString getButtonAction(int button); Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) + + // Property accessors + + int axisCount(void) { return _axisCount; } + int buttonCount(void) { return _buttonCount; } /// Start the polling thread which will in turn emit joystick signals void startPolling(Vehicle* vehicle); @@ -136,7 +141,9 @@ private: void _loadSettings(void); float _adjustRange(int value, Calibration_t calibration); void _buttonAction(const QString& action); - + bool _validAxis(int axis); + bool _validButton(int button); + // Override from QThread virtual void run(void); @@ -152,14 +159,12 @@ private: CalibrationMode_t _calibrationMode; - static const int _cAxes = 4; - int _rgAxisValues[_cAxes]; - Calibration_t _rgCalibration[_cAxes]; + int* _rgAxisValues; + Calibration_t* _rgCalibration; int _rgFunctionAxis[maxFunction]; - static const int _cButtons = 12; - bool _rgButtonValues[_cButtons]; - QString _rgButtonActions[_cButtons]; + bool* _rgButtonValues; + QString* _rgButtonActions; quint16 _lastButtonBits; ThrottleMode_t _throttleMode; diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml index fdba90d6c..11f578f8e 100644 --- a/src/VehicleSetup/JoystickConfig.qml +++ b/src/VehicleSetup/JoystickConfig.qml @@ -67,7 +67,6 @@ QGCView { if (controllerCompleted) { controllerAndViewReady = true controller.start() - updateAxisCount() } } @@ -564,7 +563,7 @@ QGCView { Repeater { id: axisMonitorRepeater - model: controller.axisCount + model: _activeJoystick.axisCount width: parent.width Row { diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc index 0f4a9df6d..bede44e95 100644 --- a/src/VehicleSetup/JoystickConfigController.cc +++ b/src/VehicleSetup/JoystickConfigController.cc @@ -58,6 +58,9 @@ JoystickConfigController::JoystickConfigController(void) : _activeJoystick(NULL) , _currentStep(-1) , _axisCount(0) + , _rgAxisInfo(NULL) + , _axisValueSave(NULL) + , _axisRawValue(NULL) , _calState(calStateAxisWait) , _statusText(NULL) , _cancelButton(NULL) @@ -133,7 +136,7 @@ void JoystickConfigController::_setupCurrentState(void) _setHelpImage(state->image); - _stickDetectAxis = _axisMax; + _stickDetectAxis = _axisNoAxis; _stickDetectSettleStarted = false; _calSaveCurrentValues(); @@ -144,7 +147,7 @@ void JoystickConfigController::_setupCurrentState(void) void JoystickConfigController::_axisValueChanged(int axis, int value) { - if (axis >= 0 && axis < _axisMax) { + if (_validAxis(axis)) { // We always update raw values _axisRawValue[axis] = value; emit axisValueChanged(axis, _axisRawValue[axis]); @@ -175,7 +178,6 @@ void JoystickConfigController::_axisValueChanged(int axis, int value) // Track the axis count by keeping track of how many axes we see if (axis + 1 > (int)_axisCount) { _axisCount = axis + 1; - emit axisCountChanged(_axisCount); } } @@ -225,7 +227,7 @@ void JoystickConfigController::_saveAllTrims(void) { // We save all trims as the first step. At this point no axes are mapped but it should still // allow us to get good trims for the roll/pitch/yaw/throttle even though we don't know which - // axiss they are yet. AS we continue through the process the other axes will get their + // axis they are yet. As we continue through the process the other axes will get their // trims reset to correct values. for (int i=0; i<_axisCount; i++) { @@ -249,6 +251,11 @@ void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t fu bool JoystickConfigController::_stickSettleComplete(int axis, int value) { + if (!_validAxis(axis)) { + qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount; + return false; + } + // We are waiting for the stick to settle out to a max position if (abs(_stickDetectValue - value) > _calSettleDelta) { @@ -286,12 +293,17 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi { qCDebug(JoystickConfigControllerLog) << "_inputStickDetect function:axis:value" << function << axis << value; + if (!_validAxis(axis)) { + qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount; + return; + } + // If this axis is already used in a mapping we can't use it again if (_rgAxisInfo[axis].function != Joystick::maxFunction) { return; } - if (_stickDetectAxis == _axisMax) { + if (_stickDetectAxis == _axisNoAxis) { // We have not detected enough movement on a axis yet if (abs(_axisValueSave[axis] - value) > _calMoveDelta) { @@ -335,12 +347,17 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, { qCDebug(JoystickConfigControllerLog) << "_inputStickMin function:axis:value" << function << axis << value; + if (!_validAxis(axis)) { + qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount; + return; + } + // We only care about the axis mapped to the function we are working on if (_rgFunctionAxisMapping[function] != axis) { return; } - if (_stickDetectAxis == _axisMax) { + if (_stickDetectAxis == _axisNoAxis) { // Setup up to detect stick being pegged to extreme position if (_rgAxisInfo[axis].reversed) { if (value > _calCenterPoint + _calMoveDelta) { @@ -386,12 +403,17 @@ void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t functio { qCDebug(JoystickConfigControllerLog) << "_inputCenterWait function:axis:value" << function << axis << value; + if (!_validAxis(axis)) { + qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount; + return; + } + // We only care about the axis mapped to the function we are working on if (_rgFunctionAxisMapping[function] != axis) { return; } - if (_stickDetectAxis == _axisMax) { + if (_stickDetectAxis == _axisNoAxis) { // Sticks have not yet moved close enough to center if (abs(_calCenterPoint - value) < _calRoughCenterDelta) { @@ -412,7 +434,7 @@ void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t functio void JoystickConfigController::_resetInternalCalibrationValues(void) { // Set all raw axiss to not reversed and center point values - for (int i=0; i<_axisMax; i++) { + for (int i=0; i<_axisCount; i++) { struct AxisInfo* info = &_rgAxisInfo[i]; info->function = Joystick::maxFunction; info->reversed = false; @@ -423,7 +445,7 @@ void JoystickConfigController::_resetInternalCalibrationValues(void) // Initialize attitude function mapping to function axis not set for (size_t i=0; ifunction = Joystick::maxFunction; } for (size_t i=0; igetCalibration(axis); @@ -472,7 +494,7 @@ void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void) /// @brief Validates the current settings against the calibration rules resetting values as necessary. void JoystickConfigController::_validateCalibration(void) { - for (int chan = 0; chan<_axisMax; chan++) { + for (int chan = 0; chan<_axisCount; chan++) { struct AxisInfo* info = &_rgAxisInfo[chan]; if (chan < _axisCount) { @@ -521,7 +543,7 @@ void JoystickConfigController::_writeCalibration(void) _validateCalibration(); - for (int axis=0; axis<_axisMax; axis++) { + for (int axis=0; axis<_axisCount; axis++) { Joystick::Calibration_t calibration; struct AxisInfo* info = &_rgAxisInfo[axis]; @@ -546,8 +568,6 @@ void JoystickConfigController::_writeCalibration(void) /// @brief Starts the calibration process void JoystickConfigController::_startCalibration(void) { - Q_ASSERT(_axisCount >= _axisMinimum); - _activeJoystick->startCalibrationMode(Joystick::CalibrationModeCalibrating); _resetInternalCalibrationValues(); @@ -580,7 +600,7 @@ void JoystickConfigController::_stopCalibration(void) void JoystickConfigController::_calSaveCurrentValues(void) { qCDebug(JoystickConfigControllerLog) << "_calSaveCurrentValues"; - for (int i = 0; i < _axisMax; i++) { + for (int i = 0; i < _axisCount; i++) { _axisValueSave[i] = _axisRawValue[i]; } } @@ -623,7 +643,7 @@ int JoystickConfigController::axisCount(void) int JoystickConfigController::rollAxisValue(void) { - if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) { return _axisRawValue[Joystick::rollFunction]; } else { return 1500; @@ -632,7 +652,7 @@ int JoystickConfigController::rollAxisValue(void) int JoystickConfigController::pitchAxisValue(void) { - if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) { return _axisRawValue[Joystick::pitchFunction]; } else { return 1500; @@ -641,7 +661,7 @@ int JoystickConfigController::pitchAxisValue(void) int JoystickConfigController::yawAxisValue(void) { - if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) { return _axisRawValue[Joystick::yawFunction]; } else { return 1500; @@ -650,7 +670,7 @@ int JoystickConfigController::yawAxisValue(void) int JoystickConfigController::throttleAxisValue(void) { - if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) { return _axisRawValue[Joystick::throttleFunction]; } else { return 1500; @@ -659,27 +679,27 @@ int JoystickConfigController::throttleAxisValue(void) bool JoystickConfigController::rollAxisMapped(void) { - return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisMax; + return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis; } bool JoystickConfigController::pitchAxisMapped(void) { - return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisMax; + return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis; } bool JoystickConfigController::yawAxisMapped(void) { - return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisMax; + return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis; } bool JoystickConfigController::throttleAxisMapped(void) { - return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisMax; + return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis; } bool JoystickConfigController::rollAxisReversed(void) { - if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].reversed; } else { return false; @@ -688,7 +708,7 @@ bool JoystickConfigController::rollAxisReversed(void) bool JoystickConfigController::pitchAxisReversed(void) { - if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].reversed; } else { return false; @@ -697,7 +717,7 @@ bool JoystickConfigController::pitchAxisReversed(void) bool JoystickConfigController::yawAxisReversed(void) { - if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].reversed; } else { return false; @@ -706,7 +726,7 @@ bool JoystickConfigController::yawAxisReversed(void) bool JoystickConfigController::throttleAxisReversed(void) { - if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisMax) { + if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].reversed; } else { return false; @@ -733,6 +753,10 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) if (_activeJoystick) { joystickTransition = true; disconnect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged); + delete _rgAxisInfo; + delete _axisValueSave; + delete _axisRawValue; + _axisCount = 0; _activeJoystick = NULL; } @@ -742,6 +766,15 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) _stopCalibration(); } _activeJoystick->startCalibrationMode(Joystick::CalibrationModeMonitor); + _axisCount = _activeJoystick->axisCount(); + _rgAxisInfo = new struct AxisInfo[_axisCount]; + _axisValueSave = new int[_axisCount]; + _axisRawValue = new int[_axisCount]; connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged); } } + +bool JoystickConfigController::_validAxis(int axis) +{ + return axis >= 0 && axis < _axisCount; +} diff --git a/src/VehicleSetup/JoystickConfigController.h b/src/VehicleSetup/JoystickConfigController.h index 030fb363a..faada2399 100644 --- a/src/VehicleSetup/JoystickConfigController.h +++ b/src/VehicleSetup/JoystickConfigController.h @@ -54,9 +54,6 @@ public: JoystickConfigController(void); ~JoystickConfigController(); - Q_PROPERTY(int minAxisCount MEMBER _axisMinimum CONSTANT) - Q_PROPERTY(int axisCount READ axisCount NOTIFY axisCountChanged) - Q_PROPERTY(QQuickItem* statusText MEMBER _statusText) Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton) Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton) @@ -102,7 +99,6 @@ public: int axisCount(void); signals: - void axisCountChanged(int axisCount); void axisValueChanged(int axis, int value); void rollAxisMappedChanged(bool mapped); @@ -171,6 +167,8 @@ private: void _advanceState(void); void _setupCurrentState(void); + bool _validAxis(int axis); + void _inputCenterWaitBegin (Joystick::AxisFunction_t function, int axis, int value); void _inputStickDetect (Joystick::AxisFunction_t function, int axis, int value); void _inputStickMin (Joystick::AxisFunction_t function, int axis, int value); @@ -219,12 +217,13 @@ private: static const int _attitudeControls = 5; - int _axisCount; ///< Number of actual joystick axes available - static const int _axisMax = 4; ///< Maximum number of supported joystick axes - static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4 - - struct AxisInfo _rgAxisInfo[_axisMax]; ///< Information associated with each axis - + int _axisCount; ///< Number of actual joystick axes available + static const int _axisNoAxis = -1; ///< Signals no axis set + static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4 + struct AxisInfo* _rgAxisInfo; ///< Information associated with each axis + int* _axisValueSave; ///< Saved values prior to detecting axis movement + int* _axisRawValue; ///< Current set of raw axis values + enum calStates _calState; ///< Current calibration state int _calStateCurrentAxis; ///< Current axis being worked on in calStateIdentify and calStateDetectInversion bool _calStateAxisComplete; ///< Work associated with current axis is complete @@ -239,11 +238,7 @@ private: static const int _calRoughCenterDelta; static const int _calMoveDelta; static const int _calSettleDelta; - static const int _calMinDelta; - - int _axisValueSave[_axisMax]; ///< Saved values prior to detecting axis movement - - int _axisRawValue[_axisMax]; ///< Current set of raw axis values + static const int _calMinDelta; int _stickDetectAxis; int _stickDetectInitialValue; -- 2.22.0