Revision 0:8080080b83cf, committed 2019-08-12

Comitter:

pmic

Date:

Mon Aug 12 13:29:07 2019 +0000

Commit message:

delete old comments

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IIR_filter.cpp	Mon Aug 12 13:29:07 2019 +0000
@@ -0,0 +1,147 @@
+#include "IIR_filter.h"
+#include "mbed.h"
+using namespace std;
+
+/*
+  IIR filter implemention for the following filter types:
+  init for: first order differentiatior:   G(s) = s/(T*s + 1)
+            first order lowpass with gain  G(s) = K/(T*s + 1)
+            second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0)        
+            nth order, with arbitrary values
+  the billinear transformation is used for s -> z
+  reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator
+*/
+
+// G(s) = s/(T*s + 1)
+IIR_filter::IIR_filter(float T, float Ts){
+           
+    // filter orders
+    nb = 1; // Filter Order
+    na = 1; // Filter Order
+    
+    // filter coefficients
+    B = (double*)malloc((nb+1)*sizeof(double));
+    A = (double*)malloc(na*sizeof(double));    
+    B[0] = 2.0/(2.0*(double)T + (double)Ts);
+    B[1] = -B[0];
+    A[0] = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
+    
+    // signal arrays
+    uk = (double*)malloc((nb+1)*sizeof(double));
+    yk = (double*)malloc(na*sizeof(double));
+    uk[0]= uk[1] = 0.0;
+    yk[0] = 0.0;
+    
+    // dc-gain
+    this->K = 0.0;
+}
+
+// G(s) = K/(T*s + 1)
+IIR_filter::IIR_filter(float T, float Ts, float K){
+    
+    // filter orders
+    nb = 1; // Filter Order
+    na = 1; // Filter Order
+    
+    // filter coefficients
+    B = (double*)malloc((nb+1)*sizeof(double));
+    A = (double*)malloc(na*sizeof(double));      
+    B[0] = (double)Ts/((double)Ts + 2.0*(double)T);
+    B[1] = B[0];
+    A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); 
+    
+    // signal arrays
+    uk = (double*)malloc((nb+1)*sizeof(double));
+    yk = (double*)malloc(na*sizeof(double));
+    uk[0]= uk[1] = 0.0;
+    yk[0] = 0.0;
+    
+    // dc-gain
+    this->K = (double)K;
+}
+
+// G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) 
+IIR_filter::IIR_filter(float w0, float D, float Ts, float K){
+    
+    // filter orders
+    nb = 2; // Filter Order
+    na = 2; // Filter Order
+    
+    // filter coefficients
+    B = (double*)malloc((nb+1)*sizeof(double));
+    A = (double*)malloc(na*sizeof(double));
+    double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0;
+    double k1 = 4.0*(double)D*(double)Ts*(double)w0;
+    double k2 = k0 + k1 + 4.0;    
+    B[0] = (double)K*k0/k2;
+    B[1] = 2.0*B[0];
+    B[2] = B[0]; 
+    A[0] = (2.0*k0 - 8.0)/k2;
+    A[1] = (k0 - k1 + 4.0)/k2;
+    
+    // signal arrays
+    uk = (double*)malloc((nb+1)*sizeof(double));
+    yk = (double*)malloc(na*sizeof(double));
+    uk[0]= uk[1] = uk[2] = 0.0;
+    yk[0] = yk[1] = 0.0;
+    
+    // dc-gain
+    this->K = (double)K;
+}
+
+IIR_filter::IIR_filter(float *b, float *a, int nb_, int na_){
+    
+    // filter orders
+    this->nb = nb_-1;    // Filter Order
+    this->na = na_;      // Filter Order
+    
+    // filter coefficients
+    B = (double*)malloc((nb+1)*sizeof(double));
+    A = (double*)malloc(na*sizeof(double));
+    uk = (double*)malloc((nb+1)*sizeof(double));
+    yk = (double*)malloc(na*sizeof(double));
+    
+    for(int k=0;k<=nb;k++){
+        B[k]=b[k];
+        uk[k]=0.0;
+        }
+    for(int k=0;k<na;k++){
+        A[k] = a[k];
+        yk[k] = 0.0;
+        }
+    
+    // dc-gain
+    this->K = 1.0;
+}
+
+    
+IIR_filter::~IIR_filter() {} 
+    
+void IIR_filter::reset(float val) {
+    for(int k=0;k < nb;k++)
+        uk[k] = (double)val;
+    for(int k=0;k < na;k++)
+        yk[k] = (double)val*K;
+        
+}
+
+/* 
+    the filter is operating as follows: 
+    (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
+    y(n) =  B[0]*u(k)   + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
+          - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
+*/
+float IIR_filter::filter(double input){
+    for(int k = nb;k > 0;k--)    // shift input values back
+        uk[k] = uk[k-1];
+    uk[0] = input;
+    double ret = 0.0;
+    for(int k = 0;k <= nb;k++)
+        ret += B[k] * uk[k];
+    for(int k = 0;k < na;k++)
+        ret -= A[k] * yk[k];
+    for(int k = na;k > 1;k--)
+        yk[k-1] = yk[k-2];
+    yk[0] = ret;
+    return (float)ret;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IIR_filter.h	Mon Aug 12 13:29:07 2019 +0000
@@ -0,0 +1,25 @@
+class IIR_filter{
+     public:
+     
+        IIR_filter(float T, float Ts);
+        IIR_filter(float T, float Ts, float K);
+        IIR_filter(float w0, float D, float Ts, float K);
+        IIR_filter(float *b, float *a, int nb_, int na_);
+                    
+        float operator()(float u){
+            return filter((double)u);
+         }
+        virtual     ~IIR_filter();
+        void        reset(float);
+        float       filter(double);
+    
+    private:
+
+        unsigned int nb;
+        unsigned int na;
+        double *B;
+        double *A;
+        double *uk;
+        double *yk;
+        double K;
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Aug 12 13:29:07 2019 +0000
@@ -0,0 +1,82 @@
+#include "mbed.h"
+#include "IIR_filter.h"
+
+/*  Notes pmic 07.08.2019:
+  - ...
+*/
+
+AnalogIn dist(PC_4);                      // analog IN (acc x) on PA_0
+float d = 0.0f;
+
+Serial pc(SERIAL_TX, SERIAL_RX);  // serial connection via USB - programmer
+InterruptIn Button(USER_BUTTON);  // User Button
+Ticker  LoopTimer;                // interrupt for control loop
+Timer t;                          // timer to analyse Button
+
+int   k;
+bool  doRun = false;
+float Ts = 0.05f;                 // sample time of main loop, 50 Hz
+
+IIR_filter pt1(0.5f, Ts, 1.0f);
+float df = 0.0f;
+
+// user defined functions
+void updateLoop(void);   // loop (via interrupt)
+void pressed(void);      // user Button pressed
+void released(void);     // user Button released
+
+// main program and control loop
+// -----------------------------------------------------------------------------
+int main()
+{
+    pc.baud(2000000);                  // for serial comm.
+    LoopTimer.attach(&updateLoop, Ts); // attach loop to timer interrupt
+    Button.fall(&pressed);             // attach key pressed function
+    Button.rise(&released);            // attach key pressed function
+    k = 0;
+    pt1.reset(0.0f);
+}
+
+// the updateLoop starts as soon as you pressed the blue botton
+void updateLoop(void)
+{
+    // d = usGain*rangefinder() + usOffset;
+    d = dist.read();
+    df = pt1(d);
+    if(doRun) {
+        ///*
+        // use this section to do dynamical measurements
+        if(doRun && k++ < 1600) {
+            pc.printf("%10i %10.9e\r\n", k, d);
+        }
+        //*/
+        /*
+        // use this section to do static measurements
+        if(doRun && k++%25 == 0) {
+            pc.printf("%10i %10.6e\r\n", k, df);
+        }
+        */
+    }
+}
+
+// buttonhandling
+// -----------------------------------------------------------------------------
+// start timer as soon as button is pressed
+void pressed()
+{
+    t.start();
+}
+
+// evaluating statemachine
+void released()
+{
+    // toggle state over boolean
+    if(doRun) {
+        k = 0;
+        pt1.reset(0.0f);
+    }
+    doRun = !doRun;
+    t.stop();
+    t.reset();
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Aug 12 13:29:07 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file