mainbletest.cpp

00001 #include "mbed.h"
00002 #include "Ticker.h"
00003 #include "BLE_Driver.h"
00004 #include "MAX30100.h"
00005 #include "I2C_Driver.h"
00006 #include "PROCESAMIENTO_SPO2_FC.h"
00007 #include "millis.h"
00008 
00009 #define nRF51DK
00010 //#define nRF52DK
00011 
00012 
00013 #ifdef nRF52DK
00014 #define SCL         28
00015 #define SDA         29
00016 #endif
00017 
00018 #ifdef nRF51DK
00019 #define SCL         7  
00020 #define SDA         6
00021 #endif
00022 
00023 //**************************
00024 int muestras_RED[200];
00025 int muestras_IR[200];
00026 int samples_index = 0;
00027 int max_min_period[2];
00028 int pico_pico,cresta, valle, cantidad;
00029 float frecuencia,periodo;
00030 
00031 int index, aux1, nmuestra;
00032 
00033 char show[10];
00034 
00035 //******************
00036 #define PULSE_MIN_THRESHOLD         100 //300 is good for finger, but for wrist you need like 20, and there is shitloads of noise
00037 #define PULSE_MAX_THRESHOLD         2000
00038 #define PULSE_GO_DOWN_THRESHOLD     1
00039 
00040 #define PULSE_BPM_SAMPLE_SIZE       10 //Moving average size
00041 struct dcFilter_t {
00042   float w;
00043   float result;
00044 };
00045 
00046 struct meanDiffFilter_t
00047 {
00048   float values[15];
00049   uint8_t index;
00050   float sum;
00051   uint8_t count;
00052 };
00053 
00054 struct butterworthFilter_t
00055 {
00056   float v[2];
00057   float result;
00058 };
00059 
00060 typedef enum PulseStateMachine {
00061     PULSE_IDLE,
00062     PULSE_TRACE_UP,
00063     PULSE_TRACE_DOWN
00064 } PulseStateMachine;
00065 
00066 dcFilter_t dcFilterIR;
00067 meanDiffFilter_t meanDiffIR;
00068 butterworthFilter_t lpbFilterIR;
00069 float prev_w = 0;
00070 uint32_t lastBeatThreshold;
00071 uint8_t currentPulseDetectorState;
00072 float currentBPM;
00073 float valuesBPM[PULSE_BPM_SAMPLE_SIZE];
00074 float valuesBPMSum;
00075 uint8_t valuesBPMCount;
00076 uint8_t bpmIndex;
00077 //**************************
00078 InterruptIn SMPRDY(p0);
00079 DigitalOut LIVE_LED(p21, 1);
00080 DigitalOut CONECT_LED(p22, 1);
00081 DigitalOut TEST_LED(p23, 1);
00082 DigitalOut LED(p24, 1);
00083 //DigitalIn TEST_BUTTON(p17,PullUp);
00084 
00085 Ticker Flasher;
00086 MAX30100 sensor;
00087 
00088 void callbackBLE(int event) {
00089     switch (event){
00090         case 1:     CONECT_LED = 1; break;
00091         case 2:     CONECT_LED = 0; break;
00092         default:    break;
00093         }
00094     }
00095 
00096 void periodicCallback(void){
00097     LIVE_LED =! LIVE_LED;
00098     }
00099 
00100 void store_Samples(void){
00101     if (samples_index == 200){
00102         periodo = valores(muestras_IR,max_min_period);
00103         frecuencia=60/(periodo*0.02);
00104         samples_index = 0;
00105         sprintf(show, "%f", frecuencia);
00106         putBLE(show);
00107         putBLE("\n");
00108         }
00109     else{
00110         //muestras_RED[samples_index] = sensor.RED;
00111         muestras_IR[samples_index] = lpbFilterIR.result;
00112         //sprintf(show, "%d", muestras_IR[samples_index]);
00113         //putBLE(show);
00114         //putBLE("\n");
00115         samples_index++;
00116         }
00117 }
00118 
00119 void sample(void){
00120     LED = 0;
00121     sensor.readSensor();
00122     sprintf(show, "%d", sensor.IR);
00123     putBLE(show);
00124     putBLE("\n");
00125     }
00126 
00127 /***************************************************************************/
00128     
00129 dcFilter_t DCfilter(float x, float prev, float alpha)
00130 {
00131     dcFilter_t filtered;
00132     filtered.w = x + alpha * prev;
00133     filtered.result = filtered.w - prev;
00134     prev_w = filtered.w;
00135     return filtered;
00136 }
00137 
00138 float meanDiff(float M, meanDiffFilter_t* filterValues)
00139 {
00140   float avg = 0;
00141 
00142   filterValues->sum -= filterValues->values[filterValues->index];
00143   filterValues->values[filterValues->index] = M;
00144   filterValues->sum += filterValues->values[filterValues->index];
00145 
00146   filterValues->index++;
00147   filterValues->index = filterValues->index % 15;
00148 
00149   if(filterValues->count < 15)
00150     filterValues->count++;
00151 
00152   avg = filterValues->sum / filterValues->count;
00153   return avg - M;
00154 }
00155 
00156 void lowPassButterworthFilter( float x, butterworthFilter_t * filterResult )
00157 {  
00158   filterResult->v[0] = filterResult->v[1];
00159 
00160   //Fs = 100Hz and Fc = 10Hz
00161   filterResult->v[1] = (2.452372752527856026e-1 * x) + (0.50952544949442879485 * filterResult->v[0]);
00162 
00163   //Fs = 100Hz and Fc = 4Hz
00164   //filterResult->v[1] = (1.367287359973195227e-1 * x) + (0.72654252800536101020 * filterResult->v[0]); //Very precise butterworth filter 
00165 
00166   filterResult->result = filterResult->v[0] + filterResult->v[1];
00167 }
00168 
00169 bool detectPulse(float sensor_value)
00170 {
00171   static float prev_sensor_value = 0;
00172   static uint8_t values_went_down = 0;
00173   static uint32_t currentBeat = 0;
00174   static uint32_t lastBeat = 0;
00175 
00176   if(sensor_value > PULSE_MAX_THRESHOLD)
00177   {
00178     currentPulseDetectorState = PULSE_IDLE;
00179     prev_sensor_value = 0;
00180     lastBeat = 0;
00181     currentBeat = 0;
00182     values_went_down = 0;
00183     lastBeatThreshold = 0;
00184     return false;
00185   }
00186 
00187   switch(currentPulseDetectorState)
00188   {
00189     case PULSE_IDLE:
00190       if(sensor_value >= PULSE_MIN_THRESHOLD) {
00191         currentPulseDetectorState = PULSE_TRACE_UP;
00192         values_went_down = 0;
00193       }
00194       break;
00195 
00196     case PULSE_TRACE_UP:
00197       if(sensor_value > prev_sensor_value)
00198       {
00199         currentBeat = millis();
00200         lastBeatThreshold = sensor_value;
00201       }
00202       else
00203       {
00204 
00205         uint32_t beatDuration = currentBeat - lastBeat;
00206         lastBeat = currentBeat;
00207 
00208         float rawBPM = 0;
00209         if(beatDuration > 0)
00210           rawBPM = 60000.0 / (float)beatDuration;
00211 //        if(debug == true) 
00212 //          Serial.println(rawBPM);
00213 
00214         //This method sometimes glitches, it's better to go through whole moving average everytime
00215         //IT's a neat idea to optimize the amount of work for moving avg. but while placing, removing finger it can screw up
00216         //valuesBPMSum -= valuesBPM[bpmIndex];
00217         //valuesBPM[bpmIndex] = rawBPM;
00218         //valuesBPMSum += valuesBPM[bpmIndex];
00219 
00220         valuesBPM[bpmIndex] = rawBPM;
00221         valuesBPMSum = 0;
00222         for(int i=0; i<PULSE_BPM_SAMPLE_SIZE; i++)
00223         {
00224           valuesBPMSum += valuesBPM[i];
00225         }
00226 
00227 /*        if(debug == true) 
00228         {
00229           Serial.print("CurrentMoving Avg: ");
00230           for(int i=0; i<PULSE_BPM_SAMPLE_SIZE; i++)
00231           {
00232             Serial.print(valuesBPM[i]);
00233             Serial.print(" ");
00234           }
00235   
00236           Serial.println(" ");
00237         }*/
00238 
00239         bpmIndex++;
00240         bpmIndex = bpmIndex % PULSE_BPM_SAMPLE_SIZE;
00241 
00242         if(valuesBPMCount < PULSE_BPM_SAMPLE_SIZE)
00243           valuesBPMCount++;
00244 
00245         currentBPM = valuesBPMSum / valuesBPMCount;
00246         //sprintf(show, "%f", currentBPM);
00247         //putBLE(show);
00248         //putBLE("\n");
00249 /*        if(debug == true) 
00250         {
00251           Serial.print("AVg. BPM: ");
00252           Serial.println(currentBPM);
00253         }*/
00254 
00255 
00256         currentPulseDetectorState = PULSE_TRACE_DOWN;
00257 
00258         return true;
00259       }
00260       break;
00261 
00262     case PULSE_TRACE_DOWN:
00263       if(sensor_value < prev_sensor_value)
00264       {
00265         values_went_down++;
00266       }
00267 
00268 
00269       if(sensor_value < PULSE_MIN_THRESHOLD)
00270       {
00271         currentPulseDetectorState = PULSE_IDLE;
00272       }
00273       break;
00274   }
00275 
00276   prev_sensor_value = sensor_value;
00277   return false;
00278 }
00279 
00280 /***************************************************************************/
00281 
00282 int main(void){
00283     millisStart();
00284     dcFilterIR.w = 0;
00285     dcFilterIR.result = 0;
00286     meanDiffIR.index = 0;
00287     meanDiffIR.sum = 0;
00288     meanDiffIR.count = 0;
00289     lpbFilterIR.v[0] = 0;
00290     lpbFilterIR.v[1] = 0;
00291     lpbFilterIR.result = 0;
00292     valuesBPM[0] = 0;
00293     valuesBPMSum = 0;
00294     valuesBPMCount = 0;
00295     bpmIndex = 0;
00296 
00297 /*    irACValueSqSum = 0;
00298     redACValueSqSum = 0;
00299     samplesRecorded = 0;
00300     pulsesDetected = 0;
00301     currentSaO2Value = 0;
00302 */
00303     lastBeatThreshold = 0;
00304 
00305     currentPulseDetectorState = PULSE_IDLE;  
00306   
00307     float meanDiffResIR;
00308     SMPRDY.fall(&sample);
00309     Flasher.attach(periodicCallback, 1);
00310     ini_i2c(SCL, SDA);
00311     sensor.begin(pw1600, i17, sr50 );
00312     iniBLE("Sigfried");
00313     while(1){
00314         sensor.readSensor();
00315         dcFilterIR = DCfilter((float)sensor.IR, dcFilterIR.w, 0.95);
00316         meanDiffResIR = meanDiff( dcFilterIR.result, &meanDiffIR);
00317         lowPassButterworthFilter( meanDiffResIR/*-dcFilterIR.result*/, &lpbFilterIR );
00318         detectPulse( lpbFilterIR.result );
00319         //store_Samples();
00320         sprintf(show, "%f", sensor.IR);
00321         putBLE(show);
00322         putBLE("\n");
00323         //wait(0.019);
00324         }
00325     }
00326     
00327