FilterTest.cpp

00001 /**
00002  *  @file       FilterTest.cpp
00003  *  @brief      Calculate filters
00004  *              HPF: 1st order / LPF: 2nd order / Notch: 2nd order
00005  *  @date       2015.02.22
00006  *  @version    1.0.1
00007  */
00008 #include "FilterTest.h"
00009 
00010 /** Constructor
00011 */
00012 FilterTest::FilterTest ()
00013 {
00014     set_hpf_coef(INIT_HB, INIT_HA);
00015     set_lpf_coef(INIT_LB, INIT_LA1, INIT_LA2);
00016     set_brf_coef(INIT_NB, INIT_NA1, INIT_NA2);
00017     reset_hpf_buf();
00018     reset_lpf_buf();
00019     reset_brf_buf();
00020 }
00021 
00022 /** Calculate filter
00023  *  @param      val         Input value
00024  *  @param      hpf_on      High pass filter enable
00025  *  @param      lpf_on      Low pass filter enable
00026  *  @param      brf_on      Notch filter enable
00027  *  @return                 Output value
00028  */
00029 double FilterTest::calc(double val, int hpf_on, int lpf_on, int brf_on)
00030 {
00031     double retVal = val;
00032     /* High pass filter */
00033     if(hpf_on) {
00034         retVal = hpf(retVal);
00035     } else {
00036         reset_hpf_buf();
00037     }
00038     /* Low pass filter */
00039     if(lpf_on) {
00040         retVal = lpf(retVal);
00041     } else {
00042         reset_lpf_buf();
00043     }
00044     /* Notch (Band reject) filter */
00045     if(brf_on) {
00046         retVal = brf(retVal);
00047     } else {
00048         reset_brf_buf();
00049     }
00050     return retVal;
00051 }
00052 
00053 /** Reset delay buffers for high pass filter
00054 */
00055 void FilterTest::reset_hpf_buf()
00056 {
00057     _hw = 0.0;
00058 }
00059 
00060 /** Reset delay buffers for low pass filter
00061 */
00062 void FilterTest::reset_lpf_buf()
00063 {
00064     _lw1 = 0.0;
00065     _lw2 = 0.0;
00066 }
00067 
00068 /** Reset delay buffers for notch filter
00069 */
00070 void FilterTest::reset_brf_buf()
00071 {
00072     _nw1 = 0.0;
00073     _nw2 = 0.0;
00074 }
00075 
00076 /** Set coefficient for HPF
00077  *  @param      hb          Numerator cofficient
00078  *  @param      ha          Denominator cofficient
00079  *  @retval     true        OK
00080  *  @retval     false       NG
00081  */
00082 bool FilterTest:: set_hpf_coef(double hb, double ha)
00083 {
00084     if(hb > 1.0) {
00085         return false;
00086     } else if((ha > 1.0)||(ha < -1.0)) {
00087         return false;
00088     } else {
00089         _hb = hb;
00090         _ha = ha;
00091         reset_hpf_buf();
00092         return true;
00093     }
00094 }
00095 
00096 /** Set coefficient for LPF
00097  *  @param      lb          Numerator cofficient
00098  *  @param      la1         Denominator cofficient 1
00099  *  @param      la2         Denominator cofficient 2
00100  *  @retval     true        OK
00101  *  @retval     false       NG
00102  */
00103 bool FilterTest:: set_lpf_coef(double lb, double la1, double la2)
00104 {
00105     if(lb > 1.0) {
00106         return false;
00107     } else if((la1 > 2.0)||(la1 < -2.0)) {
00108         return false;
00109     } else if(la2 > 1.0) {
00110         return false;
00111     } else {
00112         _lb = lb;
00113         _la1 = la1;
00114         _la2 = la2;
00115         reset_lpf_buf();
00116         return true;
00117     }
00118 }
00119 
00120 /** Set coefficient for BRF
00121  *  @param      nb          Numerator cofficient
00122  *  @param      na1         Denominator cofficient 1
00123  *  @param      na2         Denominator cofficient 2
00124  *  @retval     true        OK
00125  *  @retval     false       NG
00126  */
00127 bool FilterTest:: set_brf_coef(double nb, double na1, double na2)
00128 {
00129     if(nb > 1.0) {
00130         return false;
00131     } else if((na1 > 2.0)||(na1 < -2.0)) {
00132         return false;
00133     } else if(na2 > 1.0) {
00134         return false;
00135     } else {
00136         _nb = nb;
00137         _na1 = na1;
00138         _na2 = na2;
00139         reset_brf_buf();
00140         return true;
00141     }
00142 }
00143 
00144 /** High pass filter (1st order)
00145  *  @param      x      Input value
00146  *  @return            Output value
00147  *
00148  *       hb   v  +
00149  *  x ---I>---+---O-------+--- y
00150  *            | hw|-      |
00151  *            |  [z]      |
00152  *            |   |   ha  |
00153  *            +---O---<I--+
00154  */
00155 double FilterTest::hpf(double x)
00156 {
00157     double v, y;
00158 
00159     v = _hb * x;
00160     y = v - _hw;
00161     _hw = v + _ha * y;
00162     return y;
00163 }
00164 
00165 /** Low pass filter (2nd order)
00166  *  @param      x      Input value
00167  *  @return            Output value
00168  *
00169  *      lb  v
00170  *  x --I>--+-----O-------+--- y
00171  *          |  lw1|       |
00172  *          |    [z]      |
00173  *          |  2  |  -la1 |
00174  *          +-I>--O---<I--+
00175  *          |  lw2|       |
00176  *          |    [z]      |
00177  *          |     |  -la2 |
00178  *          +-----O---<I--+
00179  */
00180 double FilterTest::lpf(double x)
00181 {
00182     double v, y;
00183 
00184     v = _lb * x;
00185     y = v + _lw1;
00186     _lw1 = 2 * v - _la1 * y + _lw2;
00187     _lw2 = v - _la2 * y;
00188     return y;
00189 }
00190 
00191 /** Notch filter (Band reject filter) (2nd order)
00192  *  @param      x      Input value
00193  *  @return            Output value
00194  *
00195  *       nb  v
00196  *  x -+-I>--+----O-------+--- y
00197  *     |     | nw1|       |
00198  *     |     |   [z]      |
00199  *    +| na1 |    |       |
00200  *     O-I>-------O       |
00201  *    -|     | nw2|       |
00202  *     |     |   [z]      |
00203  *     |     |    |  -na2 |
00204  *     |     +----O---<I--+
00205  *     |                  |
00206  *     +------------------+
00207  */
00208 double FilterTest::brf(double x)
00209 {
00210     double v, y;
00211 
00212     v = _nb * x;
00213     y = v + _nw1;
00214     _nw1 = _na1 * ( x - y ) + _nw2;
00215     _nw2 = v - _na2 * y;
00216     return y;
00217 }