Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 23:05:19 +03:00
Code Issues Wiki Activity

Replace CALC_OFFSET with ABS

Browse source
This commit is contained in:
borisbstyle 2015-12-11 00:44:11 +01:00
parent 0e37cb3278
commit 39c455a3eb
2 changed files with 5 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/filter.c
View file

@ -29,7 +29,7 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
// 7 Tap FIR filter as described here:
// Thanks to Qcopter
void filterApply7TapFIR(int16_t data[]) {
void filterApplyFIR(int16_t data[]) {
int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
int32_t FIRsum;

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

@ -51,8 +51,6 @@ extern float dT;
extern float totalErrorRatioLimit;
extern bool allowITermShrinkOnly;
#define CALC_OFFSET(x) ( (x > 0) ? x : -x )
int16_t axisPID[3];
#ifdef BLACKBOX
@ -178,10 +176,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
if (CALC_OFFSET(errorGyroIf[axis]) < CALC_OFFSET(previousErrorGyroIf[axis])) {
if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) {
previousErrorGyroIf[axis] = errorGyroIf[axis];
} else {
errorGyroIf[axis] = constrain(errorGyroIf[axis], -CALC_OFFSET(previousErrorGyroIf[axis]), CALC_OFFSET(previousErrorGyroIf[axis]));
errorGyroIf[axis] = constrain(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis]));
}
} else {
previousErrorGyroIf[axis] = errorGyroIf[axis];
@ -329,10 +327,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
ITerm = errorGyroI[axis] >> 13;
if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
if (CALC_OFFSET(errorGyroI[axis]) < CALC_OFFSET(previousErrorGyroI[axis])) {
if (ABS(errorGyroI[axis]) < ABS(previousErrorGyroI[axis])) {
previousErrorGyroI[axis] = errorGyroI[axis];
} else {
errorGyroI[axis] = constrain(errorGyroI[axis], -CALC_OFFSET(previousErrorGyroI[axis]), CALC_OFFSET(previousErrorGyroI[axis]));
errorGyroI[axis] = constrain(errorGyroI[axis], -ABS(previousErrorGyroI[axis]), ABS(previousErrorGyroI[axis]));
}
} else {
previousErrorGyroI[axis] = errorGyroI[axis];