Diff: FiltroIIR.cpp

Revision:

0:785481a6b9bb

diff -r 000000000000 -r 785481a6b9bb FiltroIIR.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FiltroIIR.cpp	Mon Mar 31 21:49:05 2014 +0000
@@ -0,0 +1,47 @@
+#include "FiltroIIR.h"
+#include "mbed.h"
+ 
+FiltroIIR::FiltroIIR(int32_t* _ACoef, int32_t* _BCoef){
+    ACoef = _ACoef;
+    BCoef = _BCoef;
+    OrdemFiltro = sizeof(_ACoef);
+    VetorX = new int32_t [OrdemFiltro];
+    VetorY = new int32_t [OrdemFiltro];
+    Q = 12;
+    K = (1 << (Q-1));   
+}
+ 
+int32_t FiltroIIR::CalculaNovaAmostra(int32_t _amostraAtual){
+    //static int32_t y[1]; //output samples
+    //static int32_t x[1]; //input samples
+    int n;
+
+    //shift the old samples
+    for(n=OrdemFiltro; n>0; n--) {
+       VetorX[n] = VetorX[n-1];
+       VetorY[n] = VetorY[n-1];
+    }
+
+    //Calculate the new output
+    VetorX[0] = _amostraAtual;
+    VetorY[0] = QMul(ACoef[0], VetorX[0]);
+    for(n=1; n<OrdemFiltro; n++)
+        VetorY[0] += QMul(ACoef[n], VetorX[n]) - QMul(BCoef[n],VetorY[n]);
+    return abs(VetorY[0]);
+}
+
+int32_t FiltroIIR::QMul(int32_t op_a, int32_t op_b){
+    int32_t temp, result;
+    temp = op_a * op_b;
+    temp += K;
+    result = temp >> Q;
+    return result;
+}
+
+int32_t FiltroIIR::QDiv(int32_t op_a, int32_t op_b){
+    int32_t temp, result;
+    temp = op_a << Q;
+    temp += op_b/2;
+    result = temp/op_b;
+    return result;
+}