rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix abs control refactor, make abs control cutoff configurable

This commit is contained in:
Thorsten Laux 2018-12-10 09:05:20 +01:00
parent 4442de5591
commit 17a995e03e
3 changed files with 36 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/main/flight/pid.c
View File

@ -166,6 +166,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_gain = 0,
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.abs_control_cutoff = 11,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
@ -239,6 +240,15 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelax;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
#endif
#if defined(USE_ABSOLUTE_CONTROL)
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float acGain;
static FAST_RAM_ZERO_INIT float acLimit;
static FAST_RAM_ZERO_INIT float acErrorLimit;
static FAST_RAM_ZERO_INIT float acCutoff;
static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
#endif
#ifdef USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
@ -352,6 +362,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
#endif
#if defined(USE_ABSOLUTE_CONTROL)
if (itermRelax) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&acLpf[i], pt1FilterGain(acCutoff, dT));
}
}
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
}
@ -424,12 +441,6 @@ static FAST_RAM_ZERO_INIT bool itermRotation;
#if defined(USE_SMART_FEEDFORWARD)
static FAST_RAM_ZERO_INIT bool smartFeedforward;
#endif
#if defined(USE_ABSOLUTE_CONTROL)
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float acGain;
static FAST_RAM_ZERO_INIT float acLimit;
static FAST_RAM_ZERO_INIT float acErrorLimit;
#endif
#ifdef USE_LAUNCH_CONTROL
static FAST_RAM_ZERO_INIT uint8_t launchControlMode;
@ -542,6 +553,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
acGain = (float)pidProfile->abs_control_gain;
acLimit = (float)pidProfile->abs_control_limit;
acErrorLimit = (float)pidProfile->abs_control_error_limit;
acCutoff = (float)pidProfile->abs_control_cutoff;
#endif
#ifdef USE_DYN_LPF
@ -918,13 +930,12 @@ void FAST_CODE applySmartFeedforward(int axis)
#if defined(USE_ITERM_RELAX)
#if defined(USE_ABSOLUTE_CONTROL)
STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled,
const float setpointLpf, const float setpointHpf,
float *currentPidSetpoint, float *itermErrorRate)
STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate)
{
if (acGain > 0) {
const float setpointLpf = pt1FilterApply(&acLpf[axis], *currentPidSetpoint);
const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
float acErrorRate = 0;
if (itermRelaxIsEnabled) {
const float gmaxac = setpointLpf + 2 * setpointHpf;
const float gminac = setpointLpf - 2 * setpointHpf;
if (gyroRate >= gminac && gyroRate <= gmaxac) {
@ -941,9 +952,6 @@ STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRat
} else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
}
} else {
acErrorRate = *itermErrorRate;
}
if (isAirmodeActivated()) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT,
@ -964,12 +972,10 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
{
const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint);
const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
bool itermRelaxIsEnabled = false;
if (itermRelax &&
(axis < FD_YAW || itermRelax == ITERM_RELAX_RPY ||
itermRelax == ITERM_RELAX_RPY_INC)) {
itermRelaxIsEnabled = true;
const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD;
const bool isDecreasingI =
@ -989,11 +995,8 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate));
}
} else {
#if defined(USE_ABSOLUTE_CONTROL)
applyAbsoluteControl(axis, gyroRate, setpointLpf, setpointHpf, itermRelaxIsEnabled, currentPidSetpoint, itermErrorRate);
#else
UNUSED(itermRelaxIsEnabled);
applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate);
#endif
}
}

1
src/main/flight/pid.h
View File

@ -148,6 +148,7 @@ typedef struct pidProfile_s {
uint8_t abs_control_gain; // How strongly should the absolute accumulated error be corrected for
uint8_t abs_control_limit; // Limit to the correction
uint8_t abs_control_error_limit; // Limit to the accumulated error
uint8_t abs_control_cutoff; // Cutoff frequency for path estimation in abs control
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
uint16_t dyn_lpf_dterm_min_hz;
uint16_t dyn_lpf_dterm_max_hz;

1
src/main/interface/settings.c
View File

@ -934,6 +934,7 @@ const clivalue_t valueTable[] = {
{ "abs_control_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },
{ "abs_control_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_limit) },
{ "abs_control_error_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) },
{ "abs_control_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_cutoff) },
#endif
#ifdef USE_LAUNCH_CONTROL