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

Merge pull request #6065 from mikeller/joelucid_relax2

Browse source 
Joelucid's overworked ITerm relax.
This commit is contained in:
Michael Keller 2018-06-09 22:21:09 +12:00 • committed by GitHub
commit 54c3363719
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 105 additions and 69 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/common/maths.c
View file

@ -130,6 +130,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value;
}
float fapplyDeadband(float value, float deadband)
{
if (fabsf(value) < deadband) {
value = 0;
} else if (value > 0) {
value -= deadband;
} else if (value < 0) {
value += deadband;
}
return value;
}
void devClear(stdev_t *dev)
{
dev->m_n = 0;

1
src/main/common/maths.h
View file

@ -91,6 +91,7 @@ typedef union {
int gcd(int num, int denom);
float powerf(float base, int exp);
int32_t applyDeadband(int32_t value, int32_t deadband);
float fapplyDeadband(float value, float deadband);
void devClear(stdev_t *dev);
void devPush(stdev_t *dev, float x);

9
src/main/fc/fc_core.c
View file

@ -537,6 +537,14 @@ uint8_t calculateThrottlePercent(void)
return ret;
}
static bool airmodeIsActivated;
bool isAirmodeActivated()
{
return airmodeIsActivated;
}
/*
* processRx called from taskUpdateRxMain
@ -544,7 +552,6 @@ uint8_t calculateThrottlePercent(void)
bool processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;
static bool airmodeIsActivated;
static bool sharedPortTelemetryEnabled = false;
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {

1
src/main/fc/fc_core.h
View file

@ -52,3 +52,4 @@ void taskMainPidLoop(timeUs_t currentTimeUs);
bool isFlipOverAfterCrashMode(void);
void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
bool isAirmodeActivated();

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

@ -147,8 +147,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.iterm_rotation = false,
.smart_feedforward = false,
.iterm_relax = ITERM_RELAX_OFF,
.iterm_relax_cutoff_low = 3,
.iterm_relax_cutoff_high = 30,
.iterm_relax_cutoff = 11,
.iterm_relax_type = ITERM_RELAX_GYRO,
.acro_trainer_angle_limit = 20,
.acro_trainer_lookahead_ms = 50,
.acro_trainer_debug_axis = FD_ROLL,
@ -206,10 +206,10 @@ static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX)
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT][2];
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT uint8_t itermRelax;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
#endif
#ifdef USE_RC_SMOOTHING_FILTER
@ -300,8 +300,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
#if defined(USE_ITERM_RELAX)
if (itermRelax) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT));
pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT));
pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
}
}
#endif
@ -427,8 +426,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#if defined(USE_ITERM_RELAX)
itermRelax = pidProfile->iterm_relax;
itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
itermRelaxType = pidProfile->iterm_relax_type;
itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
#endif
#ifdef USE_ACRO_TRAINER
@ -803,19 +802,68 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
#endif // USE_YAW_SPIN_RECOVERY
#if defined(USE_ABSOLUTE_CONTROL)
// mix in a correction for accrued attitude error
float acCorrection = 0;
if (acGain > 0) {
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
currentPidSetpoint += acCorrection;
}
#endif
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
#ifdef USE_ABSOLUTE_CONTROL
float acCorrection = 0;
float acErrorRate;
#endif
float itermErrorRate = 0.0f;
#if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 60) {
itermErrorRate = (1 - setpointHpf / 60.0f) * (currentPidSetpoint - gyroRate);
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(currentPidSetpoint));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
}
if (itermRelaxType == ITERM_RELAX_GYRO ) {
itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
}
#if defined(USE_ABSOLUTE_CONTROL)
const float gmaxac = setpointLpf + 2 * setpointHpf;
const float gminac = setpointLpf - 2 * setpointHpf;
if (gyroRate >= gminac && gyroRate <= gmaxac) {
float acErrorRate1 = gmaxac - gyroRate;
float acErrorRate2 = gminac - gyroRate;
if (acErrorRate1 * axisError[axis] < 0) {
acErrorRate = acErrorRate1;
} else {
acErrorRate = acErrorRate2;
}
if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis] / dT;
}
} else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
}
#endif // USE_ABSOLUTE_CONTROL
} else
#endif // USE_ITERM_RELAX
{
itermErrorRate = currentPidSetpoint - gyroRate;
#if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = itermErrorRate;
#endif // USE_ABSOLUTE_CONTROL
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0 && isAirmodeActivated()) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
currentPidSetpoint += acCorrection;
itermErrorRate += acCorrection;
}
#endif
float errorRate = currentPidSetpoint - gyroRate; // r - y
handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
@ -831,48 +879,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
// -----calculate I component
float itermErrorRate = 0.0f;
#if defined(USE_ABSOLUTE_CONTROL)
float acErrorRate;
#endif
#if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint);
float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint);
#if defined(USE_ABSOLUTE_CONTROL)
gyroTargetLow += acCorrection;
gyroTargetHigh += acCorrection;
#endif
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 = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
#if defined(USE_ABSOLUTE_CONTROL)
else {
itermErrorRate = acCorrection;
}
acErrorRate = itermErrorRate - acCorrection;
#endif
} else
#endif // USE_ITERM_RELAX
{
itermErrorRate = errorRate;
#if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = errorRate;
#endif
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
}
#endif
const float ITerm = pidData[axis].I;
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);

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

@ -76,13 +76,17 @@ typedef struct pid8_s {
uint8_t D;
} pid8_t;
typedef enum
{
typedef enum {
ITERM_RELAX_OFF,
ITERM_RELAX_RP,
ITERM_RELAX_RPY
} itermRelax_e;
typedef enum {
ITERM_RELAX_GYRO,
ITERM_RELAX_SETPOINT
} itermRelaxType_e;
typedef struct pidProfile_s {
uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy
uint16_t dterm_lowpass_hz; // Delta Filter in hz
@ -124,8 +128,8 @@ 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
uint8_t iterm_relax_type; // Specifies type of relax algorithm
uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
uint8_t iterm_relax; // Enable iterm suppression during stick input
uint8_t acro_trainer_angle_limit; // Acro trainer roll/pitch angle limit in degrees
uint8_t acro_trainer_debug_axis; // The axis for which record debugging values are captured 0=roll, 1=pitch

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

@ -341,6 +341,9 @@ static const char * const lookupTableVideoSystem[] = {
static const char * const lookupTableItermRelax[] = {
"OFF", "RP", "RPY"
};
static const char * const lookupTableItermRelaxType[] = {
"GYRO", "SETPOINT"
};
#endif
#ifdef USE_ACRO_TRAINER
@ -441,6 +444,7 @@ const lookupTableEntry_t lookupTables[] = {
#endif // USE_MAX7456
#if defined(USE_ITERM_RELAX)
LOOKUP_TABLE_ENTRY(lookupTableItermRelax),
LOOKUP_TABLE_ENTRY(lookupTableItermRelaxType),
#endif
#ifdef USE_ACRO_TRAINER
LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
@ -801,9 +805,9 @@ const clivalue_t valueTable[] = {
#endif
#if defined(USE_ITERM_RELAX)
{ "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
{ "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
{ "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
#endif
{ "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

@ -100,6 +100,7 @@ typedef enum {
#endif // USE_MAX7456
#if defined(USE_ITERM_RELAX)
TABLE_ITERM_RELAX,
TABLE_ITERM_RELAX_TYPE,
#endif
#ifdef USE_ACRO_TRAINER
TABLE_ACRO_TRAINER_DEBUG,