Interrupt
Dependencies: BLE_API BLE_Driver I2C_Driver MAX30100 PROCESAMIENTO_DATOS_SP02 mbed nRF51822 millis
Fork of MAX30100_FirstTry by
Diff: mainbletest.cpp
- Revision:
- 5:c2ce7b743efa
- Parent:
- 3:98ca4bf2e74a
--- 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); } }