Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Simplify notch Q computation

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-03-20 20:01:39 +01:00
parent 0cb3ce4ede
commit c00c24924d
2 changed files with 3 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -118,10 +118,9 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
return filter->state; return filter->state;
} }
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
{ {
const float octaves = log2f((float)centerFreq / (float)cutoff) * 2; return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
} }
void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz) void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)

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

@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
float biquadFilterApply(biquadFilter_t *filter, float sample); float biquadFilterApply(biquadFilter_t *filter, float sample);
float biquadFilterReset(biquadFilter_t *filter, float value); float biquadFilterReset(biquadFilter_t *filter, float value);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
void biquadFilterUpdate(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 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);