Merge pull request #3983 from martinbudden/bf_slew_filter
Added slew filter
This commit is contained in:
commit
5c6f110d0a
|
@ -67,6 +67,32 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
|
|||
return filter->state;
|
||||
}
|
||||
|
||||
// Slew filter with limit
|
||||
|
||||
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
|
||||
{
|
||||
filter->state = 0.0f;
|
||||
filter->slewLimit = slewLimit;
|
||||
filter->threshold = threshold;
|
||||
}
|
||||
|
||||
float slewFilterApply(slewFilter_t *filter, float input)
|
||||
{
|
||||
if (filter->state >= filter->threshold) {
|
||||
if (input >= filter->state - filter->slewLimit) {
|
||||
filter->state = input;
|
||||
}
|
||||
} else if (filter->state <= -filter->threshold) {
|
||||
if (input <= filter->state + filter->slewLimit) {
|
||||
filter->state = input;
|
||||
}
|
||||
} else {
|
||||
filter->state = input;
|
||||
}
|
||||
return filter->state;
|
||||
}
|
||||
|
||||
|
||||
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) {
|
||||
float octaves = log2f((float) centerFreq / (float) cutoff) * 2;
|
||||
return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
|
||||
|
|
|
@ -32,6 +32,12 @@ typedef struct pt1Filter_s {
|
|||
float dT;
|
||||
} pt1Filter_t;
|
||||
|
||||
typedef struct slewFilter_s {
|
||||
float state;
|
||||
float slewLimit;
|
||||
float threshold;
|
||||
} slewFilter_t;
|
||||
|
||||
/* this holds the data required to update samples thru a filter */
|
||||
typedef struct biquadFilter_s {
|
||||
float b0, b1, b2, a1, a2;
|
||||
|
@ -49,7 +55,8 @@ typedef struct firFilterDenoise_s{
|
|||
typedef enum {
|
||||
FILTER_PT1 = 0,
|
||||
FILTER_BIQUAD,
|
||||
FILTER_FIR
|
||||
FILTER_FIR,
|
||||
FILTER_SLEW
|
||||
} filterType_e;
|
||||
|
||||
typedef enum {
|
||||
|
@ -86,6 +93,9 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
|
|||
float pt1FilterApply(pt1Filter_t *filter, float input);
|
||||
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
|
||||
|
||||
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
|
||||
float slewFilterApply(slewFilter_t *filter, float input);
|
||||
|
||||
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
|
||||
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
|
||||
void firFilterUpdate(firFilter_t *filter, float input);
|
||||
|
|
|
@ -145,3 +145,56 @@ TEST(FilterUnittest, TestFirFilterApply)
|
|||
expected = 7.0f * 26.0f + 6.0 * 27.0 + 5.0 * 28.0 + 4.0f * 29.0f;
|
||||
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
|
||||
}
|
||||
|
||||
TEST(FilterUnittest, TestSlewFilterInit)
|
||||
{
|
||||
slewFilter_t filter;
|
||||
|
||||
slewFilterInit(&filter, 0.0f, 0.0f);
|
||||
EXPECT_EQ(0, filter.state);
|
||||
EXPECT_EQ(0, filter.slewLimit);
|
||||
EXPECT_EQ(0, filter.threshold);
|
||||
|
||||
slewFilterInit(&filter, 1800.0f, 1900.0f);
|
||||
EXPECT_EQ(0, filter.state);
|
||||
EXPECT_EQ(1800, filter.slewLimit);
|
||||
EXPECT_EQ(1900, filter.threshold);
|
||||
}
|
||||
|
||||
TEST(FilterUnittest, TestSlewFilter)
|
||||
{
|
||||
slewFilter_t filter;
|
||||
slewFilterInit(&filter, 2000.0f, 1900.0f);
|
||||
EXPECT_EQ(0, filter.state);
|
||||
EXPECT_EQ(2000, filter.slewLimit);
|
||||
EXPECT_EQ(1900, filter.threshold);
|
||||
|
||||
slewFilterApply(&filter, 1800.0f);
|
||||
EXPECT_EQ(1800, filter.state);
|
||||
slewFilterApply(&filter, -1800.0f);
|
||||
EXPECT_EQ(-1800, filter.state);
|
||||
slewFilterApply(&filter, -200.0f);
|
||||
EXPECT_EQ(-200, filter.state);
|
||||
|
||||
slewFilterApply(&filter, 1900.0f);
|
||||
EXPECT_EQ(1900, filter.state);
|
||||
slewFilterApply(&filter, -2000.0f);
|
||||
EXPECT_EQ(1900, filter.state);
|
||||
slewFilterApply(&filter, -200.0f);
|
||||
EXPECT_EQ(1900, filter.state);
|
||||
slewFilterApply(&filter, 1800.0f);
|
||||
EXPECT_EQ(1800, filter.state);
|
||||
slewFilterApply(&filter, -200.0f);
|
||||
EXPECT_EQ(-200, filter.state);
|
||||
|
||||
slewFilterApply(&filter, -1900.0f);
|
||||
EXPECT_EQ(-1900, filter.state);
|
||||
slewFilterApply(&filter, 2000.0f);
|
||||
EXPECT_EQ(-1900, filter.state);
|
||||
slewFilterApply(&filter, 200.0f);
|
||||
EXPECT_EQ(-1900, filter.state);
|
||||
slewFilterApply(&filter, -1800.0f);
|
||||
EXPECT_EQ(-1800, filter.state);
|
||||
slewFilterApply(&filter, 200.0f);
|
||||
EXPECT_EQ(200, filter.state);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue