rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

D filter rewrite to FIR

This commit is contained in:
borisbstyle 2016-01-10 02:55:21 +01:00
parent 9f2de6f46c
commit 2ee55ece3c
5 changed files with 59 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
src/main/common/filter.c
View File

@ -38,37 +38,50 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
return filter->state;
}
static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 }; // looptime=500;
static int8_t gyroFIRCoeff_1000[FILTER_TAPS] = { 0, 0, 0, 0, 0, 0, 12, 23, 40, 51, 52, 40, 38 }; // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 0, 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 }; // looptime=500;
static int8_t gyroFIRCoeff_1000[FILTER_TAPS] = { 0, 0, 0, 0, 0, 0, 0, 12, 23, 40, 51, 52, 40, 38 }; // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime)
static int8_t deltaFIRCoeff_500[FILTER_TAPS] = {0, 0, 0, 0, 0, 18, 12, 28, 40, 44, 40, 32, 22, 20};
static int8_t deltaFIRCoeff_1000[FILTER_TAPS] = {36, 12, 14, 14, 16, 16, 18, 18, 18, 16, 16, 14, 12, 36};
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_type, uint32_t targetLooptime)
{
if (filter_level == 0) {
return NULL;
}
int8_t *filterCoeff;
// filter for 2kHz looptime
if (targetLooptime == 500) {
return gyroFIRCoeff_500;
} else { // filter for 1kHz looptime
return gyroFIRCoeff_1000;
switch(filter_type){
case(0):
filterCoeff = NULL;
break;
case(1):
if (targetLooptime == 500) {
filterCoeff = gyroFIRCoeff_500;
} else { // filter for 1kHz looptime
filterCoeff = gyroFIRCoeff_1000;
}
break;
case(2):
if (targetLooptime == 500) {
filterCoeff = deltaFIRCoeff_500;
} else { // filter for 1kHz looptime
filterCoeff = deltaFIRCoeff_1000;
}
}
return filterCoeff;
}
// Thanks to Qcopter & BorisB & DigitalEntity
void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS])
void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS])
{
int32_t FIRsum;
int axis, i;
FIRsum = 0;
int i;
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
FIRsum = 0;
for (i = 0; i <= FILTER_TAPS-2; i++) {
state[axis][i] = state[axis][i + 1];
FIRsum += state[axis][i] * (int16_t)coeff[i];
}
state[axis][FILTER_TAPS-1] = data[axis];
FIRsum += state[axis][FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
data[axis] = FIRsum / 256;
for (i = 0; i <= FILTER_TAPS-2; i++) {
state[i] = state[i + 1];
FIRsum += state[i] * (int16_t)coeff[i];
}
state[FILTER_TAPS-1] = *data;
FIRsum += state[FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
*data = FIRsum / 256;
}

4
src/main/common/filter.h
View File

@ -15,7 +15,7 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#define FILTER_TAPS 13
#define FILTER_TAPS 14
typedef struct filterStatePt1_s {
float state;
@ -25,4 +25,4 @@ typedef struct filterStatePt1_s {
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime);
void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS]);
void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS]);

86
src/main/flight/pid.c
View File

@ -64,8 +64,6 @@ uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static uint8_t deltaTotalSamples = 0;
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -85,18 +83,6 @@ void pidResetErrorGyro(void)
errorGyroIf[YAW] = 0.0f;
}
void setPidDeltaSamples(uint8_t filter) {
if (!filter) {
if (targetLooptime < 1000) {
deltaTotalSamples = 8;
} else {
deltaTotalSamples = 4;
}
} else {
deltaTotalSamples = 1;
}
}
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
@ -137,23 +123,22 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, f
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t DTermState[3];
static airModePlus_t airModePlusAxisState[3];
static int8_t * deltaFIRTable = 0L;
static int16_t deltaFIRState[3][FILTER_TAPS];
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastGyroRate[3];
static float lastError[3];
static float previousDelta[3][8];
float delta, deltaSum;
int axis, deltaCount;
float delta;
int axis;
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -234,33 +219,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITerm = errorGyroIf[axis];
//-----calculate D-term
if (pidProfile->delta_from_gyro_error || axis == YAW) {
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
} else {
delta = RateError - lastError[axis];
lastError[axis] = RateError;
}
delta = RateError - lastError[axis];
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
deltaSum = 0;
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
/* Delta filtering is in integers, which should be fine */
int16_t deltaTemp = (int16_t) delta;
filterApplyFIR(&deltaTemp, deltaFIRState[axis], deltaFIRTable);
delta = (float) deltaTemp;
// Apply moving average
if (deltaTotalSamples > 1) {
for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
deltaSum = (deltaSum / deltaTotalSamples);
}
DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
@ -288,18 +260,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
{
UNUSED(rxConfig);
int axis, deltaCount;
int32_t delta, deltaSum;
static int32_t previousDelta[3][8];
int axis;
int16_t delta;
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t lastGyroRate[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -383,32 +353,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
}
//-----calculate D-term
if (pidProfile->delta_from_gyro_error || axis == YAW) { // quick and dirty solution for testing
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
} else {
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
}
delta = (int16_t) RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
// Apply moving average
deltaSum = 0;
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
if (deltaTotalSamples > 1) {
for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
}
deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
filterApplyFIR(&delta, deltaFIRState[axis], deltaFIRTable);
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
DTerm = (delta * 2 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;

2
src/main/io/serial_cli.c
View File

@ -656,8 +656,6 @@ const clivalue_t valueTable[] = {
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
{ "delta_from_gyro_error", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} },
{ "insane_acrobility_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 30 } },
#ifdef BLACKBOX

3
src/main/sensors/gyro.c
View File

@ -127,7 +127,8 @@ void gyroUpdate(void)
}
if (gyroFIRTable) {
filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable);
int axis;
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
}
alignSensors(gyroADC, gyroADC, gyroAlign);