daiki hiraoka
/
EMG
筋電センサプログラム
筋電
FilterTest.cpp
- Committer:
- dhpt
- Date:
- 2018-01-04
- Revision:
- 13:94ce8bd0c0e1
- Parent:
- 3:5d91bfe4a79d
File content as of revision 13:94ce8bd0c0e1:
/** * @file FilterTest.cpp * @brief Calculate filters * HPF: 1st order / LPF: 2nd order / Notch: 2nd order * @date 2015.02.22 * @version 1.0.1 */ #include "FilterTest.h" /** Constructor */ FilterTest::FilterTest () { set_hpf_coef(INIT_HB, INIT_HA); set_lpf_coef(INIT_LB, INIT_LA1, INIT_LA2); set_brf_coef(INIT_NB, INIT_NA1, INIT_NA2); reset_hpf_buf(); reset_lpf_buf(); reset_brf_buf(); } /** Calculate filter * @param val Input value * @param hpf_on High pass filter enable * @param lpf_on Low pass filter enable * @param brf_on Notch filter enable * @return Output value */ double FilterTest::calc(double val, int hpf_on, int lpf_on, int brf_on) { double retVal = val; /* High pass filter */ if(hpf_on) { retVal = hpf(retVal); } else { reset_hpf_buf(); } /* Low pass filter */ if(lpf_on) { retVal = lpf(retVal); } else { reset_lpf_buf(); } /* Notch (Band reject) filter */ if(brf_on) { retVal = brf(retVal); } else { reset_brf_buf(); } return retVal; } /** Reset delay buffers for high pass filter */ void FilterTest::reset_hpf_buf() { _hw = 0.0; } /** Reset delay buffers for low pass filter */ void FilterTest::reset_lpf_buf() { _lw1 = 0.0; _lw2 = 0.0; } /** Reset delay buffers for notch filter */ void FilterTest::reset_brf_buf() { _nw1 = 0.0; _nw2 = 0.0; } /** Set coefficient for HPF * @param hb Numerator cofficient * @param ha Denominator cofficient * @retval true OK * @retval false NG */ bool FilterTest:: set_hpf_coef(double hb, double ha) { if(hb > 1.0) { return false; } else if((ha > 1.0)||(ha < -1.0)) { return false; } else { _hb = hb; _ha = ha; reset_hpf_buf(); return true; } } /** Set coefficient for LPF * @param lb Numerator cofficient * @param la1 Denominator cofficient 1 * @param la2 Denominator cofficient 2 * @retval true OK * @retval false NG */ bool FilterTest:: set_lpf_coef(double lb, double la1, double la2) { if(lb > 1.0) { return false; } else if((la1 > 2.0)||(la1 < -2.0)) { return false; } else if(la2 > 1.0) { return false; } else { _lb = lb; _la1 = la1; _la2 = la2; reset_lpf_buf(); return true; } } /** Set coefficient for BRF * @param nb Numerator cofficient * @param na1 Denominator cofficient 1 * @param na2 Denominator cofficient 2 * @retval true OK * @retval false NG */ bool FilterTest:: set_brf_coef(double nb, double na1, double na2) { if(nb > 1.0) { return false; } else if((na1 > 2.0)||(na1 < -2.0)) { return false; } else if(na2 > 1.0) { return false; } else { _nb = nb; _na1 = na1; _na2 = na2; reset_brf_buf(); return true; } } /** High pass filter (1st order) * @param x Input value * @return Output value * * hb v + * x ---I>---+---O-------+--- y * | hw|- | * | [z] | * | | ha | * +---O---<I--+ */ double FilterTest::hpf(double x) { double v, y; v = _hb * x; y = v - _hw; _hw = v + _ha * y; return y; } /** Low pass filter (2nd order) * @param x Input value * @return Output value * * lb v * x --I>--+-----O-------+--- y * | lw1| | * | [z] | * | 2 | -la1 | * +-I>--O---<I--+ * | lw2| | * | [z] | * | | -la2 | * +-----O---<I--+ */ double FilterTest::lpf(double x) { double v, y; v = _lb * x; y = v + _lw1; _lw1 = 2 * v - _la1 * y + _lw2; _lw2 = v - _la2 * y; return y; } /** Notch filter (Band reject filter) (2nd order) * @param x Input value * @return Output value * * nb v * x -+-I>--+----O-------+--- y * | | nw1| | * | | [z] | * +| na1 | | | * O-I>-------O | * -| | nw2| | * | | [z] | * | | | -na2 | * | +----O---<I--+ * | | * +------------------+ */ double FilterTest::brf(double x) { double v, y; v = _nb * x; y = v + _nw1; _nw1 = _na1 * ( x - y ) + _nw2; _nw2 = v - _na2 * y; return y; }