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

Merge pull request #3983 from martinbudden/bf_slew_filter

Browse source 
Added slew filter
This commit is contained in:
Martin Budden 2017-08-28 08:52:36 +01:00 • committed by GitHub
commit 5c6f110d0a
3 changed files with 90 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -67,6 +67,32 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
return filter->state; 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 filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) {
float octaves = log2f((float) centerFreq / (float) cutoff) * 2; float octaves = log2f((float) centerFreq / (float) cutoff) * 2;
return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);

12
src/main/common/filter.h
View file

@ -32,6 +32,12 @@ typedef struct pt1Filter_s {
float dT; float dT;
} pt1Filter_t; } 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 */ /* this holds the data required to update samples thru a filter */
typedef struct biquadFilter_s { typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2; float b0, b1, b2, a1, a2;
@ -49,7 +55,8 @@ typedef struct firFilterDenoise_s{
typedef enum { typedef enum {
FILTER_PT1 = 0, FILTER_PT1 = 0,
FILTER_BIQUAD, FILTER_BIQUAD,
FILTER_FIR FILTER_FIR,
FILTER_SLEW
} filterType_e; } filterType_e;
typedef enum { 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 pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); 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 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 firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
void firFilterUpdate(firFilter_t *filter, float input); void firFilterUpdate(firFilter_t *filter, float input);

53
src/test/unit/common_filter_unittest.cc
View file

@ -145,3 +145,56 @@ TEST(FilterUnittest, TestFirFilterApply)
expected = 7.0f * 26.0f + 6.0 * 27.0 + 5.0 * 28.0 + 4.0f * 29.0f; expected = 7.0f * 26.0f + 6.0 * 27.0 + 5.0 * 28.0 + 4.0f * 29.0f;
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); 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);
}