V001. 2017_11_30 10:21 Working code from Tuesday's lab session.
Fork of 2017_11_28_ELEC347_Coursework by
DSP Coursework ELEC347 2017-2018 Group members: Matthew Thewsey, Thomas Morris, Samuel Waggett, Christopher Hills .
Filter.cpp@7:6cb27cce4c50, 2017-11-28 (annotated)
- Committer:
- chills
- Date:
- Tue Nov 28 14:37:13 2017 +0000
- Revision:
- 7:6cb27cce4c50
- Parent:
- 6:a2737b51424c
- Child:
- 8:192b376a6da7
Finished Writing Active Updating Code; Needs Testing; Did not print to board upon initial testing;
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:a2737b51424c | 90 | printf("Num 1 = %f\n\r", _b0); |
chills | 6:a2737b51424c | 91 | printf("Num 2 = %f\n\r", _b1); |
chills | 6:a2737b51424c | 92 | printf("Num 3 = %f\n\r", _b2); |
chills | 6:a2737b51424c | 93 | printf("Den 1 = %f\n\r", _a0); |
chills | 6:a2737b51424c | 94 | printf("Den 2 = %f\n\r", _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;} |