Skip to content

Q
qgroundcontrol
Commit 9da1f5f8 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4123 from gregd72002/master 

Implementing throttle axis accumulator with deadband support
parents 3950327e 7fcae94c
Hide whitespace changes
Inline Side-by-side
Showing with 253 additions and 28 deletions
src/Joystick/Joystick.cc
View file @ 9da1f5f8
......@@ -23,6 +23,8 @@ const char* Joystick::_calibratedSettingsKey = "Calibrated1"; // Increment
const char* Joystick::_buttonActionSettingsKey = "ButtonActionName%1";
const char* Joystick::_throttleModeSettingsKey = "ThrottleMode";
const char* Joystick::_exponentialSettingsKey = "Exponential";
const char* Joystick::_accumulatorSettingsKey = "Accumulator";
const char* Joystick::_deadbandSettingsKey = "Deadband";
const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
"RollAxis",
......@@ -46,6 +48,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
, _lastButtonBits(0)
, _throttleMode(ThrottleModeCenterZero)
, _exponential(false)
, _accumulator(false)
, _deadband(false)
, _activeVehicle(NULL)
, _pollingStartedForCalibration(false)
, _multiVehicleManager(multiVehicleManager)
......@@ -86,16 +90,19 @@ void Joystick::_loadSettings(void)
_calibrated = settings.value(_calibratedSettingsKey, false).toBool();
_exponential = settings.value(_exponentialSettingsKey, false).toBool();
_accumulator = settings.value(_accumulatorSettingsKey, false).toBool();
_deadband = settings.value(_deadbandSettingsKey, false).toBool();
_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");
QString trimTpl ("Axis%1Trim");
QString revTpl ("Axis%1Rev");
QString deadbndTpl ("Axis%1Deadbnd");
for (int axis=0; axis<_axisCount; axis++) {
Calibration_t* calibration = &_rgCalibration[axis];
......@@ -109,9 +116,13 @@ void Joystick::_loadSettings(void)
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:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << 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++) {
......@@ -145,14 +156,17 @@ void Joystick::_saveSettings(void)
settings.setValue(_calibratedSettingsKey, _calibrated);
settings.setValue(_exponentialSettingsKey, _exponential);
settings.setValue(_accumulatorSettingsKey, _accumulator);
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");
QString trimTpl ("Axis%1Trim");
QString revTpl ("Axis%1Rev");
QString deadbndTpl ("Axis%1Deadbnd");
for (int axis=0; axis<_axisCount; axis++) {
Calibration_t* calibration = &_rgCalibration[axis];
......@@ -161,14 +175,16 @@ void Joystick::_saveSettings(void)
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"
qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed:deadband"
<< _name
<< axis
<< calibration->min
<< calibration->max
<< calibration->center
<< calibration->reversed;
<< calibration->reversed
<< calibration->deadband;
}
for (int function=0; function<maxFunction; function++) {
......@@ -183,7 +199,7 @@ void Joystick::_saveSettings(void)
}
/// Adjust the raw axis value to the -1:1 range given calibration information
float Joystick::_adjustRange(int value, Calibration_t calibration)
float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDeadbands)
{
float valueNormalized;
float axisLength;
......@@ -199,6 +215,12 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
axisLength = calibration.center - calibration.min;
}
if (withDeadbands) {
if (valueNormalized>calibration.deadband) valueNormalized-=calibration.deadband;
else if (valueNormalized<-calibration.deadband) valueNormalized+=calibration.deadband;
else valueNormalized = 0.f;
}
float axisPercent = valueNormalized / axisLength;
float correctedValue = axisBasis * axisPercent;
......@@ -208,13 +230,14 @@ 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.center
<< calibration.reversed
<< calibration.deadband
<< axisBasis
<< valueNormalized
<< axisLength;
......@@ -265,16 +288,25 @@ void Joystick::run(void)
if (_calibrationMode != CalibrationModeCalibrating) {
int axis = _rgFunctionAxis[rollFunction];
float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
axis = _rgFunctionAxis[pitchFunction];
float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
axis = _rgFunctionAxis[yawFunction];
float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
axis = _rgFunctionAxis[throttleFunction];
float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_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<float>(-1.f), std::min(throttle_accu, static_cast<float>(1.f)));
throttle = throttle_accu;
}
float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
......@@ -512,6 +544,11 @@ void Joystick::setThrottleMode(int mode)
}
_throttleMode = (ThrottleMode_t)mode;
if (_throttleMode == ThrottleModeDownZero) {
setAccumulator(false);
}
_saveSettings();
emit throttleModeChanged(_throttleMode);
}
......@@ -529,6 +566,31 @@ void Joystick::setExponential(bool expo)
emit exponentialChanged(_exponential);
}
bool Joystick::accumulator(void)
{
return _accumulator;
}
void Joystick::setAccumulator(bool accu)
{
_accumulator = accu;
_saveSettings();
emit accumulatorChanged(_accumulator);
}
bool Joystick::deadband(void)
{
return _deadband;
}
void Joystick::setDeadband(bool deadband)
{
_deadband = deadband;
_saveSettings();
}
void Joystick::startCalibrationMode(CalibrationMode_t mode)
{
if (mode == CalibrationModeOff) {
......
src/Joystick/Joystick.h
View file @ 9da1f5f8
......@@ -34,6 +34,7 @@ public:
int min;
int max;
int center;
int deadband;
bool reversed;
} Calibration_t;
......@@ -65,7 +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(bool exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
// Property accessors
......@@ -93,6 +95,12 @@ public:
bool exponential(void);
void setExponential(bool expo);
bool accumulator(void);
void setAccumulator(bool accu);
bool deadband(void);
void setDeadband(bool accu);
typedef enum {
CalibrationModeOff, // Not calibrating
CalibrationModeMonitor, // Monitors are active, continue to send to vehicle if already polling
......@@ -118,6 +126,8 @@ signals:
void exponentialChanged(bool exponential);
void accumulatorChanged(bool accumulator);
void enabledChanged(bool enabled);
/// Signal containing new joystick information
......@@ -133,7 +143,7 @@ signals:
protected:
void _saveSettings(void);
void _loadSettings(void);
float _adjustRange(int value, Calibration_t calibration);
float _adjustRange(int value, Calibration_t calibration, bool withDeadbands);
void _buttonAction(const QString& action);
bool _validAxis(int axis);
bool _validButton(int button);
......@@ -175,6 +185,8 @@ protected:
ThrottleMode_t _throttleMode;
bool _exponential;
bool _accumulator;
bool _deadband;
Vehicle* _activeVehicle;
bool _pollingStartedForCalibration;
......@@ -189,6 +201,8 @@ private:
static const char* _buttonActionSettingsKey;
static const char* _throttleModeSettingsKey;
static const char* _exponentialSettingsKey;
static const char* _accumulatorSettingsKey;
static const char* _deadbandSettingsKey;
};
#endif
src/VehicleSetup/JoystickConfig.qml
View file @ 9da1f5f8
......@@ -72,7 +72,7 @@ SetupPage {
Item {
property int axisValue: 0
property int deadbandValue: 0
property int __lastAxisValue: 0
readonly property int __axisValueMaxJitter: 100
......@@ -87,6 +87,20 @@ SetupPage {
color: __barColor
}
// Deadband
Rectangle {
id: deadbandBar
anchors.verticalCenter: parent.verticalCenter
x: _deadbandPosition
width: _deadbandWidth
height: parent.height / 2
color: "#8c161a"
property real _percentDeadband: ((2 * deadbandValue) / (32768.0 * 2))
property real _deadbandWidth: parent.width * _percentDeadband
property real _deadbandPosition: (parent.width - _deadbandWidth) / 2
}
// Center point
Rectangle {
anchors.horizontalCenter: parent.horizontalCenter
......@@ -126,13 +140,15 @@ SetupPage {
duration: 1500
}
/*
// Axis value debugger
/*
QGCLabel {
anchors.fill: parent
text: axisValue
}
*/
}
} // Component - axisMonitorDisplayComponent
......@@ -180,6 +196,8 @@ SetupPage {
target: controller
onRollAxisValueChanged: rollLoader.item.axisValue = value
onRollAxisDeadbandChanged: rollLoader.item.deadbandValue = value
}
}
......@@ -210,6 +228,9 @@ SetupPage {
target: controller
onPitchAxisValueChanged: pitchLoader.item.axisValue = value
onPitchAxisDeadbandChanged: pitchLoader.item.deadbandValue = value
}
}
......@@ -240,6 +261,8 @@ SetupPage {
target: controller
onYawAxisValueChanged: yawLoader.item.axisValue = value
onYawAxisDeadbandChanged: yawLoader.item.deadbandValue = value
}
}
......@@ -270,6 +293,8 @@ SetupPage {
target: controller
onThrottleAxisValueChanged: throttleLoader.item.axisValue = value
onThrottleAxisDeadbandChanged: throttleLoader.item.deadbandValue = value
}
}
} // Column - Attitude Control labels
......@@ -382,6 +407,21 @@ SetupPage {
onClicked: _activeJoystick.throttleMode = 0
}
Row {
x: 20
width: parent.width
spacing: ScreenTools.defaultFontPixelWidth
visible: _activeJoystick.throttleMode == 0
QGCCheckBox {
id: accumulator
checked: _activeJoystick.accumulator
text: qsTr("Spring loaded throttle smoothing")
onClicked: _activeJoystick.accumulator = checked
}
}
QGCRadioButton {
exclusiveGroup: throttleModeExclusiveGroup
text: qsTr("Full down stick is zero throttle")
......@@ -435,6 +475,20 @@ SetupPage {
onActivated: _activeVehicle.joystickMode = index
}
}
Row {
width: parent.width
spacing: ScreenTools.defaultFontPixelWidth
visible: advancedSettings.checked
QGCCheckBox {
id: deadband
checked: controller.deadbandToggle
text: qsTr("Deadbands")
onClicked: controller.deadbandToggle = checked
}
}
}
} // Column - left column
......@@ -697,3 +751,5 @@ SetupPage {
} // Item
} // Component - pageComponent
} // SetupPage
src/VehicleSetup/JoystickConfigController.cc
View file @ 9da1f5f8
......@@ -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
......@@ -224,16 +236,28 @@ void JoystickConfigController::_saveAllTrims(void)
_advanceState();
}
void JoystickConfigController::_axisDeadbandChanged(int axis, int value)
{
value = abs(value)<_calValidMaxValue?abs(value):_calValidMaxValue;
_rgAxisInfo[axis].deadband = value;
qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
}
/// @brief Waits for the sticks to be centered, enabling Next when done.
void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t function, int axis, int value)
{
Q_UNUSED(function);
Q_UNUSED(axis);
Q_UNUSED(value);
// FIXME: Doesn't wait for center
//sensing deadband
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
}
bool JoystickConfigController::_stickSettleComplete(int axis, int value)
......@@ -420,6 +444,7 @@ void JoystickConfigController::_resetInternalCalibrationValues(void)
struct AxisInfo* info = &_rgAxisInfo[i];
info->function = Joystick::maxFunction;
info->reversed = false;
info->deadband = 0;
info->axisMin = JoystickConfigController::_calCenterPoint;
info->axisMax = JoystickConfigController::_calCenterPoint;
info->axisTrim = JoystickConfigController::_calCenterPoint;
......@@ -457,7 +482,8 @@ void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
info->axisMin = calibration.min;
info->axisMax = calibration.max;
info->reversed = calibration.reversed;
info->deadband = calibration.deadband;
qCDebug(JoystickConfigControllerLog) << "Read settings name:axis:min:max:trim:reversed" << joystick->name() << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed;
}
......@@ -512,6 +538,7 @@ void JoystickConfigController::_validateCalibration(void)
info->axisMin = _calDefaultMinValue;
info->axisMax = _calDefaultMaxValue;
info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
info->deadband = 0;
info->reversed = false;
}
}
......@@ -534,6 +561,7 @@ void JoystickConfigController::_writeCalibration(void)
calibration.min = info->axisMin;
calibration.max = info->axisMax;
calibration.reversed = info->reversed;
calibration.deadband = info->deadband;
joystick->setCalibration(axis, calibration);
}
......@@ -664,6 +692,42 @@ int JoystickConfigController::throttleAxisValue(void)
}
}
int JoystickConfigController::rollAxisDeadband(void)
{
if ((_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].deadband;
} else {
return 0;
}
}
int JoystickConfigController::pitchAxisDeadband(void)
{
if ((_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].deadband;
} else {
return 0;
}
}
int JoystickConfigController::yawAxisDeadband(void)
{
if ((_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].deadband;
} else {
return 0;
}
}
int JoystickConfigController::throttleAxisDeadband(void)
{
if ((_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) && (_activeJoystick->deadband())) {
return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].deadband;
} else {
return 0;
}
}
bool JoystickConfigController::rollAxisMapped(void)
{
return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis;
......@@ -731,6 +795,11 @@ void JoystickConfigController::_signalAllAttiudeValueChanges(void)
emit pitchAxisReversedChanged(pitchAxisReversed());
emit yawAxisReversedChanged(yawAxisReversed());
emit throttleAxisReversedChanged(throttleAxisReversed());
emit rollAxisDeadbandChanged(rollAxisDeadband());
emit pitchAxisDeadbandChanged(pitchAxisDeadband());
emit yawAxisDeadbandChanged(yawAxisDeadband());
emit throttleAxisDeadbandChanged(throttleAxisDeadband());
}
void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
......
src/VehicleSetup/JoystickConfigController.h
View file @ 9da1f5f8
......@@ -51,29 +51,41 @@ public:
Q_PROPERTY(bool pitchAxisMapped READ pitchAxisMapped NOTIFY pitchAxisMappedChanged)
Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged)
Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged)
Q_PROPERTY(int rollAxisValue READ rollAxisValue NOTIFY rollAxisValueChanged)
Q_PROPERTY(int pitchAxisValue READ pitchAxisValue NOTIFY pitchAxisValueChanged)
Q_PROPERTY(int yawAxisValue READ yawAxisValue NOTIFY yawAxisValueChanged)
Q_PROPERTY(int throttleAxisValue READ throttleAxisValue NOTIFY throttleAxisValueChanged)
Q_PROPERTY(int rollAxisDeadband READ rollAxisDeadband NOTIFY rollAxisDeadbandChanged)
Q_PROPERTY(int pitchAxisDeadband READ pitchAxisDeadband NOTIFY pitchAxisDeadbandChanged)
Q_PROPERTY(int yawAxisDeadband READ yawAxisDeadband NOTIFY yawAxisDeadbandChanged)
Q_PROPERTY(int throttleAxisDeadband READ throttleAxisDeadband NOTIFY throttleAxisDeadbandChanged)
Q_PROPERTY(int rollAxisReversed READ rollAxisReversed NOTIFY rollAxisReversedChanged)
Q_PROPERTY(int pitchAxisReversed READ pitchAxisReversed NOTIFY pitchAxisReversedChanged)
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);
Q_INVOKABLE void skipButtonClicked(void);
Q_INVOKABLE void nextButtonClicked(void);
Q_INVOKABLE void start(void);
int rollAxisValue(void);
int pitchAxisValue(void);
int yawAxisValue(void);
int throttleAxisValue(void);
int rollAxisDeadband(void);
int pitchAxisDeadband(void);
int yawAxisDeadband(void);
int throttleAxisDeadband(void);
bool rollAxisMapped(void);
bool pitchAxisMapped(void);
bool yawAxisMapped(void);
......@@ -84,11 +96,14 @@ public:
bool yawAxisReversed(void);
bool throttleAxisReversed(void);
bool getDeadbandToggle(void);
void setDeadbandToggle(bool);
int axisCount(void);
signals:
void axisValueChanged(int axis, int value);
void rollAxisMappedChanged(bool mapped);
void pitchAxisMappedChanged(bool mapped);
void yawAxisMappedChanged(bool mapped);
......@@ -98,11 +113,18 @@ signals:
void pitchAxisValueChanged(int value);
void yawAxisValueChanged(int value);
void throttleAxisValueChanged(int value);
void rollAxisDeadbandChanged(int value);
void pitchAxisDeadbandChanged(int value);
void yawAxisDeadbandChanged(int value);
void throttleAxisDeadbandChanged(int value);
void rollAxisReversedChanged(bool reversed);
void pitchAxisReversedChanged(bool reversed);
void yawAxisReversedChanged(bool reversed);
void throttleAxisReversedChanged(bool reversed);
void deadbandToggled(bool value);
void imageHelpChanged(QString source);
......@@ -112,6 +134,7 @@ signals:
private slots:
void _activeJoystickChanged(Joystick* joystick);
void _axisValueChanged(int axis, int value);
void _axisDeadbandChanged(int axis, int value);
private:
/// @brief The states of the calibration state machine.
......@@ -144,6 +167,7 @@ private:
int axisMin; ///< Minimum axis value
int axisMax; ///< Maximum axis value
int axisTrim; ///< Trim position
int deadband; ///< Deadband
};
Joystick* _activeJoystick;
......
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