diff --git a/src/main/config/config.c b/src/main/config/config.c index 69d1e3a896..8c5637079d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -179,6 +179,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->gyro_soft_lpf = 1; // filtering ON by default pidProfile->dterm_cut_hz = 0; pidProfile->delta_from_gyro_error = 0; + pidProfile->airModeInsaneAcrobilityFactor = 0; pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully pidProfile->I_f[ROLL] = 0.3f; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8050500576..9287161927 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -22,6 +22,7 @@ #include #include "build_config.h" +#include "debug.h" #include "common/axis.h" #include "common/maths.h" @@ -96,10 +97,48 @@ void setPidDeltaSamples(uint8_t filter) { } } +void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) { + float rcCommandReflection = (float)rcCommand[axis] / 500.0f; + + //Ki scaler + axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f); + + //dynamic Ki handler + if (axisState->isCurrentlyAtZero) { + if (axisState->previousReferenceIsPositive ^ IS_POSITIVE(referenceTerm)) { + axisState->isCurrentlyAtZero = false; + } else { + axisState->iTermScaler = 0; + errorGyroIf[axis] = 0; + errorGyroI[axis] = 0; + } + } + + if (!axisState->iTermScaler) { + if (!axisState->isCurrentlyAtZero) { + if (IS_POSITIVE(referenceTerm)) { + axisState->previousReferenceIsPositive = true; + } else { + axisState->previousReferenceIsPositive = false; + } + } else { + axisState->isCurrentlyAtZero = true; + } + } + + if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor) { + axisState->wowFactor = 1.0f - (ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f + axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000; + } else { + axisState->wowFactor = 1; + axisState->factor = 0; + } +} + const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static filterStatePt1_t DTermState[3]; -//static filterStatePt1_t yawPTermState; +static airModePlus_t airModePlusAxisState[3]; static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) @@ -173,7 +212,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + 0.5f * (lastError[axis] + RateError) * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); + + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + airModePlus(&airModePlusAxisState[axis], axis, pidProfile, PTerm); + errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler; + } if (allowITermShrinkOnly || motorLimitReached) { if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) { @@ -190,12 +234,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa ITerm = errorGyroIf[axis]; //-----calculate D-term - if (!pidProfile->delta_from_gyro_error) { - delta = RateError - lastError[axis]; - lastError[axis] = RateError; - } else { + if (pidProfile->delta_from_gyro_error || axis == YAW) { delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyroRate[axis] = gyroRate; + } else { + delta = RateError - lastError[axis]; + lastError[axis] = RateError; } // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference @@ -221,6 +265,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]); + } + #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); @@ -310,12 +358,18 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - errorGyroI[axis] = errorGyroI[axis] + ((((lastError[axis] + RateError) / 2) * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis]; + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + airModePlus(&airModePlusAxisState[axis], axis, pidProfile, (float) PTerm); + errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler; + } + ITerm = errorGyroI[axis] >> 13; if (allowITermShrinkOnly || motorLimitReached) { @@ -329,12 +383,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat } //-----calculate D-term - if (!pidProfile->delta_from_gyro_error) { // quick and dirty solution for testing - delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastError[axis] = RateError; - } else { + if (pidProfile->delta_from_gyro_error || axis == YAW) { // quick and dirty solution for testing delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyroRate[axis] = gyroRate; + } else { + delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastError[axis] = RateError; } // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference @@ -359,6 +413,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]); + } + #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cbabf8d821..5a834faaca 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -20,6 +20,7 @@ #define GYRO_I_MAX 256 // Gyro I limiter #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter +#define IS_POSITIVE(x) ((x > 0) ? true : false) typedef enum { PIDROLL, @@ -57,7 +58,7 @@ typedef struct pidProfile_s { float H_level; uint8_t H_sensitivity; - uint16_t yaw_p_limit; // set P term limit (fixed value was 300) + uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 uint8_t delta_from_gyro_error; // Used for filering Pterm noise on noisy frames uint8_t gyro_soft_lpf; // Gyro FIR filter @@ -71,6 +72,14 @@ typedef struct pidProfile_s { #endif } pidProfile_t; +typedef struct airModePlus { + float factor; + float wowFactor; + float iTermScaler; + bool isCurrentlyAtZero; + bool previousReferenceIsPositive; +} airModePlus_t; + extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 45976df840..1051a5d177 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -658,6 +658,7 @@ const clivalue_t valueTable[] = { { "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } }, { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } }, { "delta_from_gyro_error", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} }, + { "insane_acrobility_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 30 } }, #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } },