RT2_Cuboid_demo / Mbed 2 deprecated nucf446-test_GPA

Test of pmic GPA with filter

Dependencies:   mbed

Fork of nucf446-cuboid-balance1_strong by RT2_Cuboid_demo

IIR_filter.cpp@5:d6c7ccbbce78, 2018-03-09 (annotated)

Committer:
pmic
Date:
Fri Mar 09 12:53:45 2018 +0000
Revision:
5:d6c7ccbbce78
Parent:
0:15be70d21d7c
Child:
10:600d7cf652e7
basic extentions and corrections which where used for the psuechstag

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rtlabor 0:15be70d21d7c 1 #include "IIR_filter.h"
rtlabor 0:15be70d21d7c 2 #include "mbed.h"
rtlabor 0:15be70d21d7c 3 using namespace std;
pmic 5:d6c7ccbbce78 4
pmic 5:d6c7ccbbce78 5 /*
pmic 5:d6c7ccbbce78 6 IIR filter implemention for the following filter types:
pmic 5:d6c7ccbbce78 7 init for: first order differentiatior: G(s) = s/(T*s + 1)
pmic 5:d6c7ccbbce78 8 first order lowpass with gain G(s) = K/(T*s + 1)
pmic 5:d6c7ccbbce78 9 second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0)
pmic 5:d6c7ccbbce78 10 the billinear transformation is used for s -> z
pmic 5:d6c7ccbbce78 11 reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator
pmic 5:d6c7ccbbce78 12 */
pmic 5:d6c7ccbbce78 13
pmic 5:d6c7ccbbce78 14 // G(s) = s/(T*s + 1)
pmic 5:d6c7ccbbce78 15 IIR_filter::IIR_filter(float T,float Ts){
pmic 5:d6c7ccbbce78 16
pmic 5:d6c7ccbbce78 17 // filter orders
pmic 5:d6c7ccbbce78 18 nb = 1; // Filter Order
pmic 5:d6c7ccbbce78 19 na = 1; // Filter Order
rtlabor 0:15be70d21d7c 20
pmic 5:d6c7ccbbce78 21 // filter coefficients
pmic 5:d6c7ccbbce78 22 B = (float*)malloc((nb+1)*sizeof(float));
pmic 5:d6c7ccbbce78 23 A = (float*)malloc(na*sizeof(float));
pmic 5:d6c7ccbbce78 24 B[0] = 2.0f/(2.0f*T + Ts);
pmic 5:d6c7ccbbce78 25 B[1] = -B[0];
pmic 5:d6c7ccbbce78 26 A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts);
pmic 5:d6c7ccbbce78 27
pmic 5:d6c7ccbbce78 28 // signal arrays
pmic 5:d6c7ccbbce78 29 uk = (float*)malloc((nb+1)*sizeof(float));
pmic 5:d6c7ccbbce78 30 yk = (float*)malloc(na*sizeof(float));
pmic 5:d6c7ccbbce78 31 uk[0]= uk[1] = 0.0f;
pmic 5:d6c7ccbbce78 32 yk[0] = 0.0f;
pmic 5:d6c7ccbbce78 33
pmic 5:d6c7ccbbce78 34 // dc-gain
pmic 5:d6c7ccbbce78 35 this->K = 0.0f;
pmic 5:d6c7ccbbce78 36 }
pmic 5:d6c7ccbbce78 37
pmic 5:d6c7ccbbce78 38 // G(s) = K/(T*s + 1)
pmic 5:d6c7ccbbce78 39 IIR_filter::IIR_filter(float T,float Ts,float K){
pmic 5:d6c7ccbbce78 40
pmic 5:d6c7ccbbce78 41 // filter orders
rtlabor 0:15be70d21d7c 42 nb = 1; // Filter Order
rtlabor 0:15be70d21d7c 43 na = 1; // Filter Order
pmic 5:d6c7ccbbce78 44
pmic 5:d6c7ccbbce78 45 // filter coefficients
pmic 5:d6c7ccbbce78 46 B = (float*)malloc((nb+1)*sizeof(float));
pmic 5:d6c7ccbbce78 47 A = (float*)malloc(na*sizeof(float));
pmic 5:d6c7ccbbce78 48 B[0] = Ts/(Ts + 2.0f*T);
pmic 5:d6c7ccbbce78 49 B[1] = B[0];
pmic 5:d6c7ccbbce78 50 A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T);
pmic 5:d6c7ccbbce78 51
pmic 5:d6c7ccbbce78 52 // signal arrays
pmic 5:d6c7ccbbce78 53 uk = (float*)malloc((nb+1)*sizeof(float));
pmic 5:d6c7ccbbce78 54 yk = (float*)malloc(na*sizeof(float));
rtlabor 0:15be70d21d7c 55 uk[0]= uk[1] = 0.0f;
rtlabor 0:15be70d21d7c 56 yk[0] = 0.0f;
pmic 5:d6c7ccbbce78 57
pmic 5:d6c7ccbbce78 58 // dc-gain
pmic 5:d6c7ccbbce78 59 this->K = K;
pmic 5:d6c7ccbbce78 60 }
pmic 5:d6c7ccbbce78 61
pmic 5:d6c7ccbbce78 62 // G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2)
pmic 5:d6c7ccbbce78 63 IIR_filter::IIR_filter(float w0,float D, float Ts, float K){
pmic 5:d6c7ccbbce78 64
pmic 5:d6c7ccbbce78 65 // filter orders
pmic 5:d6c7ccbbce78 66 nb = 2; // Filter Order
pmic 5:d6c7ccbbce78 67 na = 2; // Filter Order
pmic 5:d6c7ccbbce78 68
pmic 5:d6c7ccbbce78 69 // filter coefficients
pmic 5:d6c7ccbbce78 70 B = (float*)malloc((nb+1)*sizeof(float));
pmic 5:d6c7ccbbce78 71 A = (float*)malloc(na*sizeof(float));
pmic 5:d6c7ccbbce78 72 float k0 = Ts*Ts*w0*w0;
pmic 5:d6c7ccbbce78 73 float k1 = 4.0f*D*Ts*w0;
pmic 5:d6c7ccbbce78 74 float k2 = k0 + k1 + 4.0f;
pmic 5:d6c7ccbbce78 75 B[0] = K*k0/k2;
pmic 5:d6c7ccbbce78 76 B[1] = 2.0f*B[0];
pmic 5:d6c7ccbbce78 77 B[2] = B[0];
pmic 5:d6c7ccbbce78 78 A[0] = (2.0f*k0 - 8.0f)/k2;
pmic 5:d6c7ccbbce78 79 A[1] = (k0 - k1 + 4.0f)/k2;
pmic 5:d6c7ccbbce78 80
pmic 5:d6c7ccbbce78 81 // signal arrays
pmic 5:d6c7ccbbce78 82 uk = (float*)malloc((nb+1)*sizeof(float));
pmic 5:d6c7ccbbce78 83 yk = (float*)malloc(na*sizeof(float));
pmic 5:d6c7ccbbce78 84 uk[0]= uk[1] = uk[2] = 0.0f;
pmic 5:d6c7ccbbce78 85 yk[0] = yk[1] = 0.0f;
pmic 5:d6c7ccbbce78 86
pmic 5:d6c7ccbbce78 87 // dc-gain
pmic 5:d6c7ccbbce78 88 this->K = K;
pmic 5:d6c7ccbbce78 89 }
rtlabor 0:15be70d21d7c 90
rtlabor 0:15be70d21d7c 91 IIR_filter::~IIR_filter() {}
rtlabor 0:15be70d21d7c 92
rtlabor 0:15be70d21d7c 93 void IIR_filter::reset(float val) {
pmic 5:d6c7ccbbce78 94 for(int k=0;k < nb;k++)
rtlabor 0:15be70d21d7c 95 uk[k] = val;
pmic 5:d6c7ccbbce78 96 for(int k=0;k < na;k++)
pmic 5:d6c7ccbbce78 97 yk[k] = val*K;
rtlabor 0:15be70d21d7c 98
rtlabor 0:15be70d21d7c 99 }
pmic 5:d6c7ccbbce78 100
pmic 5:d6c7ccbbce78 101 /*
pmic 5:d6c7ccbbce78 102 the filter is operating as follows:
pmic 5:d6c7ccbbce78 103 (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
pmic 5:d6c7ccbbce78 104 y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
pmic 5:d6c7ccbbce78 105 - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
rtlabor 0:15be70d21d7c 106 */
rtlabor 0:15be70d21d7c 107 float IIR_filter::filter(float input){
pmic 5:d6c7ccbbce78 108 for(int k = nb;k > 0;k--) // shift input values back
rtlabor 0:15be70d21d7c 109 uk[k] = uk[k-1];
rtlabor 0:15be70d21d7c 110 uk[0] = input;
rtlabor 0:15be70d21d7c 111 float ret = 0.0f;
pmic 5:d6c7ccbbce78 112 for(int k = 0;k <= nb;k++)
pmic 5:d6c7ccbbce78 113 ret += B[k] * uk[k];
pmic 5:d6c7ccbbce78 114 for(int k = 0;k < na;k++)
pmic 5:d6c7ccbbce78 115 ret -= A[k] * yk[k];
pmic 5:d6c7ccbbce78 116 for(int k = na;k > 1;k--)
rtlabor 0:15be70d21d7c 117 yk[k-1] = yk[k-2];
rtlabor 0:15be70d21d7c 118 yk[0] = ret;
rtlabor 0:15be70d21d7c 119 return ret;
pmic 5:d6c7ccbbce78 120 }
pmic 5:d6c7ccbbce78 121
pmic 5:d6c7ccbbce78 122 // (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
pmic 5:d6c7ccbbce78 123 /*
pmic 5:d6c7ccbbce78 124 IIR_filter::IIR_filter(float *a[], float *b[], float K){
pmic 5:d6c7ccbbce78 125
pmic 5:d6c7ccbbce78 126 this->A = A[0];
pmic 5:d6c7ccbbce78 127 this->B = B[0];
pmic 5:d6c7ccbbce78 128 nb = sizeof(B)/sizeof(B[0]);
pmic 5:d6c7ccbbce78 129 na = sizeof(A)/sizeof(A[0]);
pmic 5:d6c7ccbbce78 130 uk = (float*)malloc((nb+1)*sizeof(float));
pmic 5:d6c7ccbbce78 131 yk = (float*)malloc(na*sizeof(float));
pmic 5:d6c7ccbbce78 132 for(int ii=0; ii<nb; ii++){
pmic 5:d6c7ccbbce78 133 uk[ii] = 0.0f;
pmic 5:d6c7ccbbce78 134 }
pmic 5:d6c7ccbbce78 135 for(int ii=0; ii<na; ii++){
pmic 5:d6c7ccbbce78 136 yk[ii] = 0.0f;
pmic 5:d6c7ccbbce78 137 }
pmic 5:d6c7ccbbce78 138 this->K = K; %%% THIS IMPLEMENTATION SUITS NOT THE RESET PROCESS %%%
pmic 5:d6c7ccbbce78 139 }*/