From cc03f6a69f6cd4883eb7bfdc3d9bb3cc78fd5c1d Mon Sep 17 00:00:00 2001
From: Angelo Elias Dalzotto <angelodalzotto97@gmail.com>
Date: Tue, 7 May 2019 11:32:48 -0300
Subject: [PATCH] fixed mean diff and enhanced moving average

---
 MAX30100.cpp | 48 +++++++++++++++++++++++++++++-------------------
 MAX30100.h   |  4 +++-
 2 files changed, 32 insertions(+), 20 deletions(-)

diff --git a/MAX30100.cpp b/MAX30100.cpp
index 25206d1..4d09a38 100644
--- a/MAX30100.cpp
+++ b/MAX30100.cpp
@@ -62,12 +62,13 @@ MAX30100::MAX30100(
   lpbFilterIR.v[1] = 0;
   lpbFilterIR.result = 0;
 
+  memset(meanDiffIR.values, 0, sizeof(float)*MEAN_FILTER_SIZE);
   meanDiffIR.index = 0;
   meanDiffIR.sum = 0;
   meanDiffIR.count = 0;
 
-
-  valuesBPM[0] = 0;
+  // Erase all valuesBPM
+  memset(valuesBPM, 0, sizeof(float)*PULSE_BPM_SAMPLE_SIZE);
   valuesBPMSum = 0;
   valuesBPMCount = 0;
   bpmIndex = 0;
@@ -203,23 +204,38 @@ bool MAX30100::detectPulse(float sensor_value)
         if(debug == true) 
           Serial.println(rawBPM);
 
-        //This method sometimes glitches, it's better to go through whole moving average everytime
-        //IT's a neat idea to optimize the amount of work for moving avg. but while placing, removing finger it can screw up
-        //valuesBPMSum -= valuesBPM[bpmIndex];
-        //valuesBPM[bpmIndex] = rawBPM;
-        //valuesBPMSum += valuesBPM[bpmIndex];
+        currentPulseDetectorState = PULSE_TRACE_DOWN;
 
-        valuesBPM[bpmIndex] = rawBPM;
-        valuesBPMSum = 0;
-        for(int i=0; i<PULSE_BPM_SAMPLE_SIZE; i++)
-        {
-          valuesBPMSum += valuesBPM[i];
+        // Reset moving avg after a while without pulses
+        if(beatDuration > PULSE_BEAT_DURATION_RST){
+          memset(valuesBPM, 0, sizeof(float)*PULSE_BPM_SAMPLE_SIZE);
+          valuesBPMSum = 0;
+          bpmIndex = 0;
+          valuesBPMCount = 0;
+
+          if(debug)
+            Serial.println("Moving avg. reseted");
         }
 
+        // Eliminate glitchy values
+        if(rawBPM > PULSE_MAX_VALUE || rawBPM < PULSE_MIN_VALUE){
+          if(debug)
+            Serial.println("BPM out of bounds. Not adding to moving avg.");
+            
+          return false;
+        }
+
+        valuesBPMSum -= valuesBPM[bpmIndex];
+        valuesBPM[bpmIndex] = rawBPM;
+        valuesBPMSum += valuesBPM[bpmIndex];
+
+        if(valuesBPMCount < PULSE_BPM_SAMPLE_SIZE)
+          valuesBPMCount++;
+
         if(debug == true) 
         {
           Serial.print("CurrentMoving Avg: ");
-          for(int i=0; i<PULSE_BPM_SAMPLE_SIZE; i++)
+          for(int i=0; i<valuesBPMCount; i++)
           {
             Serial.print(valuesBPM[i]);
             Serial.print(" ");
@@ -231,9 +247,6 @@ bool MAX30100::detectPulse(float sensor_value)
         bpmIndex++;
         bpmIndex = bpmIndex % PULSE_BPM_SAMPLE_SIZE;
 
-        if(valuesBPMCount < PULSE_BPM_SAMPLE_SIZE)
-          valuesBPMCount++;
-
         currentBPM = valuesBPMSum / valuesBPMCount;
         if(debug == true) 
         {
@@ -241,9 +254,6 @@ bool MAX30100::detectPulse(float sensor_value)
           Serial.println(currentBPM);
         }
 
-
-        currentPulseDetectorState = PULSE_TRACE_DOWN;
-
         return true;
       }
       break;
diff --git a/MAX30100.h b/MAX30100.h
index bbae26c..6446e48 100644
--- a/MAX30100.h
+++ b/MAX30100.h
@@ -64,7 +64,9 @@ SOFTWARE.
 #define PULSE_GO_DOWN_THRESHOLD     1
 
 #define PULSE_BPM_SAMPLE_SIZE       10 //Moving average size
-
+#define PULSE_BEAT_DURATION_RST   2500 // Time in ms to reset the moving avg
+#define PULSE_MAX_VALUE            220
+#define PULSE_MIN_VALUE             50