File content as of revision 5:c2ce7b743efa:

#include "mbed.h"
#include "Ticker.h"
#include "BLE_Driver.h"
#include "MAX30100.h"
#include "I2C_Driver.h"
#include "PROCESAMIENTO_SPO2_FC.h"
#include "millis.h"

#define nRF51DK
//#define nRF52DK


#ifdef nRF52DK
#define SCL         28
#define SDA         29
#endif

#ifdef nRF51DK
#define SCL         7  
#define SDA         6
#endif

//**************************
int muestras_RED[200];
int muestras_IR[200];
int samples_index = 0;
int max_min_period[2];
int pico_pico,cresta, valle, cantidad;
float frecuencia,periodo;

int index, aux1, nmuestra;

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);
DigitalOut TEST_LED(p23, 1);
DigitalOut LED(p24, 1);
//DigitalIn TEST_BUTTON(p17,PullUp);

Ticker Flasher;
MAX30100 sensor;

void callbackBLE(int event) {
    switch (event){
        case 1:     CONECT_LED = 1; break;
        case 2:     CONECT_LED = 0; break;
        default:    break;
        }
    }

void periodicCallback(void){
    LIVE_LED =! LIVE_LED;
    }

void store_Samples(void){
    if (samples_index == 200){
        periodo = valores(muestras_IR,max_min_period);
        frecuencia=60/(periodo*0.02);
        samples_index = 0;
        sprintf(show, "%f", frecuencia);
        putBLE(show);
        putBLE("\n");
        }
    else{
        //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++;
        }
}

void sample(void){
    LED = 0;
    sensor.readSensor();
    sprintf(show, "%d", sensor.IR);
    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();
        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);
        }
    }