Successful add of order 2 Butterworth notch.

Author: finnkuusisto

src/Devlpr.cpp

@@ -7,6 +7,7 @@ Devlpr::Devlpr(int pin)
     emgRunningSum = 0;
     bufInd = BUFSIZE - 1;
     numFuncs = 0;
+    lastFiltVal = 0;
 }
 
 void Devlpr::tick()
@@ -77,6 +78,11 @@ int Devlpr::lastValueCentered()
     return lastVal - wAvg;
 }
 
+int Devlpr::lastValueFiltered()
+{
+    return lastFiltVal;
+}
+
 unsigned int Devlpr::windowPeakAmplitude()
 {
     // use the window average as the reference point (should be close to DC offset)
@@ -149,6 +155,8 @@ void Devlpr::readEMG()
     emgRunningSum = emgRunningSum - buf[bufInd] + emgVal;
     // now replace
     buf[bufInd] = emgVal;
+    // and we need to calculate the filtered value every sample
+    calcFiltered();
 }
 
 void Devlpr::flexCheck(unsigned long currMicros)
@@ -169,3 +177,19 @@ void Devlpr::flexCheck(unsigned long currMicros)
     // need to update the current peak-to-peak for next time
     prevPeakToPeak = currPeakToPeak;
 }
+
+void Devlpr::calcFiltered()
+{
+    // NOTE: IIR filter is recurrent and needs to run every tick to work
+    // we need to operate on 0-centered(ish) data and we will be doing float math
+    float xn = lastValueCentered();
+    // compute the recurrence by section
+    for (int s = 0; s < N_SECTIONS; s++) {
+        float xn_tmp = xn;
+        xn = notch60[s][0] * xn_tmp + z[s][0];
+        z[s][0] = (notch60[s][1] * xn_tmp - notch60[s][4] * xn + z[s][1]);
+        z[s][1] = (notch60[s][2] * xn_tmp - notch60[s][5] * xn);
+    }
+    // and store our most recent value for reference later (as an int now)
+    lastFiltVal = (int)xn;
+}

src/Devlpr.h

@@ -10,6 +10,7 @@ class Devlpr
         void tick();
         unsigned int lastValue();
         int lastValueCentered();
+        int lastValueFiltered();
         unsigned int windowAvg();
         unsigned int windowPeakAmplitude();
         unsigned int windowPeakToPeakAmplitude();
@@ -26,6 +27,18 @@ class Devlpr
         unsigned int emgVal; // ATMEGA boards have 10-bit ADC (0-1023)
         unsigned int emgRunningSum; // if BUFSIZE is small, uint is fine
         void readEMG();
+        // emg filtering
+        void calcFiltered();
+        int lastFiltVal;
+        static const byte N_SECTIONS = 2; // 2nd order
+        float notch60[2][6] = { // 2nd order Butterworth notch for 60Hz
+            {0.95654323 ,-1.77962093 ,0.95654323 ,1. ,-1.80093517 ,0.95415195},
+            {1.         ,-1.860471   ,1.         ,1. ,-1.83739919 ,0.95894143}
+        };
+        float z[2][2] = { // maintain recurrent state
+            {0.0, 0.0},
+            {0.0, 0.0}
+        };
         // scheduling
         unsigned long lastTickMicros = 0L;
         // emg scheduling