mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-22 15:55:48 +03:00
3D Airmode enhancements // Iterm Shrinking replaced to limiting
This commit is contained in:
parent
b86f304dec
commit
55cf3913a0
3 changed files with 15 additions and 21 deletions
|
@ -452,7 +452,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
||||||
}
|
}
|
||||||
|
|
||||||
// in 3D mode, mixer gain has to be halved
|
// 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) {
|
if (motorCount > 1) {
|
||||||
for (i = 0; i < motorCount; i++) {
|
for (i = 0; i < motorCount; i++) {
|
||||||
currentMixer[i].pitch *= 0.5f;
|
currentMixer[i].pitch *= 0.5f;
|
||||||
|
@ -782,8 +782,6 @@ void mixTable(void)
|
||||||
axisPID[ROLL] * currentMixer[i].roll +
|
axisPID[ROLL] * currentMixer[i].roll +
|
||||||
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
|
-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] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
|
||||||
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
|
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
|
||||||
}
|
}
|
||||||
|
|
|
@ -50,7 +50,7 @@
|
||||||
|
|
||||||
extern float dT;
|
extern float dT;
|
||||||
extern bool motorLimitReached;
|
extern bool motorLimitReached;
|
||||||
extern bool allowITermShrinkOnly;
|
extern bool preventItermWindup;
|
||||||
|
|
||||||
int16_t axisPID[3];
|
int16_t axisPID[3];
|
||||||
|
|
||||||
|
@ -197,11 +197,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler;
|
errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (allowITermShrinkOnly || motorLimitReached) {
|
if (preventItermWindup || motorLimitReached) {
|
||||||
if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) {
|
if (ABS(errorGyroIf[axis]) > ABS(previousErrorGyroIf[axis])) {
|
||||||
previousErrorGyroIf[axis] = errorGyroIf[axis];
|
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis]));
|
||||||
} else {
|
|
||||||
errorGyroIf[axis] = constrain(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis]));
|
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
previousErrorGyroIf[axis] = errorGyroIf[axis];
|
previousErrorGyroIf[axis] = errorGyroIf[axis];
|
||||||
|
@ -338,10 +336,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
||||||
errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler;
|
errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (allowITermShrinkOnly || motorLimitReached) {
|
if (preventItermWindup || motorLimitReached) {
|
||||||
if (ABS(errorGyroI[axis]) < ABS(previousErrorGyroI[axis])) {
|
if (ABS(errorGyroI[axis]) > ABS(previousErrorGyroI[axis])) {
|
||||||
previousErrorGyroI[axis] = errorGyroI[axis];
|
|
||||||
} else {
|
|
||||||
errorGyroI[axis] = constrain(errorGyroI[axis], -ABS(previousErrorGyroI[axis]), ABS(previousErrorGyroI[axis]));
|
errorGyroI[axis] = constrain(errorGyroI[axis], -ABS(previousErrorGyroI[axis]), ABS(previousErrorGyroI[axis]));
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -102,7 +102,7 @@ enum {
|
||||||
|
|
||||||
// AIR MODE Reset timers
|
// AIR MODE Reset timers
|
||||||
#define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000) // 1 sec delay to disable AIR MODE Iterm resetting
|
#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;
|
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
|
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
|
// 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 ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !airModeErrorResetIsEnabled) {
|
||||||
if (calculateRollPitchCenterStatus(&masterConfig.rxConfig) == CENTERED) {
|
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 {
|
} else {
|
||||||
allowITermShrinkOnly = false; // Iterm should considered safe to increase
|
preventItermWindup = false; // Iterm should considered safe to increase
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -497,20 +497,20 @@ void processRx(void)
|
||||||
ResetErrorActivated = true; // As RX code is not executed each loop a flag has to be set for fast looptimes
|
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
|
airModeErrorResetTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
|
||||||
airModeErrorResetIsEnabled = true; // Enable Reset again especially after Disarm
|
airModeErrorResetIsEnabled = true; // Enable Reset again especially after Disarm
|
||||||
allowITermShrinkOnly = false; // Reset shrinking
|
preventItermWindup = false; // Reset limiting
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
if (!(feature(FEATURE_MOTOR_STOP)) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
|
if (!(feature(FEATURE_MOTOR_STOP)) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
|
||||||
if (airModeErrorResetIsEnabled) {
|
if (airModeErrorResetIsEnabled) {
|
||||||
if (millis() > airModeErrorResetTimeout && calculateRollPitchCenterStatus(&masterConfig.rxConfig) == NOT_CENTERED) { // Only disable error reset when roll and pitch not centered
|
if (millis() > airModeErrorResetTimeout && calculateRollPitchCenterStatus(&masterConfig.rxConfig) == NOT_CENTERED) { // Only disable error reset when roll and pitch not centered
|
||||||
airModeErrorResetIsEnabled = false;
|
airModeErrorResetIsEnabled = false;
|
||||||
allowITermShrinkOnly = false; // Reset shrinking for Iterm
|
preventItermWindup = false; // Reset limiting for Iterm
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
allowITermShrinkOnly = false; // Reset shrinking for Iterm
|
preventItermWindup = false; // Reset limiting for Iterm
|
||||||
}
|
}
|
||||||
} else {
|
} 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
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue