From add116ff544d470899d6e4a410267656496ef4bc Mon Sep 17 00:00:00 2001 From: Edgar Bonet Date: Fri, 15 Apr 2022 15:36:33 +0200 Subject: [PATCH] Make the filter state instance data In order for multiple instances of EMGFilters to be able to work independently, they must each store its own state. Make this state private class members instead of global variables. --- EMGFilters.cpp | 191 +++++++++++++++++++++---------------------------- EMGFilters.h | 36 ++++++++++ 2 files changed, 117 insertions(+), 110 deletions(-) diff --git a/EMGFilters.cpp b/EMGFilters.cpp index 366eeea..b981de7 100644 --- a/EMGFilters.cpp +++ b/EMGFilters.cpp @@ -60,131 +60,102 @@ static float ahf_denominator_coef_60Hz[2][6] = { {1.0000, -1.8407, 0.9894, 1.0000, -1.8584, 0.9891}}; static float ahf_output_gain_coef_60Hz[2] = {1.3430, 1.4206}; -enum FILTER_TYPE { - FILTER_TYPE_LOWPASS = 0, - FILTER_TYPE_HIGHPASS, -}; - -class FILTER_2nd { - private: - // second-order filter - float states[2]; - float num[3]; - float den[3]; - - public: - void init(FILTER_TYPE ftype, int sampleFreq) { - states[0] = 0; - states[1] = 0; - if (ftype == FILTER_TYPE_LOWPASS) { - // 2th order butterworth lowpass filter - // cutoff frequency 150Hz - if (sampleFreq == SAMPLE_FREQ_500HZ) { - for (int i = 0; i < 3; i++) { - num[i] = lpf_numerator_coef[0][i]; - den[i] = lpf_denominator_coef[0][i]; - } - } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { - for (int i = 0; i < 3; i++) { - num[i] = lpf_numerator_coef[1][i]; - den[i] = lpf_denominator_coef[1][i]; - } +void FILTER_2nd::init(FILTER_TYPE ftype, int sampleFreq) { + states[0] = 0; + states[1] = 0; + if (ftype == FILTER_TYPE_LOWPASS) { + // 2th order butterworth lowpass filter + // cutoff frequency 150Hz + if (sampleFreq == SAMPLE_FREQ_500HZ) { + for (int i = 0; i < 3; i++) { + num[i] = lpf_numerator_coef[0][i]; + den[i] = lpf_denominator_coef[0][i]; } - } else if (ftype == FILTER_TYPE_HIGHPASS) { - // 2th order butterworth - // cutoff frequency 20Hz - if (sampleFreq == SAMPLE_FREQ_500HZ) { - for (int i = 0; i < 3; i++) { - num[i] = hpf_numerator_coef[0][i]; - den[i] = hpf_denominator_coef[0][i]; - } - } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { - for (int i = 0; i < 3; i++) { - num[i] = hpf_numerator_coef[1][i]; - den[i] = hpf_denominator_coef[1][i]; - } + } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { + for (int i = 0; i < 3; i++) { + num[i] = lpf_numerator_coef[1][i]; + den[i] = lpf_denominator_coef[1][i]; + } + } + } else if (ftype == FILTER_TYPE_HIGHPASS) { + // 2th order butterworth + // cutoff frequency 20Hz + if (sampleFreq == SAMPLE_FREQ_500HZ) { + for (int i = 0; i < 3; i++) { + num[i] = hpf_numerator_coef[0][i]; + den[i] = hpf_denominator_coef[0][i]; + } + } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { + for (int i = 0; i < 3; i++) { + num[i] = hpf_numerator_coef[1][i]; + den[i] = hpf_denominator_coef[1][i]; } } } +} + +float FILTER_2nd::update(float input) { + float tmp = (input - den[1] * states[0] - den[2] * states[1]) / den[0]; + float output = num[0] * tmp + num[1] * states[0] + num[2] * states[1]; + // save last states + states[1] = states[0]; + states[0] = tmp; + return output; +} - float update(float input) { - float tmp = (input - den[1] * states[0] - den[2] * states[1]) / den[0]; - float output = num[0] * tmp + num[1] * states[0] + num[2] * states[1]; - // save last states - states[1] = states[0]; - states[0] = tmp; - return output; +void FILTER_4th::init(int sampleFreq, int humFreq) { + gain = 0; + for (int i = 0; i < 4; i++) { + states[i] = 0; } -}; - -class FILTER_4th { - private: - // fourth-order filter - // cascade two 2nd-order filters - float states[4]; - float num[6]; - float den[6]; - float gain; - - public: - void init(int sampleFreq, int humFreq) { - gain = 0; - for (int i = 0; i < 4; i++) { - states[i] = 0; + if (humFreq == NOTCH_FREQ_50HZ) { + if (sampleFreq == SAMPLE_FREQ_500HZ) { + for (int i = 0; i < 6; i++) { + num[i] = ahf_numerator_coef_50Hz[0][i]; + den[i] = ahf_denominator_coef_50Hz[0][i]; + } + gain = ahf_output_gain_coef_50Hz[0]; + } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { + for (int i = 0; i < 6; i++) { + num[i] = ahf_numerator_coef_50Hz[1][i]; + den[i] = ahf_denominator_coef_50Hz[1][i]; + } + gain = ahf_output_gain_coef_50Hz[1]; } - if (humFreq == NOTCH_FREQ_50HZ) { - if (sampleFreq == SAMPLE_FREQ_500HZ) { - for (int i = 0; i < 6; i++) { - num[i] = ahf_numerator_coef_50Hz[0][i]; - den[i] = ahf_denominator_coef_50Hz[0][i]; - } - gain = ahf_output_gain_coef_50Hz[0]; - } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { - for (int i = 0; i < 6; i++) { - num[i] = ahf_numerator_coef_50Hz[1][i]; - den[i] = ahf_denominator_coef_50Hz[1][i]; - } - gain = ahf_output_gain_coef_50Hz[1]; + } else if (humFreq == NOTCH_FREQ_60HZ) { + if (sampleFreq == SAMPLE_FREQ_500HZ) { + for (int i = 0; i < 6; i++) { + num[i] = ahf_numerator_coef_60Hz[0][i]; + den[i] = ahf_denominator_coef_60Hz[0][i]; } - } else if (humFreq == NOTCH_FREQ_60HZ) { - if (sampleFreq == SAMPLE_FREQ_500HZ) { - for (int i = 0; i < 6; i++) { - num[i] = ahf_numerator_coef_60Hz[0][i]; - den[i] = ahf_denominator_coef_60Hz[0][i]; - } - gain = ahf_output_gain_coef_60Hz[0]; - } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { - for (int i = 0; i < 6; i++) { - num[i] = ahf_numerator_coef_60Hz[1][i]; - den[i] = ahf_denominator_coef_60Hz[1][i]; - } - gain = ahf_output_gain_coef_60Hz[1]; + gain = ahf_output_gain_coef_60Hz[0]; + } else if (sampleFreq == SAMPLE_FREQ_1000HZ) { + for (int i = 0; i < 6; i++) { + num[i] = ahf_numerator_coef_60Hz[1][i]; + den[i] = ahf_denominator_coef_60Hz[1][i]; } + gain = ahf_output_gain_coef_60Hz[1]; } } +} - float update(float input) { - float output; - float stageIn; - float stageOut; +float FILTER_4th::update(float input) { + float output; + float stageIn; + float stageOut; - stageOut = num[0] * input + states[0]; - states[0] = (num[1] * input + states[1]) - den[1] * stageOut; - states[1] = num[2] * input - den[2] * stageOut; - stageIn = stageOut; - stageOut = num[3] * stageOut + states[2]; - states[2] = (num[4] * stageIn + states[3]) - den[4] * stageOut; - states[3] = num[5] * stageIn - den[5] * stageOut; + stageOut = num[0] * input + states[0]; + states[0] = (num[1] * input + states[1]) - den[1] * stageOut; + states[1] = num[2] * input - den[2] * stageOut; + stageIn = stageOut; + stageOut = num[3] * stageOut + states[2]; + states[2] = (num[4] * stageIn + states[3]) - den[4] * stageOut; + states[3] = num[5] * stageIn - den[5] * stageOut; - output = gain * stageOut; + output = gain * stageOut; - return output; - } -}; - -FILTER_2nd LPF; -FILTER_2nd HPF; -FILTER_4th AHF; + return output; +} void EMGFilters::init(SAMPLE_FREQUENCY sampleFreq, NOTCH_FREQUENCY notchFreq, diff --git a/EMGFilters.h b/EMGFilters.h index e66f527..28b09d3 100644 --- a/EMGFilters.h +++ b/EMGFilters.h @@ -36,6 +36,39 @@ enum NOTCH_FREQUENCY { NOTCH_FREQ_50HZ = 50, NOTCH_FREQ_60HZ = 60 }; enum SAMPLE_FREQUENCY { SAMPLE_FREQ_500HZ = 500, SAMPLE_FREQ_1000HZ = 1000 }; +enum FILTER_TYPE { + FILTER_TYPE_LOWPASS = 0, + FILTER_TYPE_HIGHPASS, +}; + +class FILTER_2nd { + private: + // second-order filter + float states[2]; + float num[3]; + float den[3]; + + public: + void init(FILTER_TYPE ftype, int sampleFreq); + + float update(float input); +}; + +class FILTER_4th { + private: + // fourth-order filter + // cascade two 2nd-order filters + float states[4]; + float num[6]; + float den[6]; + float gain; + + public: + void init(int sampleFreq, int humFreq); + + float update(float input); +}; + // \brief EMGFilter provides an anti-hum notch filter to filter out 50HZ or // 60HZ power line noise, a lowpass filter to filter out signals above // 150HZ, and a highpass filter to filter out noise below 20HZ; @@ -71,6 +104,9 @@ class EMGFilters { bool m_notchFilterEnabled; bool m_lowpassFilterEnabled; bool m_highpassFilterEnabled; + FILTER_2nd LPF; + FILTER_2nd HPF; + FILTER_4th AHF; }; #endif