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

Replace median by dynamic moving average

Browse source
This commit is contained in:
borisbstyle 2015-12-28 00:47:30 +01:00
parent 6d4dfcf58c
commit 63f88cc208
1 changed files with 26 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -63,6 +63,8 @@ uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static int32_t errorGyroI[3] = { 0, 0, 0 }; static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static uint8_t deltaTotalSamples = 0;
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, static void pidRewrite(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);
@ -82,6 +84,14 @@ void pidResetErrorGyro(void)
errorGyroIf[YAW] = 0.0f; errorGyroIf[YAW] = 0.0f;
} }
void setPidDeltaSamples(void) {
if (targetLooptime < 1000) {
deltaTotalSamples = 8;
} else {
deltaTotalSamples = 4;
}
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t DTermState[3]; static filterStatePt1_t DTermState[3];
@ -94,12 +104,14 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos; int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3]; static float lastError[3];
static float deltaOld[3][9]; static float previousDelta[3][8];
float delta, deltaSum; float delta, deltaSum;
int axis, deltaCount; int axis, deltaCount;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaTotalSamples) setPidDeltaSamples();
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions // Figure out the raw stick positions
@ -193,16 +205,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// would be scaled by different dt each time. Division by dT fixes that. // would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT); delta *= (1.0f / dT);
// Apply median filter for averaging // Apply moving average
for (deltaCount = 8; deltaCount > 0; deltaCount--) { for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1]; previousDelta[axis][0] = delta;
} for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
deltaOld[axis][0] = delta; deltaSum = (deltaSum / deltaTotalSamples);
if (targetLooptime < 1000){
deltaSum = quickMedianFilter9f(deltaOld[axis]);
} else {
deltaSum = quickMedianFilter7f(deltaOld[axis]);
}
if (pidProfile->dterm_cut_hz) { if (pidProfile->dterm_cut_hz) {
// Dterm low pass // Dterm low pass
@ -236,7 +243,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t errorAngle; int32_t errorAngle;
int axis, deltaCount; int axis, deltaCount;
int32_t delta, deltaSum; int32_t delta, deltaSum;
static int32_t deltaOld[3][9]; static int32_t previousDelta[3][8];
int32_t PTerm, ITerm, DTerm; int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 }; static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 }; static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
@ -245,6 +252,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
int32_t stickPosAil, stickPosEle, mostDeflectedPos; int32_t stickPosAil, stickPosEle, mostDeflectedPos;
if (!deltaTotalSamples) setPidDeltaSamples();
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions // Figure out the raw stick positions
@ -338,18 +347,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// would be scaled by different dt each time. Division by dT fixes that. // would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
// Apply median filter for averaging // Apply moving average
for (deltaCount = 8; deltaCount > 0; deltaCount--) { for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1]; previousDelta[axis][0] = delta;
} for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
deltaOld[axis][0] = delta; deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
if (targetLooptime < 1000){
deltaSum = quickMedianFilter9(deltaOld[axis]);
} else {
deltaSum = quickMedianFilter7(deltaOld[axis]);
}
deltaSum *= 3; // Get same scaling
if (pidProfile->dterm_cut_hz) { if (pidProfile->dterm_cut_hz) {
// Dterm delta low pass // Dterm delta low pass