File content as of revision 0:17706252d1ed:

#include "Filter.h"
#include "define.h"

Filter::Filter()
{
}

void Filter::setLowPassPara(double T)
{
    T_LPF = T;
}

double Filter::LowPassFilter(double input)
{
    static double preOutput = 0.0;

    double Output = (INT_TIME * input + T_LPF * preOutput)/(T_LPF + INT_TIME);

    preOutput = Output;

    return Output;
}

void Filter::setNotchPara(double Omega)
{
    // 落としたい角周波数[rad/s]をOm_nに入れる
    Om_n = Omega;
    sq_Om = pow(Om_n, 2.0); // Om_nの2乗
    sq_dt = pow(INT_TIME, 2.0); // dtの2乗

}

double Filter::NotchFilter(double input)
{
    static double preOutput[2] = {0.0, 0.0};
    static double preInput[2] = {0.0, 0.0};
    
    double Output = (2*(1 + Om_n * INT_TIME) * preOutput[0]-preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * preInput[0] + preInput[1]) / (1 + 2 * Om_n * INT_TIME + sq_Om * sq_dt);
    
    preInput[1] = preInput[0];
    preInput[0] = input;
    
    preOutput[1] = preOutput[0];
    preOutput[0] = Output;
    
    return Output;
}