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

Merge pull request #7193 from joelucid/ac_fixes 

fix abs control refactor, make abs control cutoff configurable
This commit is contained in:
Michael Keller 2018-12-13 00:28:13 +13:00 committed by GitHub
parent 9bae508ab8 8bcc4dab64
commit b04d6d69af
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 41 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -114,7 +114,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5);
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 6);
void resetPidProfile(pidProfile_t *pidProfile)
{
@ -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,31 +930,27 @@ 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) {
const float acErrorRate1 = gmaxac - gyroRate;
const float acErrorRate2 = gminac - gyroRate;
if (acErrorRate1 * axisError[axis] < 0) {
acErrorRate = acErrorRate1;
} else {
acErrorRate = acErrorRate2;
}
if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis] * pidFrequency;
}
const float gmaxac = setpointLpf + 2 * setpointHpf;
const float gminac = setpointLpf - 2 * setpointHpf;
if (gyroRate >= gminac && gyroRate <= gmaxac) {
const float acErrorRate1 = gmaxac - gyroRate;
const float acErrorRate2 = gminac - gyroRate;
if (acErrorRate1 * axisError[axis] < 0) {
acErrorRate = acErrorRate1;
} else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
acErrorRate = acErrorRate2;
}
if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis] * pidFrequency;
}
} else {
acErrorRate = *itermErrorRate;
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
}
if (isAirmodeActivated()) {
@ -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
}
}

5
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;
@ -213,9 +214,7 @@ bool pidAntiGravityEnabled(void);
extern float axisError[XYZ_AXIS_COUNT];
void applyItermRelax(const int axis, const float iterm,
const float gyroRate, float *itermErrorRate, float *currentPidSetpoint);
void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled,
const float setpointLpf, const float setpointHpf,
float *currentPidSetpoint, float *itermErrorRate);
void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
void rotateItermAndAxisError();
float pidLevel(int axis, const pidProfile_t *pidProfile,
const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);

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

13
src/test/unit/pid_unittest.cc
View File

@ -589,29 +589,22 @@ TEST(pidControllerTest, testAbsoluteControl) {
pidStabilisationState(PID_STABILISATION_ON);
float gyroRate = 0;
bool itermRelaxIsEnabled = false;
float setpointLpf = 6;
float setpointHpf = 30;
float itermErrorRate = 10;
float currentPidSetpoint = 10;
applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf,
&currentPidSetpoint, &itermErrorRate);
applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8));
ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8));
itermRelaxIsEnabled = true;
applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf,
&currentPidSetpoint, &itermErrorRate);
applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8));
ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8));
gyroRate = -53;
axisError[FD_PITCH] = -60;
applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf,
&currentPidSetpoint, &itermErrorRate);
applyAbsoluteControl(FD_PITCH, gyroRate, &currentPidSetpoint, &itermErrorRate);
ASSERT_NEAR(-79.2, itermErrorRate, calculateTolerance(-79.2));
ASSERT_NEAR(-79.2, currentPidSetpoint, calculateTolerance(-79.2));
}