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

Calculate PT1 filter constant in initialiastion

This commit is contained in:
Martin Budden 2016-12-17 08:17:58 +00:00
parent 9e2fd0e51c
commit 0dbe9629f4
2 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -46,11 +46,12 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
{
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->dT = dT;
filter->k = filter->dT / (filter->RC + filter->dT);
}
float pt1FilterApply(pt1Filter_t *filter, float input)
{
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
filter->state = filter->state + filter->k * (input - filter->state);
return filter->state;
}
@ -60,9 +61,10 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->dT = dT;
filter->k = filter->dT / (filter->RC + filter->dT);
}
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
filter->state = filter->state + filter->k * (input - filter->state);
return filter->state;
}

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

@ -25,6 +25,7 @@
typedef struct pt1Filter_s {
float state;
float k;
float RC;
float dT;
} pt1Filter_t;