Browse Source

Making deadbands stupid-proof; general cleanup

QGC4.4
Gregory Dymarek 8 years ago
parent
commit
8bc154ae36
  1. 12
      src/Joystick/Joystick.cc
  2. 7
      src/Joystick/Joystick.h
  3. 6
      src/VehicleSetup/JoystickConfig.qml
  4. 51
      src/VehicleSetup/JoystickConfigController.cc
  5. 9
      src/VehicleSetup/JoystickConfigController.h

12
src/Joystick/Joystick.cc

@ -96,7 +96,7 @@ void Joystick::_loadSettings(void) @@ -96,7 +96,7 @@ void Joystick::_loadSettings(void)
_throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk);
badSettings |= !convertOk;
qCDebug(JoystickLog) << "_loadSettings calibrated:throttlemode:exponential:badsettings" << _calibrated << _throttleMode << _exponential << badSettings;
qCDebug(JoystickLog) << "_loadSettings calibrated:throttlemode:exponential:deadband:badsettings" << _calibrated << _throttleMode << _exponential << _deadband << badSettings;
QString minTpl ("Axis%1Min");
QString maxTpl ("Axis%1Max");
@ -122,7 +122,7 @@ void Joystick::_loadSettings(void) @@ -122,7 +122,7 @@ void Joystick::_loadSettings(void)
calibration->reversed = settings.value(revTpl.arg(axis), false).toBool();
qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings;
qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings;
}
for (int function=0; function<maxFunction; function++) {
@ -160,7 +160,7 @@ void Joystick::_saveSettings(void) @@ -160,7 +160,7 @@ void Joystick::_saveSettings(void)
settings.setValue(_deadbandSettingsKey, _deadband);
settings.setValue(_throttleModeSettingsKey, _throttleMode);
qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode" << _calibrated << _throttleMode;
qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband" << _calibrated << _throttleMode << _deadband;
QString minTpl ("Axis%1Min");
QString maxTpl ("Axis%1Max");
@ -177,7 +177,7 @@ void Joystick::_saveSettings(void) @@ -177,7 +177,7 @@ void Joystick::_saveSettings(void)
settings.setValue(revTpl.arg(axis), calibration->reversed);
settings.setValue(deadbndTpl.arg(axis), calibration->deadband);
qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed"
qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed:deadband"
<< _name
<< axis
<< calibration->min
@ -232,12 +232,13 @@ float Joystick::_adjustRange(int value, Calibration_t calibration) @@ -232,12 +232,13 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
}
#if 0
qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:basis:normalized:length"
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
@ -585,7 +586,6 @@ void Joystick::setDeadband(bool deadband) @@ -585,7 +586,6 @@ void Joystick::setDeadband(bool deadband)
_deadband = deadband;
_saveSettings();
emit deadbandChanged(_deadband);
}
void Joystick::startCalibrationMode(CalibrationMode_t mode)

7
src/Joystick/Joystick.h

@ -66,9 +66,8 @@ public: @@ -66,9 +66,8 @@ public:
Q_INVOKABLE QString getButtonAction(int button);
Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged)
Q_PROPERTY(int exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
Q_PROPERTY(int accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
Q_PROPERTY(int deadband READ deadband WRITE setDeadband NOTIFY deadbandChanged)
Q_PROPERTY(bool exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
// Property accessors
@ -129,8 +128,6 @@ signals: @@ -129,8 +128,6 @@ signals:
void accumulatorChanged(bool accumulator);
void deadbandChanged(bool deadband);
void enabledChanged(bool enabled);
/// Signal containing new joystick information

6
src/VehicleSetup/JoystickConfig.qml

@ -430,10 +430,10 @@ SetupPage { @@ -430,10 +430,10 @@ SetupPage {
QGCCheckBox {
id: deadband
checked: _activeJoystick.deadband
text: qsTr("Deadband (requires re-calibration)")
checked: controller.deadbandToggle
text: qsTr("Deadbands")
onClicked: _activeJoystick.deadband = checked
onClicked: controller.deadbandToggle = checked
}
}

51
src/VehicleSetup/JoystickConfigController.cc

@ -210,6 +210,18 @@ void JoystickConfigController::cancelButtonClicked(void) @@ -210,6 +210,18 @@ void JoystickConfigController::cancelButtonClicked(void)
_stopCalibration();
}
bool JoystickConfigController::getDeadbandToggle() {
return _activeJoystick->deadband();
}
void JoystickConfigController::setDeadbandToggle(bool deadband) {
_activeJoystick->setDeadband(deadband);
_signalAllAttiudeValueChanges();
emit deadbandToggled(deadband);
}
void JoystickConfigController::_saveAllTrims(void)
{
// We save all trims as the first step. At this point no axes are mapped but it should still
@ -228,27 +240,9 @@ void JoystickConfigController::_axisDeadbandChanged(int axis, int value) @@ -228,27 +240,9 @@ void JoystickConfigController::_axisDeadbandChanged(int axis, int value)
{
value = abs(value)<_calValidMaxValue?abs(value):_calValidMaxValue;
if (value>_rgAxisInfo[axis].deadband) {
_rgAxisInfo[axis].deadband = value;
switch (_rgAxisInfo[axis].function) {
case Joystick::rollFunction:
emit rollAxisDeadbandChanged(_rgAxisInfo[axis].deadband);
break;
case Joystick::pitchFunction:
emit pitchAxisDeadbandChanged(_rgAxisInfo[axis].deadband);
break;
case Joystick::yawFunction:
emit yawAxisDeadbandChanged(_rgAxisInfo[axis].deadband);
break;
case Joystick::throttleFunction:
emit throttleAxisDeadbandChanged(_rgAxisInfo[axis].deadband);
break;
default:
break;
}
qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
}
_rgAxisInfo[axis].deadband = value;
qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
}
/// @brief Waits for the sticks to be centered, enabling Next when done.
@ -257,16 +251,13 @@ void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t fu @@ -257,16 +251,13 @@ void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t fu
Q_UNUSED(function);
//sensing deadband
if (_activeJoystick->deadband()) {
if (abs(value)*1.5>_rgAxisInfo[axis].deadband) {
_axisDeadbandChanged(axis,abs(value)*1.5);
}
if (abs(value)*1.1f>_rgAxisInfo[axis].deadband) { //add 10% on top of existing deadband
_axisDeadbandChanged(axis,abs(value)*1.1f);
}
_nextButton->setEnabled(true);
// FIXME: Doesn't wait for center
// FIXME: Ideally the deadband should be probed only around the center
}
bool JoystickConfigController::_stickSettleComplete(int axis, int value)
@ -703,7 +694,7 @@ int JoystickConfigController::throttleAxisValue(void) @@ -703,7 +694,7 @@ int JoystickConfigController::throttleAxisValue(void)
int JoystickConfigController::rollAxisDeadband(void)
{
if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) {
if ((_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].deadband;
} else {
return 0;
@ -712,7 +703,7 @@ int JoystickConfigController::rollAxisDeadband(void) @@ -712,7 +703,7 @@ int JoystickConfigController::rollAxisDeadband(void)
int JoystickConfigController::pitchAxisDeadband(void)
{
if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) {
if ((_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].deadband;
} else {
return 0;
@ -721,7 +712,7 @@ int JoystickConfigController::pitchAxisDeadband(void) @@ -721,7 +712,7 @@ int JoystickConfigController::pitchAxisDeadband(void)
int JoystickConfigController::yawAxisDeadband(void)
{
if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) {
if ((_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].deadband;
} else {
return 0;
@ -730,7 +721,7 @@ int JoystickConfigController::yawAxisDeadband(void) @@ -730,7 +721,7 @@ int JoystickConfigController::yawAxisDeadband(void)
int JoystickConfigController::throttleAxisDeadband(void)
{
if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) {
if ((_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].deadband;
} else {
return 0;

9
src/VehicleSetup/JoystickConfigController.h

@ -67,6 +67,8 @@ public: @@ -67,6 +67,8 @@ public:
Q_PROPERTY(int yawAxisReversed READ yawAxisReversed NOTIFY yawAxisReversedChanged)
Q_PROPERTY(int throttleAxisReversed READ throttleAxisReversed NOTIFY throttleAxisReversedChanged)
Q_PROPERTY(bool deadbandToggle READ getDeadbandToggle WRITE setDeadbandToggle NOTIFY deadbandToggled)
Q_PROPERTY(QString imageHelp MEMBER _imageHelp NOTIFY imageHelpChanged)
Q_INVOKABLE void cancelButtonClicked(void);
@ -78,7 +80,7 @@ public: @@ -78,7 +80,7 @@ public:
int pitchAxisValue(void);
int yawAxisValue(void);
int throttleAxisValue(void);
int rollAxisDeadband(void);
int pitchAxisDeadband(void);
int yawAxisDeadband(void);
@ -94,6 +96,9 @@ public: @@ -94,6 +96,9 @@ public:
bool yawAxisReversed(void);
bool throttleAxisReversed(void);
bool getDeadbandToggle(void);
void setDeadbandToggle(bool);
int axisCount(void);
signals:
@ -118,6 +123,8 @@ signals: @@ -118,6 +123,8 @@ signals:
void pitchAxisReversedChanged(bool reversed);
void yawAxisReversedChanged(bool reversed);
void throttleAxisReversedChanged(bool reversed);
void deadbandToggled(bool value);
void imageHelpChanged(QString source);

Loading…
Cancel
Save