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

3D Airmode enhancements // Iterm Shrinking replaced to limiting

Browse source
This commit is contained in:
borisbstyle 2016-01-25 12:05:30 +01:00
parent b86f304dec
commit 55cf3913a0
3 changed files with 15 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/flight/mixer.c
View file

@ -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];
}

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

@ -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 {

16
src/main/mw.c
View file

@ -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