Matthew Thewsey
/
ELEC347_Coursework_NonVariable
Non-variable (TESTEDandWORKING)
Fork of ELEC347_Coursework by
Filter.cpp
- Committer:
- chills
- Date:
- 2017-11-28
- Revision:
- 8:192b376a6da7
- Parent:
- 7:6cb27cce4c50
- Child:
- 11:7efb6e4759cc
File content as of revision 8:192b376a6da7:
#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\t", _b0); printf("Num 2 = %f\t", _b1); printf("Num 3 = %f\t", _b2); printf("Den 1 = %f\t", _a0); printf("Den 2 = %f\t", _a1); printf("Den 3 = %f\n\r", _a2); } void FILTER::Update_Fo(int Fo_New){_Fo = Fo_New;} int FILTER::Get_Fo(){return _Fo;} void FILTER::Update_Q(int Q_New){_Q = Q_New;} int FILTER::Get_Q(){return _Q;} void FILTER::Update_Boost(double Boost_New){_Boost = Boost_New;} double FILTER::Get_Boost(){return _Boost;}