diff --git a/src/main/common/maths.c b/src/main/common/maths.c index e7813b132e..dd443ee80b 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 300c18fae6..19842ebb40 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 748020edaa..d0aaf44fcd 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -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)) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 95d9655857..e2ed7fcc9a 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -52,3 +52,4 @@ void taskMainPidLoop(timeUs_t currentTimeUs); bool isFlipOverAfterCrashMode(void); void runawayTakeoffTemporaryDisable(uint8_t disableFlag); +bool isAirmodeActivated(); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e44b470766..4707b74abf 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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, ¤tPidSetpoint, &errorRate); @@ -831,49 +879,7 @@ 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); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cb53cea862..d99e870824 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 29044b276f..45fb4cd349 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -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) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index ba0a8727c9..cbcc60266f 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -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,