Skip to content

Q
qgroundcontrol
Commit 8c1cbc6d authored by Gregory Dymarek's avatar Gregory Dymarek
Browse files
Options

Implementing throttle axis accumulator with deadband support

parent 24596a5c
Hide whitespace changes
Inline Side-by-side
Showing with 80 additions and 8 deletions
src/Joystick/Joystick.cc
View file @ 8c1cbc6d
......@@ -23,6 +23,7 @@ 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::_rgFunctionSettingsKey[Joystick::maxFunction] = {
"RollAxis",
......@@ -46,6 +47,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
, _lastButtonBits(0)
, _throttleMode(ThrottleModeCenterZero)
, _exponential(false)
, _accumulator(false)
, _activeVehicle(NULL)
, _pollingStartedForCalibration(false)
, _multiVehicleManager(multiVehicleManager)
......@@ -86,6 +88,7 @@ void Joystick::_loadSettings(void)
_calibrated = settings.value(_calibratedSettingsKey, false).toBool();
_exponential = settings.value(_exponentialSettingsKey, false).toBool();
_accumulator = settings.value(_accumulatorSettingsKey, false).toBool();
_throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk);
badSettings |= !convertOk;
......@@ -96,6 +99,7 @@ void Joystick::_loadSettings(void)
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 +113,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:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings;
}
for (int function=0; function<maxFunction; function++) {
......@@ -145,6 +153,7 @@ void Joystick::_saveSettings(void)
settings.setValue(_calibratedSettingsKey, _calibrated);
settings.setValue(_exponentialSettingsKey, _exponential);
settings.setValue(_accumulatorSettingsKey, _accumulator);
settings.setValue(_throttleModeSettingsKey, _throttleMode);
qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode" << _calibrated << _throttleMode;
......@@ -153,6 +162,7 @@ void Joystick::_saveSettings(void)
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,6 +171,7 @@ 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"
<< _name
......@@ -168,7 +179,8 @@ void Joystick::_saveSettings(void)
<< calibration->min
<< calibration->max
<< calibration->center
<< calibration->reversed;
<< calibration->reversed
<< calibration->deadband;
}
for (int function=0; function<maxFunction; function++) {
......@@ -189,6 +201,8 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
float axisLength;
float axisBasis;
if (value > calibration.center) {
axisBasis = 1.0f;
valueNormalized = value - calibration.center;
......@@ -199,6 +213,13 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
axisLength = calibration.center - calibration.min;
}
if (_accumulator) {//deadband will be applied only in accumulator mode
int deadband = calibration.deadband*1.5; //we are increasing deadband to accommodate slight variations
if (valueNormalized>deadband) valueNormalized-=deadband;
else if (valueNormalized<-deadband) valueNormalized+=deadband;
else valueNormalized = 0.f;
}
float axisPercent = valueNormalized / axisLength;
float correctedValue = axisBasis * axisPercent;
......@@ -214,7 +235,7 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
<< calibration.min
<< calibration.max
<< calibration.center
<< calibration.center
<< calibration.deadband
<< axisBasis
<< valueNormalized
<< axisLength;
......@@ -276,6 +297,15 @@ void Joystick::run(void)
axis = _rgFunctionAxis[throttleFunction];
float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
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)));
float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
......@@ -529,6 +559,19 @@ 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);
}
void Joystick::startCalibrationMode(CalibrationMode_t mode)
{
if (mode == CalibrationModeOff) {
......
src/Joystick/Joystick.h
View file @ 8c1cbc6d
......@@ -34,6 +34,7 @@ public:
int min;
int max;
int center;
int deadband;
bool reversed;
} Calibration_t;
......@@ -66,6 +67,7 @@ public:
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)
// Property accessors
......@@ -93,6 +95,9 @@ public:
bool exponential(void);
void setExponential(bool expo);
bool accumulator(void);
void setAccumulator(bool accu);
typedef enum {
CalibrationModeOff, // Not calibrating
CalibrationModeMonitor, // Monitors are active, continue to send to vehicle if already polling
......@@ -118,6 +123,8 @@ signals:
void exponentialChanged(bool exponential);
void accumulatorChanged(bool accumulator);
void enabledChanged(bool enabled);
/// Signal containing new joystick information
......@@ -175,6 +182,7 @@ protected:
ThrottleMode_t _throttleMode;
bool _exponential;
bool _accumulator;
Vehicle* _activeVehicle;
bool _pollingStartedForCalibration;
......@@ -189,6 +197,7 @@ private:
static const char* _buttonActionSettingsKey;
static const char* _throttleModeSettingsKey;
static const char* _exponentialSettingsKey;
static const char* _accumulatorSettingsKey;
};
#endif
src/VehicleSetup/JoystickConfig.qml
View file @ 8c1cbc6d
......@@ -382,6 +382,20 @@ SetupPage {
onClicked: _activeJoystick.throttleMode = 0
}
Row {
width: parent.width
spacing: ScreenTools.defaultFontPixelWidth
visible: _activeJoystick.throttleMode == 0
QGCCheckBox {
id: accumulator
checked: _activeJoystick.accumulator
text: qsTr("Use accumulator on throttle")
onClicked: _activeJoystick.accumulator = checked
}
}
QGCRadioButton {
exclusiveGroup: throttleModeExclusiveGroup
text: qsTr("Full down stick is zero throttle")
......
src/VehicleSetup/JoystickConfigController.cc
View file @ 8c1cbc6d
......@@ -228,12 +228,13 @@ void JoystickConfigController::_saveAllTrims(void)
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
_rgAxisInfo[axis].deadband = std::max(abs(value),_rgAxisInfo[axis].deadband);
_nextButton->setEnabled(true);
// FIXME: Doesn't wait for center
// FIXME: Ideally the deadband should be probed only around the center
qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
}
bool JoystickConfigController::_stickSettleComplete(int axis, int value)
......@@ -420,6 +421,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,6 +459,7 @@ 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 +515,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 +538,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);
}
......
src/VehicleSetup/JoystickConfigController.h
View file @ 8c1cbc6d
......@@ -144,6 +144,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