Diff: mainbletest.cpp

Revision:

5:c2ce7b743efa

Parent:

3:98ca4bf2e74a

Child:

6:c08d3fe6347e

--- a/mainbletest.cpp	Tue Mar 28 23:17:44 2017 +0000
+++ b/mainbletest.cpp	Tue May 30 01:00:34 2017 +0000
@@ -4,6 +4,7 @@
 #include "MAX30100.h"
 #include "I2C_Driver.h"
 #include "PROCESAMIENTO_SPO2_FC.h"
+#include "millis.h"
 
 #define nRF51DK
 //#define nRF52DK
@@ -32,7 +33,48 @@
 char show[10];
 
 //******************
+#define PULSE_MIN_THRESHOLD         100 //300 is good for finger, but for wrist you need like 20, and there is shitloads of noise
+#define PULSE_MAX_THRESHOLD         2000
+#define PULSE_GO_DOWN_THRESHOLD     1
 
+#define PULSE_BPM_SAMPLE_SIZE       10 //Moving average size
+struct dcFilter_t {
+  float w;
+  float result;
+};
+
+struct meanDiffFilter_t
+{
+  float values[15];
+  uint8_t index;
+  float sum;
+  uint8_t count;
+};
+
+struct butterworthFilter_t
+{
+  float v[2];
+  float result;
+};
+
+typedef enum PulseStateMachine {
+    PULSE_IDLE,
+    PULSE_TRACE_UP,
+    PULSE_TRACE_DOWN
+} PulseStateMachine;
+
+dcFilter_t dcFilterIR;
+meanDiffFilter_t meanDiffIR;
+butterworthFilter_t lpbFilterIR;
+float prev_w = 0;
+uint32_t lastBeatThreshold;
+uint8_t currentPulseDetectorState;
+float currentBPM;
+float valuesBPM[PULSE_BPM_SAMPLE_SIZE];
+float valuesBPMSum;
+uint8_t valuesBPMCount;
+uint8_t bpmIndex;
+//**************************
 InterruptIn SMPRDY(p0);
 DigitalOut LIVE_LED(p21, 1);
 DigitalOut CONECT_LED(p22, 1);
@@ -65,11 +107,11 @@
         putBLE("\n");
         }
     else{
-        muestras_RED[samples_index] = sensor.RED;
-        muestras_IR[samples_index] = sensor.IR;
-        sprintf(show, "%d", muestras_IR[samples_index]);
-        putBLE(show);
-        putBLE("\n");
+        //muestras_RED[samples_index] = sensor.RED;
+        muestras_IR[samples_index] = lpbFilterIR.result;
+        //sprintf(show, "%d", muestras_IR[samples_index]);
+        //putBLE(show);
+        //putBLE("\n");
         samples_index++;
         }
 }
@@ -81,15 +123,203 @@
     putBLE(show);
     putBLE("\n");
     }
+
+/***************************************************************************/
+    
+dcFilter_t DCfilter(float x, float prev, float alpha)
+{
+    dcFilter_t filtered;
+    filtered.w = x + alpha * prev;
+    filtered.result = filtered.w - prev;
+    prev_w = filtered.w;
+    return filtered;
+}
+
+float meanDiff(float M, meanDiffFilter_t* filterValues)
+{
+  float avg = 0;
+
+  filterValues->sum -= filterValues->values[filterValues->index];
+  filterValues->values[filterValues->index] = M;
+  filterValues->sum += filterValues->values[filterValues->index];
+
+  filterValues->index++;
+  filterValues->index = filterValues->index % 15;
+
+  if(filterValues->count < 15)
+    filterValues->count++;
+
+  avg = filterValues->sum / filterValues->count;
+  return avg - M;
+}
+
+void lowPassButterworthFilter( float x, butterworthFilter_t * filterResult )
+{  
+  filterResult->v[0] = filterResult->v[1];
+
+  //Fs = 100Hz and Fc = 10Hz
+  filterResult->v[1] = (2.452372752527856026e-1 * x) + (0.50952544949442879485 * filterResult->v[0]);
+
+  //Fs = 100Hz and Fc = 4Hz
+  //filterResult->v[1] = (1.367287359973195227e-1 * x) + (0.72654252800536101020 * filterResult->v[0]); //Very precise butterworth filter 
+
+  filterResult->result = filterResult->v[0] + filterResult->v[1];
+}
+
+bool detectPulse(float sensor_value)
+{
+  static float prev_sensor_value = 0;
+  static uint8_t values_went_down = 0;
+  static uint32_t currentBeat = 0;
+  static uint32_t lastBeat = 0;
+
+  if(sensor_value > PULSE_MAX_THRESHOLD)
+  {
+    currentPulseDetectorState = PULSE_IDLE;
+    prev_sensor_value = 0;
+    lastBeat = 0;
+    currentBeat = 0;
+    values_went_down = 0;
+    lastBeatThreshold = 0;
+    return false;
+  }
+
+  switch(currentPulseDetectorState)
+  {
+    case PULSE_IDLE:
+      if(sensor_value >= PULSE_MIN_THRESHOLD) {
+        currentPulseDetectorState = PULSE_TRACE_UP;
+        values_went_down = 0;
+      }
+      break;
+
+    case PULSE_TRACE_UP:
+      if(sensor_value > prev_sensor_value)
+      {
+        currentBeat = millis();
+        lastBeatThreshold = sensor_value;
+      }
+      else
+      {
+
+        uint32_t beatDuration = currentBeat - lastBeat;
+        lastBeat = currentBeat;
+
+        float rawBPM = 0;
+        if(beatDuration > 0)
+          rawBPM = 60000.0 / (float)beatDuration;
+//        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];
+
+        valuesBPM[bpmIndex] = rawBPM;
+        valuesBPMSum = 0;
+        for(int i=0; i<PULSE_BPM_SAMPLE_SIZE; i++)
+        {
+          valuesBPMSum += valuesBPM[i];
+        }
+
+/*        if(debug == true) 
+        {
+          Serial.print("CurrentMoving Avg: ");
+          for(int i=0; i<PULSE_BPM_SAMPLE_SIZE; i++)
+          {
+            Serial.print(valuesBPM[i]);
+            Serial.print(" ");
+          }
+  
+          Serial.println(" ");
+        }*/
+
+        bpmIndex++;
+        bpmIndex = bpmIndex % PULSE_BPM_SAMPLE_SIZE;
+
+        if(valuesBPMCount < PULSE_BPM_SAMPLE_SIZE)
+          valuesBPMCount++;
+
+        currentBPM = valuesBPMSum / valuesBPMCount;
+        //sprintf(show, "%f", currentBPM);
+        //putBLE(show);
+        //putBLE("\n");
+/*        if(debug == true) 
+        {
+          Serial.print("AVg. BPM: ");
+          Serial.println(currentBPM);
+        }*/
+
+
+        currentPulseDetectorState = PULSE_TRACE_DOWN;
+
+        return true;
+      }
+      break;
+
+    case PULSE_TRACE_DOWN:
+      if(sensor_value < prev_sensor_value)
+      {
+        values_went_down++;
+      }
+
+
+      if(sensor_value < PULSE_MIN_THRESHOLD)
+      {
+        currentPulseDetectorState = PULSE_IDLE;
+      }
+      break;
+  }
+
+  prev_sensor_value = sensor_value;
+  return false;
+}
+
+/***************************************************************************/
+
 int main(void){
+    millisStart();
+    dcFilterIR.w = 0;
+    dcFilterIR.result = 0;
+    meanDiffIR.index = 0;
+    meanDiffIR.sum = 0;
+    meanDiffIR.count = 0;
+    lpbFilterIR.v[0] = 0;
+    lpbFilterIR.v[1] = 0;
+    lpbFilterIR.result = 0;
+    valuesBPM[0] = 0;
+    valuesBPMSum = 0;
+    valuesBPMCount = 0;
+    bpmIndex = 0;
+
+/*    irACValueSqSum = 0;
+    redACValueSqSum = 0;
+    samplesRecorded = 0;
+    pulsesDetected = 0;
+    currentSaO2Value = 0;
+*/
+    lastBeatThreshold = 0;
+
+    currentPulseDetectorState = PULSE_IDLE;  
+  
+    float meanDiffResIR;
     SMPRDY.fall(&sample);
     Flasher.attach(periodicCallback, 1);
     ini_i2c(SCL, SDA);
     sensor.begin(pw1600, i17, sr50 );
     iniBLE("Sigfried");
     while(1){
-        //sensor.readSensor();
+        sensor.readSensor();
+        dcFilterIR = DCfilter((float)sensor.IR, dcFilterIR.w, 0.95);
+        meanDiffResIR = meanDiff( dcFilterIR.result, &meanDiffIR);
+        lowPassButterworthFilter( meanDiffResIR/*-dcFilterIR.result*/, &lpbFilterIR );
+        detectPulse( lpbFilterIR.result );
         //store_Samples();
+        sprintf(show, "%f", sensor.IR);
+        putBLE(show);
+        putBLE("\n");
         //wait(0.019);
         }
     }