File content as of revision 6:a2737b51424c:

#include "mbed.h"
#include "Filter.hpp"
#include <math.h>


#define PI 3.14159

FILTER::FILTER(int Fs, int Fo, double Boost, int Q) //Constuctor
{
    this-> _Fs = Fs;
    this-> _Fo = Fo;
    this-> _Boost = Boost;
    this-> _Q = Q;


    _centreTap = 1;
    _G = 0;
    _k = 0;
    _Wo = 0;
    _Wo_d = 0;
    _Wo_d2 = 0;
    _Com_Den = 0;
    _NC0 = 0;
    _NC1 = 0;
    _NC2 = 0;
    _DC1 = 0;
    _DC2 = 0;

    _xn = 0;     //This value is the input value
    _xnm1 = 0;
    _xnm2 = 0;

    _yn = 0;
    _ynm1 = 0;
    _ynm2 = 0;
}

FILTER::~FILTER() {}

void FILTER::setvalue(float RAW_input)
{
            
    _xn = RAW_input;
    _centreTap = _xn*_b0 + _xnm1*_b1 + _xnm2*_b2; //IIR Filter
    _yn = _centreTap*_a0 - _a1*_ynm1 - _a2*_ynm2; //Result in yn

    _FilterOutput=_yn+0.5f;       //Output resultant to DAC. Again MBED uses 0.0 to 1.0 float!!!!!! and Offset to give 0 to 3V3 range

    //THESE NEED TO BE LOADED IN THIS ORDER OTHERWISE ALL xnm VALUES WILL BECOME THE SAME AS xn
    _xnm2 = _xnm1;
    _xnm1 = _xn;

    //THESE NEED TO BE LOADED IN THIS ORDER OTHERWISE ALL ynm VALUES WILL BECOME THE SAME AS yn
    _ynm2 = _ynm1;
    _ynm1 = _yn;

}

float FILTER::getvalue(void)
{
    return _FilterOutput;  
}

void FILTER::Define_Filter()//Define the filter coefficients
{
    _G = pow(10,_Boost/20.0);
    _k = 3 * ((_G - 1) / (_G+1));
    _Wo = 2 * PI * _Fo;
    _Wo_d = tan(_Wo / (2*_Fs));
    _Wo_d2 = (_Wo_d * _Wo_d);                   //Omega-Dash-Naught-Squared
    _Com_Den = 1 + ((3-_k)*_Wo_d/_Q) + _Wo_d2;
    _NC0 = 1 + ((3+_k)*_Wo_d/_Q) + _Wo_d2;
    _NC1 = -2 + (2 * _Wo_d2);
    _NC2 = 1 - ((3+_k)*_Wo_d/_Q) + _Wo_d2;
    _DC1 = _NC1;
    _DC2 = 1 - ((3-_k)*_Wo_d/_Q) + _Wo_d2;
    
    //Calculate Coefficients
    
    _b0 = _NC0 / _Com_Den;
    _b1 = _NC1 / _Com_Den;
    _b2 = _NC2 / _Com_Den;
    _a0 = 1.0000;
    _a1 = _DC1 / _Com_Den;
    _a2 = _DC2 / _Com_Den;
}

void FILTER::Print_Filter()                //Print Filter Coefficients
{
    printf("Num 1 = %f\n\r", _b0);
    printf("Num 2 = %f\n\r", _b1);
    printf("Num 3 = %f\n\r", _b2);
    printf("Den 1 = %f\n\r", _a0);
    printf("Den 2 = %f\n\r", _a1);
    printf("Den 3 = %f\n\r", _a2);
}