File content as of revision 1:537eb14c5332:

/**
 *  @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;
}