diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ddc83acab3..3d75bde6b0 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1258,7 +1258,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentProfile->pidProfile.pidAtMinThrottle); // Betaflight PID controller parameters - BLACKBOX_PRINT_HEADER_LINE("itermThrottleThreshold:%d", currentProfile->pidProfile.itermThrottleThreshold); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold:%d", currentProfile->pidProfile.itermThrottleThreshold); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain:%d", castFloatBytesToInt(currentProfile->pidProfile.itermAcceleratorGain)); BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", currentProfile->pidProfile.setpointRelaxRatio); BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentProfile->pidProfile.dtermSetpointWeight); BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.yawRateAccelLimit)); diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 4c55480dda..cec34cf1b3 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,7 +17,7 @@ #pragma once -#define EEPROM_CONF_VERSION 151 +#define EEPROM_CONF_VERSION 152 void initEEPROM(void); void writeEEPROM(); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 218772e864..6b89e9cb5a 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -701,6 +701,7 @@ const clivalue_t valueTable[] = { { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, + { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 10d5be8de6..974e459a5a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -137,10 +137,10 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) static void resetPidProfile(pidProfile_t *pidProfile) { - pidProfile->P8[ROLL] = 43; + pidProfile->P8[ROLL] = 40; pidProfile->I8[ROLL] = 40; pidProfile->D8[ROLL] = 20; - pidProfile->P8[PITCH] = 58; + pidProfile->P8[PITCH] = 55; pidProfile->I8[PITCH] = 50; pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 70; @@ -177,13 +177,14 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_notch_cutoff = 160; pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; - pidProfile->levelAngleLimit = 70; // 70 degrees - pidProfile->levelSensitivity = 100; // 100 degrees at full stick + pidProfile->levelAngleLimit = 55; + pidProfile->levelSensitivity = 55; pidProfile->setpointRelaxRatio = 30; pidProfile->dtermSetpointWeight = 200; pidProfile->yawRateAccelLimit = 10.0f; pidProfile->rateAccelLimit = 0.0f; - pidProfile->itermThrottleThreshold = 350; + pidProfile->itermThrottleThreshold = 300; + pidProfile->itermAcceleratorGain = 4.0f; } void resetProfile(profile_t *profile) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f3d27dd9f1..da66940d10 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -200,7 +200,9 @@ void scaleRcCommandToFpvCamAngle(void) { const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; if(ABS(rcCommandSpeed) > throttleVelocityThreshold) - pidResetErrorGyroState(); + pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain); + else + pidSetItermAccelerator(1.0f); } void processRcCommand(void) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3fc7165dd2..e3415cb01a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -65,6 +65,12 @@ void pidResetErrorGyroState(void) } } +static float itermAccelerator = 1.0f; + +void pidSetItermAccelerator(float newItermAccelerator) { + itermAccelerator = newItermAccelerator; +} + void pidStabilisationState(pidStabilisationState_e pidControllerState) { pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; @@ -237,7 +243,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an const float setpointRateScaler = constrainf(1.0f - (ABS(currentPidSetpoint) / accumulationThreshold), 0.0f, 1.0f); float ITerm = previousGyroIf[axis]; - ITerm += Ki[axis] * errorRate * dT * setpointRateScaler; + ITerm += Ki[axis] * errorRate * dT * setpointRateScaler * itermAccelerator; // limit maximum integrator value to prevent WindUp ITerm = constrainf(ITerm, -250.0f, 250.0f); previousGyroIf[axis] = ITerm; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 5ebe5cad3e..3054afdaf9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -78,6 +78,7 @@ typedef struct pidProfile_s { // Betaflight PID controller parameters uint16_t itermThrottleThreshold; // max allowed throttle delta before errorGyroReset in ms + float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms @@ -102,6 +103,7 @@ extern uint8_t PIDweight[3]; void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); void pidSetTargetLooptime(uint32_t pidLooptime); +void pidSetItermAccelerator(float newItermAccelerator); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index b20fd48457..3319fa9cf1 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -60,17 +60,16 @@ void targetConfiguration(master_t *config) }*/ for (int profileId = 0; profileId < 2; profileId++) { - config->profile[profileId].pidProfile.P8[ROLL] = 95; + config->profile[profileId].pidProfile.P8[ROLL] = 60; config->profile[profileId].pidProfile.I8[ROLL] = 70; config->profile[profileId].pidProfile.D8[ROLL] = 17; - config->profile[profileId].pidProfile.P8[PITCH] = 120; + config->profile[profileId].pidProfile.P8[PITCH] = 80; config->profile[profileId].pidProfile.I8[PITCH] = 90; config->profile[profileId].pidProfile.D8[PITCH] = 18; config->profile[profileId].pidProfile.P8[YAW] = 200; config->profile[profileId].pidProfile.I8[YAW] = 45; config->profile[profileId].pidProfile.P8[PIDLEVEL] = 30; config->profile[profileId].pidProfile.D8[PIDLEVEL] = 30; - config->profile[profileId].pidProfile.levelSensitivity = 50; for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 100;