Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +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 // 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];
} }

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

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

14
src/main/mw.c
View file

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