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@0:15be70d21d7c, 2018-01-10 (annotated)

Committer:
rtlabor
Date:
Wed Jan 10 16:08:07 2018 +0000
Revision:
0:15be70d21d7c
Child:
5:d6c7ccbbce78
balance cuboid

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;
rtlabor 0:15be70d21d7c 4 /* IIR filter. implemention is for n-th order, init only for 1st order
rtlabor 0:15be70d21d7c 5 coeffissients based on bilinear transform if 1st order filter
rtlabor 0:15be70d21d7c 6
rtlabor 0:15be70d21d7c 7 1 Ts/(Ts+2*tau) * (z+1)
rtlabor 0:15be70d21d7c 8 G(s) = ------- G(z) = ---------------------------
rtlabor 0:15be70d21d7c 9 tau*s+1 z + (Ts-2*tau)/(Ts+2*tau)
rtlabor 0:15be70d21d7c 10 */
rtlabor 0:15be70d21d7c 11 IIR_filter::IIR_filter(float tau,float Ts){
rtlabor 0:15be70d21d7c 12 b = (float*)malloc( 2 * sizeof(float) );
rtlabor 0:15be70d21d7c 13 a = (float*)malloc( 1 * sizeof(float) );
rtlabor 0:15be70d21d7c 14 uk = (float*)malloc( 2 * sizeof(float) );
rtlabor 0:15be70d21d7c 15 yk = (float*)malloc( 1 * sizeof(float) );
rtlabor 0:15be70d21d7c 16 nb = 1; // Filter Order
rtlabor 0:15be70d21d7c 17 na = 1; // Filter Order
rtlabor 0:15be70d21d7c 18 b[0] = Ts/(Ts+2.0f*tau);
rtlabor 0:15be70d21d7c 19 b[1] = b[0];
rtlabor 0:15be70d21d7c 20 a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
rtlabor 0:15be70d21d7c 21 uk[0]= uk[1] = 0.0f;
rtlabor 0:15be70d21d7c 22 yk[0] = 0.0f;
rtlabor 0:15be70d21d7c 23 this->dc = 1.0f;
rtlabor 0:15be70d21d7c 24 }
rtlabor 0:15be70d21d7c 25 // the following filter has in addition a dc-gain ( dc/(tau*s+1) )
rtlabor 0:15be70d21d7c 26 IIR_filter::IIR_filter(float tau,float Ts,float dc){
rtlabor 0:15be70d21d7c 27 b = (float*)malloc( 2 * sizeof(float) );
rtlabor 0:15be70d21d7c 28 a = (float*)malloc( 1 * sizeof(float) );
rtlabor 0:15be70d21d7c 29 uk = (float*)malloc( 2 * sizeof(float) );
rtlabor 0:15be70d21d7c 30 yk = (float*)malloc( 1 * sizeof(float) );
rtlabor 0:15be70d21d7c 31 nb = 1; // Filter Order
rtlabor 0:15be70d21d7c 32 na = 1; // Filter Order
rtlabor 0:15be70d21d7c 33 b[0] = dc * Ts/(Ts+2.0f*tau);
rtlabor 0:15be70d21d7c 34 b[1] = b[0];
rtlabor 0:15be70d21d7c 35 a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
rtlabor 0:15be70d21d7c 36 uk[0]= uk[1] = 0.0f;
rtlabor 0:15be70d21d7c 37 yk[0] = 0.0f;
rtlabor 0:15be70d21d7c 38 this->dc = dc;
rtlabor 0:15be70d21d7c 39 }
rtlabor 0:15be70d21d7c 40
rtlabor 0:15be70d21d7c 41 IIR_filter::~IIR_filter() {}
rtlabor 0:15be70d21d7c 42
rtlabor 0:15be70d21d7c 43 void IIR_filter::reset(float val) {
rtlabor 0:15be70d21d7c 44 for(unsigned int k=0;k < nb;k++)
rtlabor 0:15be70d21d7c 45 uk[k] = val;
rtlabor 0:15be70d21d7c 46 for(unsigned int k=0;k < na;k++)
rtlabor 0:15be70d21d7c 47 yk[k] = val*dc;
rtlabor 0:15be70d21d7c 48
rtlabor 0:15be70d21d7c 49 }
rtlabor 0:15be70d21d7c 50 /* the filter step:
rtlabor 0:15be70d21d7c 51 y(n) = b[0]*u_k + b[1]*u_k-1 + ... + b[nb]*u_k-nb
rtlabor 0:15be70d21d7c 52 - a[0]*y_k-1 - ... - a[na]*y_n-na // mind: a[0] corresponds to z^(na-1)
rtlabor 0:15be70d21d7c 53 */
rtlabor 0:15be70d21d7c 54 float IIR_filter::filter(float input){
rtlabor 0:15be70d21d7c 55 unsigned int k;
rtlabor 0:15be70d21d7c 56 for(k = nb;k > 0;k--) // shift input values back
rtlabor 0:15be70d21d7c 57 uk[k] = uk[k-1];
rtlabor 0:15be70d21d7c 58 uk[0] = input;
rtlabor 0:15be70d21d7c 59 float ret = 0.0f;
rtlabor 0:15be70d21d7c 60 for(k = 0;k <= nb;k++)
rtlabor 0:15be70d21d7c 61 ret += b[k] * uk[k];
rtlabor 0:15be70d21d7c 62 for(k = 0;k < na;k++)
rtlabor 0:15be70d21d7c 63 ret -= a[k] * yk[k];
rtlabor 0:15be70d21d7c 64 for(k = na;k > 1;k--)
rtlabor 0:15be70d21d7c 65 yk[k-1] = yk[k-2];
rtlabor 0:15be70d21d7c 66 yk[0] = ret;
rtlabor 0:15be70d21d7c 67 return ret;
rtlabor 0:15be70d21d7c 68 }