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

Iterm Shrink Only Option for AIR Mode

Browse source 
Fix previous Iterm
This commit is contained in:
borisbstyle 2015-12-08 15:50:02 +01:00
parent b84e9f4676
commit 66fb254d62
2 changed files with 35 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,6 +49,9 @@
extern float dT;
extern float rpy_limiting;
extern bool allowITermShrinkOnly;
#define CALC_OFFSET(x) ( (x > 0) ? x : -x )
int16_t axisPID[3];
@ -96,6 +99,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float delta;
int axis;
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (FLIGHT_MODE(HORIZON_MODE)) {
@ -172,6 +176,16 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (allowITermShrinkOnly) {
if (CALC_OFFSET(errorGyroIf[axis]) < CALC_OFFSET(previousErrorGyroIf[axis])) {
previousErrorGyroIf[axis] = errorGyroIf[axis];
} else {
errorGyroIf[axis] = constrain(errorGyroIf[axis], -CALC_OFFSET(previousErrorGyroIf[axis]), CALC_OFFSET(previousErrorGyroIf[axis]));
}
} else {
previousErrorGyroIf[axis] = errorGyroIf[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
ITerm = errorGyroIf[axis];
@ -218,6 +232,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t delta;
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError;
int8_t horizonLevelStrength = 100;
@ -299,8 +314,19 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// 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);
ITerm = errorGyroI[axis] >> 13;
if (allowITermShrinkOnly) {
if (CALC_OFFSET(errorGyroI[axis]) < CALC_OFFSET(previousErrorGyroI[axis])) {
previousErrorGyroI[axis] = errorGyroI[axis];
} else {
errorGyroIf[axis] = constrain(errorGyroIf[axis], -CALC_OFFSET(previousErrorGyroI[axis]), CALC_OFFSET(previousErrorGyroI[axis]));
}
} else {
previousErrorGyroI[axis] = errorGyroI[axis];
}
//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;

21
src/main/mw.c
View file

@ -103,7 +103,7 @@ enum {
// AIR MODE Reset timers
#define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000) // 1 sec delay to disable AIR MODE Iterm resetting
#define ERROR_RESET_ACTIVATE_DELAY (5 * 1000) // 5 sec delay to enable AIR MODE Iterm resetting
bool allowITermShrinkOnly = false;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
@ -569,32 +569,29 @@ void processRx(void)
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
static bool airModeErrorResetIsEnabled = true; // Should always initialize with reset enabled
static uint32_t airModeErrorResetToggleTimeout = 0; // Timeout for both activate and deactivate mode
static bool airModeErrorResetIsEnabled = true; // Should always initialize with reset enabled
static uint32_t airModeErrorResetTimeout = 0; // Timeout for both activate and deactivate mode
if (throttleStatus == THROTTLE_LOW) {
// Evaluate if the error needs resetting again. Should be entered when in air mode during landings etc...
// When in AIR Mode LOW Throttle and reset was already disabled we will only prevent further growing
if ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !airModeErrorResetIsEnabled) {
if ((millis() > airModeErrorResetToggleTimeout) && isRollPitchCentered()) {
airModeErrorResetIsEnabled = true;
if (isRollPitchCentered()) {
allowITermShrinkOnly = true; // Iterm is now only allowed to shrink
}
}
// Conditions to reset Error
if (!IS_RC_MODE_ACTIVE(BOXARM) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled)) {
pidResetErrorGyro();
airModeErrorResetToggleTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
airModeErrorResetTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
}
} else {
if (!(feature(FEATURE_MOTOR_STOP)) && IS_RC_MODE_ACTIVE(BOXARM) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (airModeErrorResetIsEnabled) {
if ((millis() > airModeErrorResetToggleTimeout) && !isRollPitchCentered()) { // Only disable error reset when roll and pitch not centered
if ((millis() > airModeErrorResetTimeout) && !isRollPitchCentered()) { // Only disable error reset when roll and pitch not centered
airModeErrorResetIsEnabled = false;
airModeErrorResetToggleTimeout = millis() + ERROR_RESET_ACTIVATE_DELAY; // Start reset activate timer
allowITermShrinkOnly = false; // Reset shrinking for Iterm
}
} else if (!airModeErrorResetIsEnabled) {
airModeErrorResetToggleTimeout = millis() + ERROR_RESET_ACTIVATE_DELAY; // Reset activate timer
}
} else {
airModeErrorResetIsEnabled = true; // This is needed to reset procedure when switch is toggled in air