Skip to content

Q
qgroundcontrol
Commit 7170ae4f authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Merge pull request #2068 from mavlink/Joystick 

Joystick fix
parents c2a3cd0b 7125c130
Show whitespace changes
Inline Side-by-side
Showing with 123 additions and 72 deletions
src/Joystick/Joystick.cc
View file @ 7170ae4f
......@@ -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,12 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int sdlI
Joystick::~Joystick()
{
#ifndef __mobile__
delete _rgAxisValues;
delete _rgCalibration;
delete _rgButtonValues;
delete _rgButtonActions;
#endif
}
#ifndef __mobile__
......@@ -115,7 +128,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 +156,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 +184,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 +206,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];
}
......@@ -278,10 +291,6 @@ void Joystick::run(void)
float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
axis = _rgFunctionAxis[yawFunction];
_rgCalibration[axis].min = -32768;
_rgCalibration[axis].max = 32767;
_rgCalibration[axis].center = 0;
_rgCalibration[axis].reversed = false;
float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
axis = _rgFunctionAxis[throttleFunction];
......@@ -398,7 +407,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 +420,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 +429,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 +460,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 +474,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 +552,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__
src/Joystick/Joystick.h
View file @ 7170ae4f
......@@ -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)
......@@ -78,6 +78,11 @@ public:
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);
void stopPolling(void);
......@@ -136,6 +141,8 @@ 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;
......
src/VehicleSetup/JoystickConfig.qml
View file @ 7170ae4f
......@@ -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 {
......
src/VehicleSetup/JoystickConfigController.cc
View file @ 7170ae4f
......@@ -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; i<Joystick::maxFunction; i++) {
_rgFunctionAxisMapping[i] = _axisMax;
_rgFunctionAxisMapping[i] = _axisNoAxis;
}
_signalAllAttiudeValueChanges();
......@@ -436,16 +458,16 @@ void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
// Initialize all function mappings to not set
for (int i=0; i<_axisMax; i++) {
for (int i=0; i<_axisCount; i++) {
struct AxisInfo* info = &_rgAxisInfo[i];
info->function = Joystick::maxFunction;
}
for (size_t i=0; i<Joystick::maxFunction; i++) {
_rgFunctionAxisMapping[i] = _axisMax;
_rgFunctionAxisMapping[i] = _axisNoAxis;
}
for (int axis=0; axis<_axisMax; axis++) {
for (int axis=0; axis<_axisCount; axis++) {
struct AxisInfo* info = &_rgAxisInfo[axis];
Joystick::Calibration_t calibration = joystick->getCalibration(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;
}
src/VehicleSetup/JoystickConfigController.h
View file @ 7170ae4f
......@@ -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);
......@@ -220,10 +218,11 @@ 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 _axisNoAxis = -1; ///< Signals no axis set
static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4
struct AxisInfo _rgAxisInfo[_axisMax]; ///< Information associated with each axis
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
......@@ -241,10 +240,6 @@ private:
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
int _stickDetectAxis;
int _stickDetectInitialValue;
int _stickDetectValue;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment