From 55cf3913a005d4d376af35f2b7f4e3a386a8f795 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 25 Jan 2016 12:05:30 +0100 Subject: [PATCH] 3D Airmode enhancements // Iterm Shrinking replaced to limiting --- src/main/flight/mixer.c | 4 +--- src/main/flight/pid.c | 16 ++++++---------- src/main/mw.c | 16 ++++++++-------- 3 files changed, 15 insertions(+), 21 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d046856a6f..9a425c46e3 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -452,7 +452,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura } // in 3D mode, mixer gain has to be halved - if (feature(FEATURE_3D) && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) { + if (feature(FEATURE_3D)) { if (motorCount > 1) { for (i = 0; i < motorCount; i++) { currentMixer[i].pitch *= 0.5f; @@ -782,8 +782,6 @@ void mixTable(void) axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; - if (feature(FEATURE_3D)) rollPitchYawMix[i] /= 2; // 3D feature uses half of the resolution - if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 83ba25b2d6..aad6bfa7cd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -50,7 +50,7 @@ extern float dT; extern bool motorLimitReached; -extern bool allowITermShrinkOnly; +extern bool preventItermWindup; int16_t axisPID[3]; @@ -197,11 +197,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler; } - if (allowITermShrinkOnly || motorLimitReached) { - if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) { - previousErrorGyroIf[axis] = errorGyroIf[axis]; - } else { - errorGyroIf[axis] = constrain(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis])); + if (preventItermWindup || motorLimitReached) { + if (ABS(errorGyroIf[axis]) > ABS(previousErrorGyroIf[axis])) { + errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis])); } } else { previousErrorGyroIf[axis] = errorGyroIf[axis]; @@ -338,10 +336,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler; } - if (allowITermShrinkOnly || motorLimitReached) { - if (ABS(errorGyroI[axis]) < ABS(previousErrorGyroI[axis])) { - previousErrorGyroI[axis] = errorGyroI[axis]; - } else { + if (preventItermWindup || motorLimitReached) { + if (ABS(errorGyroI[axis]) > ABS(previousErrorGyroI[axis])) { errorGyroI[axis] = constrain(errorGyroI[axis], -ABS(previousErrorGyroI[axis]), ABS(previousErrorGyroI[axis])); } } else { diff --git a/src/main/mw.c b/src/main/mw.c index 02dc73c281..c7782ec42c 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -102,7 +102,7 @@ enum { // AIR MODE Reset timers #define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000) // 1 sec delay to disable AIR MODE Iterm resetting -bool allowITermShrinkOnly = false; +bool preventItermWindup = false; static bool ResetErrorActivated = true; uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop @@ -486,9 +486,9 @@ void processRx(void) // 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 (calculateRollPitchCenterStatus(&masterConfig.rxConfig) == CENTERED) { - allowITermShrinkOnly = true; // Iterm is now only allowed to shrink + preventItermWindup = true; // Iterm is now limited to the last value } else { - allowITermShrinkOnly = false; // Iterm should considered safe to increase + preventItermWindup = false; // Iterm should considered safe to increase } } @@ -497,22 +497,22 @@ void processRx(void) ResetErrorActivated = true; // As RX code is not executed each loop a flag has to be set for fast looptimes airModeErrorResetTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer airModeErrorResetIsEnabled = true; // Enable Reset again especially after Disarm - allowITermShrinkOnly = false; // Reset shrinking + preventItermWindup = false; // Reset limiting } } else { if (!(feature(FEATURE_MOTOR_STOP)) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) { if (airModeErrorResetIsEnabled) { if (millis() > airModeErrorResetTimeout && calculateRollPitchCenterStatus(&masterConfig.rxConfig) == NOT_CENTERED) { // Only disable error reset when roll and pitch not centered airModeErrorResetIsEnabled = false; - allowITermShrinkOnly = false; // Reset shrinking for Iterm + preventItermWindup = false; // Reset limiting for Iterm } } else { - allowITermShrinkOnly = false; // Reset shrinking for Iterm + preventItermWindup = false; // Reset limiting for Iterm } } else { - allowITermShrinkOnly = false; // Reset shrinking. Usefull when flipping between normal and AIR mode + preventItermWindup = false; // Reset limiting. Usefull when flipping between normal and AIR mode } - ResetErrorActivated = false; // Disable resetting of error + ResetErrorActivated = false; // Disable resetting of error } // When armed and motors aren't spinning, do beeps and then disarm