Matthew Thewsey / Mbed 2 deprecated ELEC347_Coursework_NonVariable

Non-variable (TESTEDandWORKING)

Dependencies:   mbed-rtos mbed

Fork of ELEC347_Coursework by CMST

Filter.cpp@8:192b376a6da7, 2017-11-28 (annotated)

Committer:
chills
Date:
Tue Nov 28 15:19:08 2017 +0000
Revision:
8:192b376a6da7
Parent:
7:6cb27cce4c50
Child:
11:7efb6e4759cc
Print Coefficients is Working Live

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chills 6:a2737b51424c 1 #include "mbed.h"
mwthewsey 0:d39a06ca6bf1 2 #include "Filter.hpp"
mwthewsey 2:f6e49378dd8a 3 #include <math.h>
mwthewsey 0:d39a06ca6bf1 4
chills 6:a2737b51424c 5
mwthewsey 2:f6e49378dd8a 6 #define PI 3.14159
mwthewsey 2:f6e49378dd8a 7
mwthewsey 2:f6e49378dd8a 8 FILTER::FILTER(int Fs, int Fo, double Boost, int Q) //Constuctor
mwthewsey 0:d39a06ca6bf1 9 {
mwthewsey 2:f6e49378dd8a 10 this-> _Fs = Fs;
mwthewsey 0:d39a06ca6bf1 11 this-> _Fo = Fo;
mwthewsey 0:d39a06ca6bf1 12 this-> _Boost = Boost;
mwthewsey 0:d39a06ca6bf1 13 this-> _Q = Q;
mwthewsey 0:d39a06ca6bf1 14
mwthewsey 0:d39a06ca6bf1 15
mwthewsey 0:d39a06ca6bf1 16 _centreTap = 1;
mwthewsey 0:d39a06ca6bf1 17 _G = 0;
mwthewsey 0:d39a06ca6bf1 18 _k = 0;
mwthewsey 0:d39a06ca6bf1 19 _Wo = 0;
mwthewsey 2:f6e49378dd8a 20 _Wo_d = 0;
mwthewsey 2:f6e49378dd8a 21 _Wo_d2 = 0;
mwthewsey 2:f6e49378dd8a 22 _Com_Den = 0;
mwthewsey 2:f6e49378dd8a 23 _NC0 = 0;
mwthewsey 2:f6e49378dd8a 24 _NC1 = 0;
mwthewsey 2:f6e49378dd8a 25 _NC2 = 0;
mwthewsey 2:f6e49378dd8a 26 _DC1 = 0;
mwthewsey 2:f6e49378dd8a 27 _DC2 = 0;
mwthewsey 0:d39a06ca6bf1 28
mwthewsey 2:f6e49378dd8a 29 _xn = 0; //This value is the input value
mwthewsey 0:d39a06ca6bf1 30 _xnm1 = 0;
mwthewsey 0:d39a06ca6bf1 31 _xnm2 = 0;
mwthewsey 0:d39a06ca6bf1 32
mwthewsey 0:d39a06ca6bf1 33 _yn = 0;
mwthewsey 0:d39a06ca6bf1 34 _ynm1 = 0;
mwthewsey 0:d39a06ca6bf1 35 _ynm2 = 0;
mwthewsey 0:d39a06ca6bf1 36 }
mwthewsey 0:d39a06ca6bf1 37
mwthewsey 0:d39a06ca6bf1 38 FILTER::~FILTER() {}
mwthewsey 0:d39a06ca6bf1 39
mwthewsey 2:f6e49378dd8a 40 void FILTER::setvalue(float RAW_input)
mwthewsey 0:d39a06ca6bf1 41 {
mwthewsey 0:d39a06ca6bf1 42
mwthewsey 0:d39a06ca6bf1 43 _xn = RAW_input;
mwthewsey 2:f6e49378dd8a 44 _centreTap = _xn*_b0 + _xnm1*_b1 + _xnm2*_b2; //IIR Filter
mwthewsey 2:f6e49378dd8a 45 _yn = _centreTap*_a0 - _a1*_ynm1 - _a2*_ynm2; //Result in yn
mwthewsey 0:d39a06ca6bf1 46
mwthewsey 2:f6e49378dd8a 47 _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
mwthewsey 0:d39a06ca6bf1 48
mwthewsey 0:d39a06ca6bf1 49 //THESE NEED TO BE LOADED IN THIS ORDER OTHERWISE ALL xnm VALUES WILL BECOME THE SAME AS xn
mwthewsey 0:d39a06ca6bf1 50 _xnm2 = _xnm1;
mwthewsey 0:d39a06ca6bf1 51 _xnm1 = _xn;
mwthewsey 0:d39a06ca6bf1 52
mwthewsey 0:d39a06ca6bf1 53 //THESE NEED TO BE LOADED IN THIS ORDER OTHERWISE ALL ynm VALUES WILL BECOME THE SAME AS yn
mwthewsey 0:d39a06ca6bf1 54 _ynm2 = _ynm1;
mwthewsey 0:d39a06ca6bf1 55 _ynm1 = _yn;
mwthewsey 0:d39a06ca6bf1 56
mwthewsey 0:d39a06ca6bf1 57 }
mwthewsey 0:d39a06ca6bf1 58
mwthewsey 0:d39a06ca6bf1 59 float FILTER::getvalue(void)
mwthewsey 0:d39a06ca6bf1 60 {
mwthewsey 2:f6e49378dd8a 61 return _FilterOutput;
mwthewsey 0:d39a06ca6bf1 62 }
mwthewsey 0:d39a06ca6bf1 63
mwthewsey 2:f6e49378dd8a 64 void FILTER::Define_Filter()//Define the filter coefficients
mwthewsey 2:f6e49378dd8a 65 {
mwthewsey 2:f6e49378dd8a 66 _G = pow(10,_Boost/20.0);
mwthewsey 2:f6e49378dd8a 67 _k = 3 * ((_G - 1) / (_G+1));
mwthewsey 2:f6e49378dd8a 68 _Wo = 2 * PI * _Fo;
mwthewsey 2:f6e49378dd8a 69 _Wo_d = tan(_Wo / (2*_Fs));
mwthewsey 2:f6e49378dd8a 70 _Wo_d2 = (_Wo_d * _Wo_d); //Omega-Dash-Naught-Squared
mwthewsey 2:f6e49378dd8a 71 _Com_Den = 1 + ((3-_k)*_Wo_d/_Q) + _Wo_d2;
mwthewsey 2:f6e49378dd8a 72 _NC0 = 1 + ((3+_k)*_Wo_d/_Q) + _Wo_d2;
mwthewsey 2:f6e49378dd8a 73 _NC1 = -2 + (2 * _Wo_d2);
mwthewsey 2:f6e49378dd8a 74 _NC2 = 1 - ((3+_k)*_Wo_d/_Q) + _Wo_d2;
mwthewsey 2:f6e49378dd8a 75 _DC1 = _NC1;
mwthewsey 2:f6e49378dd8a 76 _DC2 = 1 - ((3-_k)*_Wo_d/_Q) + _Wo_d2;
mwthewsey 2:f6e49378dd8a 77
mwthewsey 2:f6e49378dd8a 78 //Calculate Coefficients
mwthewsey 2:f6e49378dd8a 79
mwthewsey 2:f6e49378dd8a 80 _b0 = _NC0 / _Com_Den;
mwthewsey 2:f6e49378dd8a 81 _b1 = _NC1 / _Com_Den;
mwthewsey 2:f6e49378dd8a 82 _b2 = _NC2 / _Com_Den;
mwthewsey 2:f6e49378dd8a 83 _a0 = 1.0000;
mwthewsey 2:f6e49378dd8a 84 _a1 = _DC1 / _Com_Den;
mwthewsey 2:f6e49378dd8a 85 _a2 = _DC2 / _Com_Den;
mwthewsey 2:f6e49378dd8a 86 }
mwthewsey 2:f6e49378dd8a 87
chills 6:a2737b51424c 88 void FILTER::Print_Filter() //Print Filter Coefficients
chills 6:a2737b51424c 89 {
chills 8:192b376a6da7 90 printf("Num 1 = %f\t", _b0);
chills 8:192b376a6da7 91 printf("Num 2 = %f\t", _b1);
chills 8:192b376a6da7 92 printf("Num 3 = %f\t", _b2);
chills 8:192b376a6da7 93 printf("Den 1 = %f\t", _a0);
chills 8:192b376a6da7 94 printf("Den 2 = %f\t", _a1);
chills 6:a2737b51424c 95 printf("Den 3 = %f\n\r", _a2);
chills 7:6cb27cce4c50 96 }
chills 7:6cb27cce4c50 97
chills 7:6cb27cce4c50 98 void FILTER::Update_Fo(int Fo_New){_Fo = Fo_New;}
chills 7:6cb27cce4c50 99 int FILTER::Get_Fo(){return _Fo;}
chills 7:6cb27cce4c50 100
chills 7:6cb27cce4c50 101 void FILTER::Update_Q(int Q_New){_Q = Q_New;}
chills 7:6cb27cce4c50 102 int FILTER::Get_Q(){return _Q;}
chills 7:6cb27cce4c50 103
chills 7:6cb27cce4c50 104 void FILTER::Update_Boost(double Boost_New){_Boost = Boost_New;}
chills 7:6cb27cce4c50 105 double FILTER::Get_Boost(){return _Boost;}