Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Merge pull request #5963 from joelucid/iterm_relax

Browse source 
ITerm relax feature
This commit is contained in:
Michael Keller 2018-05-30 09:32:21 +12:00 • committed by GitHub
commit eecb59db45
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 68 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.c
View file

@ -69,4 +69,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"USB",
"SMARTAUDIO",
"RTH",
"ITERM_RELAX"
};

1
src/main/build/debug.h
View file

@ -87,6 +87,7 @@ typedef enum {
DEBUG_USB,
DEBUG_SMARTAUDIO,
DEBUG_RTH,
DEBUG_ITERM_RELAX,
DEBUG_COUNT
} debugType_e;

43
src/main/flight/pid.c
View file

@ -139,7 +139,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
.throttle_boost = 0,
.throttle_boost_cutoff = 15,
.iterm_rotation = false,
.smart_feedforward = false
.smart_feedforward = false,
.iterm_relax = ITERM_RELAX_OFF,
.iterm_relax_cutoff_low = 3,
.iterm_relax_cutoff_high = 15,
);
}
@ -195,6 +198,10 @@ static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[3][2];
static FAST_RAM_ZERO_INIT itermRelax_e itermRelax;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
void pidInitFilters(const pidProfile_t *pidProfile)
{
@ -271,6 +278,11 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
if (itermRelax)
for (int i = 0; i < 3; i++) {
pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT));
pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT));
}
}
typedef struct pidCoefficient_s {
@ -334,8 +346,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
crashLimitYaw = pidProfile->crash_limit_yaw;
itermLimit = pidProfile->itermLimit;
throttleBoost = pidProfile->throttle_boost * 0.1f;
itermRotation = pidProfile->iterm_rotation == 1;
smartFeedforward = pidProfile->smart_feedforward == 1;
itermRotation = pidProfile->iterm_rotation;
smartFeedforward = pidProfile->smart_feedforward;
itermRelax = pidProfile->iterm_relax;
itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
}
void pidInit(const pidProfile_t *pidProfile)
@ -601,8 +616,28 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
// -----calculate I component
float itermErrorRate;
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint);
const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint);
if (axis < FD_YAW) {
int itemOffset = (axis << 1);
DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset++, gyroTargetHigh);
DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset, gyroTargetLow);
}
const float gmax = MAX(gyroTargetHigh, gyroTargetLow);
const float gmin = MIN(gyroTargetHigh, gyroTargetLow);
if (gyroRate >= gmin && gyroRate <= gmax) {
itermErrorRate = 0.0f;
} else {
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
} else {
itermErrorRate = errorRate;
}
const float ITerm = pidData[axis].I;
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * errorRate * dynCi, -itermLimit, itermLimit);
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
// Only increase ITerm if output is not saturated

13
src/main/flight/pid.h
View file

@ -76,6 +76,15 @@ typedef struct pid8_s {
uint8_t D;
} pid8_t;
typedef enum
{
ITERM_RELAX_OFF,
ITERM_RELAX_RP,
ITERM_RELAX_RPY
} itermRelax_e;
typedef struct pidProfile_s {
pid8_t pid[PID_ITEM_COUNT];
@ -116,6 +125,10 @@ typedef struct pidProfile_s {
uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system
uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign.
uint8_t iterm_relax_cutoff_low; // Slowest setpoint response to prevent iterm accumulation
uint8_t iterm_relax_cutoff_high; // Fastest setpoint response to prevent iterm accumulation
itermRelax_e iterm_relax; // Enable iterm suppression during stick input
} pidProfile_t;
#ifndef USE_OSD_SLAVE

9
src/main/interface/settings.c
View file

@ -333,6 +333,11 @@ static const char * const lookupTableVideoSystem[] = {
};
#endif // USE_MAX7456
static const char * const lookupTableItermRelax[] = {
"OFF", "RP", "RPY"
};
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = {
@ -407,6 +412,7 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_MAX7456
LOOKUP_TABLE_ENTRY(lookupTableVideoSystem),
#endif // USE_MAX7456
LOOKUP_TABLE_ENTRY(lookupTableItermRelax),
};
#undef LOOKUP_TABLE_ENTRY
@ -741,6 +747,9 @@ const clivalue_t valueTable[] = {
{ "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) },
{ "smart_feedforward", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, smart_feedforward) },
{ "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
{ "iterm_relax_cutoff_low", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_low) },
{ "iterm_relax_cutoff_high", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_high) },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },

1
src/main/interface/settings.h
View file

@ -97,6 +97,7 @@ typedef enum {
#ifdef USE_MAX7456
TABLE_VIDEO_SYSTEM,
#endif // USE_MAX7456
TABLE_ITERM_RELAX,
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;

5
src/test/unit/pid_unittest.cc
View file

@ -22,7 +22,7 @@
#include "unittest_macros.h"
#include "gtest/gtest.h"
#include "build/debug.h"
bool simulateMixerSaturated = false;
float simulatedSetpointRate[3] = { 0,0,0 };
@ -30,6 +30,9 @@ float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedThrottlePIDAttenuation = 1.0f;
float simulatedMotorMixRange = 0.0f;
int16_t debug[DEBUG16_VALUE_COUNT];
uint8_t debugMode;
extern "C" {
#include "build/debug.h"
#include "common/axis.h"