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