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

Merge pull request #10757 from KarateBrot/biquadCrossfeed 

Add biquad crossfeeding + RPM filter fix
This commit is contained in:
J Blackman 2021-07-18 13:31:20 +10:00 committed by GitHub
parent 3d5a0e585c 48f14686d5
commit 6d286e25f1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 76 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/cli/settings.c
View File

@ -1659,6 +1659,7 @@ const clivalue_t valueTable[] = {
{ "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) },
{ "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) },
{ "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) },
{ "gyro_rpm_notch_fade_range_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_fade_range_hz) },
{ "rpm_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) },
#endif

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

@ -165,19 +165,19 @@ float filterGetNotchQ(float centerFreq, float cutoffFreq)
/* sets up a biquad filter as a 2nd order butterworth LPF */
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
}
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
{
biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType);
biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType, weight);
// zero initial samples
filter->x1 = filter->x2 = 0;
filter->y1 = filter->y2 = 0;
}
FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
{
// setup variables
const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
@ -219,11 +219,14 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
filter->b2 /= a0;
filter->a1 /= a0;
filter->a2 /= a0;
// update weight
filter->weight = weight;
}
FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{
biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
}
/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
@ -243,12 +246,24 @@ FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
return result;
}
/* Computes a biquadFilter_t filter in df1 and crossfades input with output */
FAST_CODE float biquadFilterApplyDF1Weighted(biquadFilter_t* filter, float input)
{
// compute result
const float result = biquadFilterApplyDF1(filter, input);
// crossfading of input and output to turn filter on/off gradually
return filter->weight * result + (1 - filter->weight) * input;
}
/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->x1;
filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
filter->x2 = filter->b2 * input - filter->a2 * result;
return result;
}

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

@ -52,6 +52,7 @@ typedef struct slewFilter_s {
typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2;
float x1, x2, y1, y2;
float weight;
} biquadFilter_t;
typedef struct laggedMovingAverage_s {
@ -80,11 +81,12 @@ typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
float nullFilterApply(filter_t *filter, float input);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq);

2
src/main/flight/gyroanalyse.c
View File

@ -329,7 +329,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH, 1.0f);
}
}

2
src/main/flight/pid_init.c
View File

@ -95,7 +95,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH, 1.0f);
}
} else {
pidRuntime.dtermNotchApplyFn = nullFilterApply;

70
src/main/flight/rpm_filter.c
View File

@ -52,57 +52,63 @@
static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS];
typedef struct rpmNotchFilter_s
{
uint8_t harmonics;
float minHz;
float maxHz;
float q;
float loopTime;
typedef struct rpmNotchFilter_s {
uint8_t harmonics;
float minHz;
float maxHz;
float fadeRangeHz;
float q;
uint32_t looptime;
biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
} rpmNotchFilter_t;
FAST_DATA_ZERO_INIT static float erpmToHz;
FAST_DATA_ZERO_INIT static float filteredMotorErpm[MAX_SUPPORTED_MOTORS];
FAST_DATA_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS];
FAST_DATA_ZERO_INIT static float minMotorFrequency;
FAST_DATA_ZERO_INIT static uint8_t numberFilters;
FAST_DATA_ZERO_INIT static uint8_t numberRpmNotchFilters;
FAST_DATA_ZERO_INIT static uint8_t filterUpdatesPerIteration;
FAST_DATA_ZERO_INIT static float pidLooptime;
FAST_DATA_ZERO_INIT static rpmNotchFilter_t filters[2];
FAST_DATA_ZERO_INIT static rpmNotchFilter_t* gyroFilter;
FAST_DATA_ZERO_INIT static rpmNotchFilter_t *gyroFilter;
FAST_DATA_ZERO_INIT static uint8_t currentMotor;
FAST_DATA_ZERO_INIT static uint8_t currentHarmonic;
FAST_DATA_ZERO_INIT static uint8_t currentFilterNumber;
FAST_DATA static rpmNotchFilter_t* currentFilter = &filters[0];
FAST_DATA static rpmNotchFilter_t *currentFilter = &filters[0];
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 4);
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 5);
void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
{
config->gyro_rpm_notch_harmonics = 3;
config->gyro_rpm_notch_min = 100;
config->gyro_rpm_notch_fade_range_hz = 50;
config->gyro_rpm_notch_q = 500;
config->rpm_lpf = 150;
}
static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minHz, int q, float looptime)
static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const uint32_t looptime)
{
filter->harmonics = harmonics;
filter->minHz = minHz;
filter->q = q / 100.0f;
filter->loopTime = looptime;
filter->harmonics = config->gyro_rpm_notch_harmonics;
filter->minHz = config->gyro_rpm_notch_min;
filter->maxHz = 0.48f * 1e6f / looptime; // don't go quite to nyquist to avoid oscillations
filter->fadeRangeHz = config->gyro_rpm_notch_fade_range_hz;
filter->q = config->gyro_rpm_notch_q / 100.0f;
filter->looptime = looptime;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < harmonics; i++) {
for (int i = 0; i < filter->harmonics; i++) {
biquadFilterInit(
&filter->notch[axis][motor][i], minHz * i, looptime, filter->q, FILTER_NOTCH);
&filter->notch[axis][motor][i], filter->minHz * i, filter->looptime, filter->q, FILTER_NOTCH, 0.0f);
}
}
}
@ -122,10 +128,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
pidLooptime = gyro.targetLooptime;
if (config->gyro_rpm_notch_harmonics) {
gyroFilter = &filters[numberRpmNotchFilters++];
rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics,
config->gyro_rpm_notch_min, config->gyro_rpm_notch_q, gyro.targetLooptime);
// don't go quite to nyquist to avoid oscillations
gyroFilter->maxHz = 0.48f / (gyro.targetLooptime * 1e-6f);
rpmNotchFilterInit(gyroFilter, config, pidLooptime);
} else {
gyroFilter = NULL;
}
@ -142,26 +145,24 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
}
static float applyFilter(rpmNotchFilter_t* filter, int axis, float value)
static float applyFilter(rpmNotchFilter_t *filter, const int axis, float value)
{
if (filter == NULL) {
return value;
}
for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < filter->harmonics; i++) {
value = biquadFilterApplyDF1(&filter->notch[axis][motor][i], value);
value = biquadFilterApplyDF1Weighted(&filter->notch[axis][motor][i], value);
}
}
return value;
}
float rpmFilterGyro(int axis, float value)
float rpmFilterGyro(const int axis, float value)
{
return applyFilter(gyroFilter, axis, value);
}
FAST_DATA_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS];
FAST_CODE_NOINLINE void rpmFilterUpdate()
{
if (gyroFilter == NULL) {
@ -177,23 +178,33 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
}
for (int i = 0; i < filterUpdatesPerIteration; i++) {
float frequency = constrainf(
(currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz);
biquadFilter_t* template = &currentFilter->notch[0][currentMotor][currentHarmonic];
biquadFilter_t *template = &currentFilter->notch[0][currentMotor][currentHarmonic];
// uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
/* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */
// fade out notch when approaching minHz (turn it off)
float weight = 1.0f;
if (frequency < currentFilter->minHz + currentFilter->fadeRangeHz) {
weight = (frequency - currentFilter->minHz) / currentFilter->fadeRangeHz;
}
biquadFilterUpdate(
template, frequency, currentFilter->loopTime, currentFilter->q, FILTER_NOTCH);
template, frequency, currentFilter->looptime, currentFilter->q, FILTER_NOTCH, weight);
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilter_t* clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
clone->b0 = template->b0;
clone->b1 = template->b1;
clone->b2 = template->b2;
clone->a1 = template->a1;
clone->a2 = template->a2;
clone->weight = template->weight;
}
if (++currentHarmonic == currentFilter->harmonics) {
@ -207,7 +218,6 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
}
currentFilter = &filters[currentFilterNumber];
}
}
}

11
src/main/flight/rpm_filter.h
View File

@ -25,17 +25,18 @@
typedef struct rpmFilterConfig_s
{
uint8_t gyro_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off
uint8_t gyro_rpm_notch_min; // minimum frequency of the notches
uint16_t gyro_rpm_notch_q; // q of the notches
uint8_t gyro_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off
uint8_t gyro_rpm_notch_min; // minimum frequency of the notches
uint16_t gyro_rpm_notch_fade_range_hz; // range in which to gradually turn off notches down to minHz
uint16_t gyro_rpm_notch_q; // q of the notches
uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm
uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm
} rpmFilterConfig_t;
PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
void rpmFilterInit(const rpmFilterConfig_t *config);
float rpmFilterGyro(int axis, float values);
float rpmFilterGyro(const int axis, float value);
void rpmFilterUpdate();
bool isRpmFilterEnabled(void);
float rpmMinMotorFrequency();

6
src/main/sensors/gyro_init.c
View File

@ -109,7 +109,7 @@ static void gyroInitFilterNotch1(uint16_t notchHz, uint16_t notchCutoffHz)
gyro.notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyro.notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyro.notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
}
}
}
@ -124,7 +124,7 @@ static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
gyro.notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyro.notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyro.notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
}
}
}
@ -141,7 +141,7 @@ static void gyroInitFilterDynamicNotch()
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
}
}
}