Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +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

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

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

@ -48,6 +48,12 @@ typedef enum {
DELTA_FROM_MEASUREMENT DELTA_FROM_MEASUREMENT
} pidDeltaType_e; } 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) #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s { typedef struct pidProfile_s {
@ -77,5 +83,5 @@ bool antiWindupProtection;
void pidSetController(pidControllerType_e type); void pidSetController(pidControllerType_e type);
void pidResetErrorAngle(void); 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; antiWindupProtection = false;
} }
} else { } else {
pidResetErrorGyro(); if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
pidResetErrorGyroState(RESET_ITERM);
} else {
pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID);
}
pidResetErrorAngle(); pidResetErrorAngle();
} }
} else { } else {
pidResetErrorGyroState(RESET_DISABLE);
antiWindupProtection = false; antiWindupProtection = false;
} }