Diff: FilterTest.cpp

Revision:

3:5d91bfe4a79d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FilterTest.cpp	Sun Feb 05 07:32:30 2017 +0000
@@ -0,0 +1,217 @@
+/**
+ *  @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;
+}