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

Added slew filter

This commit is contained in:
Martin Budden 2017-08-28 08:12:02 +01:00
parent ee48272462
commit 8c20811792
3 changed files with 90 additions and 1 deletions
Show Stats 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;
}
// 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);

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

@ -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);

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;
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);
}