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

Spool up prevention without airmode

Browse source
This commit is contained in:
borisbstyle 2016-02-20 21:25:01 +01:00
parent 390da2371e
commit 67d3d84a95
3 changed files with 34 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -67,6 +67,7 @@ static int32_t errorGyroI[3], errorGyroILimit[3];
static float errorGyroIf[3], errorGyroIfLimit[3];
static int32_t errorAngleI[2];
static float errorAngleIf[2];
static bool lowThrottlePidReduction;
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -85,8 +86,9 @@ void pidResetErrorAngle(void)
errorAngleIf[PITCH] = 0.0f;
}
void pidResetErrorGyro(void)
void pidResetErrorGyroState(uint8_t resetOption)
{
if (resetOption >= RESET_ITERM) {
int axis;
for (axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
@ -94,6 +96,13 @@ void pidResetErrorGyro(void)
}
}
if (resetOption == RESET_ITERM_AND_REDUCE_PID) {
lowThrottlePidReduction = true;
} else {
lowThrottlePidReduction = false;
}
}
void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
@ -242,6 +251,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (lowThrottlePidReduction) axisPID[axis] /= 4;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
@ -347,6 +358,8 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 4;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
@ -377,6 +390,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
axisPID[FD_YAW] = PTerm + ITerm;
if (lowThrottlePidReduction) axisPID[FD_YAW] /= 4;
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
@ -506,6 +520,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 4;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;

8
src/main/flight/pid.h
View file

@ -48,6 +48,12 @@ typedef enum {
DELTA_FROM_MEASUREMENT
} pidDeltaType_e;
typedef enum {
RESET_DISABLE = 0,
RESET_ITERM,
RESET_ITERM_AND_REDUCE_PID
} pidErrorResetOption_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s {
@ -77,5 +83,5 @@ bool antiWindupProtection;
void pidSetController(pidControllerType_e type);
void pidResetErrorAngle(void);
void pidResetErrorGyro(void);
void pidResetErrorGyroState(uint8_t resetOption);

7
src/main/mw.c
View file

@ -460,10 +460,15 @@ void processRx(void)
antiWindupProtection = false;
}
} else {
pidResetErrorGyro();
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
pidResetErrorGyroState(RESET_ITERM);
} else {
pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID);
}
pidResetErrorAngle();
}
} else {
pidResetErrorGyroState(RESET_DISABLE);
antiWindupProtection = false;
}