Diff: PulseSensor.cpp

Revision:

11:d32f4f43161d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PulseSensor.cpp	Sun Oct 16 14:25:59 2016 +0000
@@ -0,0 +1,149 @@
+#include "mbed.h"
+#include <limits.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif // __cplusplus
+
+#define NEED_DEBUG 
+#ifdef NEED_DEBUG
+#define DEBUG(...) { printf(__VA_ARGS__); }
+#else
+#define DEBUG(...) /* nothing */
+#endif 
+
+#define N 10
+#define DEFAULT_THRESHOLD 500
+#define DEFAULT_AMP 100
+
+
+volatile int BPM = 60;
+volatile int IBI = 600;
+
+volatile bool Pulse = false;
+volatile bool QS = false;
+
+volatile int Rate[N];
+volatile int CurrentBeatTime = 0;
+volatile int LastBeatTime = 0;
+
+volatile uint16_t Signal;
+volatile uint16_t P = DEFAULT_THRESHOLD;
+volatile uint16_t T = DEFAULT_THRESHOLD;
+volatile uint16_t Threshold = DEFAULT_THRESHOLD;
+
+volatile int Amplifier = DEFAULT_AMP;
+
+Timer timer;
+AnalogIn ain(p5);
+
+void initPulseSensor(void) {
+    for (int i = 0; i < N; ++i) {
+        Rate[i] = 0;
+    }
+    timer.start();
+    LastBeatTime = timer.read_ms();
+}
+
+int getBPM(void) {
+    return BPM;
+}
+
+bool isQS(void) {
+    bool qs = QS;
+    QS = false;
+    return qs;
+}
+
+
+void reset() {
+    Threshold = DEFAULT_THRESHOLD;
+    Amplifier = DEFAULT_AMP;
+    P = T = DEFAULT_THRESHOLD;
+}
+
+void calcHeartRate(void) {
+    
+    Signal = ain.read_u16();
+    CurrentBeatTime = timer.read_ms();
+    // DEBUG("%d\t%d\r\n", CurrentBeatTime, Signal);
+    
+    int interval = 0;
+    if (CurrentBeatTime < LastBeatTime) {
+        interval = INT_MAX - CurrentBeatTime + LastBeatTime;
+    } else {
+        interval = CurrentBeatTime - LastBeatTime;
+    }
+    
+    if ((Signal < Threshold) && (interval > IBI * 3/5)) {
+        if (Signal < T) {   // hold bottom 
+            T = Signal;
+            // DEBUG("T:%d\r\n", T);
+        }
+    } else if ((Signal > Threshold) && (Signal > P)) {
+        P = Signal; // hold peak 
+        // DEBUG("P:%d\r\n", P);
+    }
+    
+    if (interval > 250 && interval < 2500) { // msec
+    
+        if ((Signal > Threshold) && !Pulse && (interval > IBI * 3/5)) {
+            Pulse = true;
+            IBI = interval;
+            
+            if (Rate[0] < 0) {  // first time
+                Rate[0] = 0;
+                LastBeatTime = timer.read_ms();
+                return;
+            } else if (Rate[0] == 0) { // second time
+                for (int i = 0; i < N; ++i) {
+                    Rate[i] = IBI;
+                }
+            }
+            
+            int running_total = 0;
+            for (int i = 0; i < N-1; ++i) {
+                Rate[i] = Rate[i+1];
+                running_total += Rate[i];
+            }
+            
+            Rate[N-1] = IBI;
+            running_total += IBI;
+            running_total /= N;
+            BPM = 60000 / running_total;
+            QS = true;
+            LastBeatTime = timer.read_ms();
+            // DEBUG("P:%d T:%d AMP:%d THR:%d BPM:%d\r\n"
+            //        ,P ,T ,Amplifier ,Threshold ,BPM);
+        }
+    }
+    
+    // check if Signal is under Threshold
+    // if (Pulse) {
+    if ((Signal < Threshold) && Pulse) {
+        Pulse = false;
+        if (P >= T) {
+            Amplifier = P - T;
+            Threshold = Amplifier/2 + T;  // update Threshold
+            P = T = Threshold;
+            // DEBUG("Update Threshold:%d\r\n", Threshold);
+        } else {
+            // DEBUG("Error: T(%d) over P(%d)\r\n", T, P);
+            reset();
+        }
+    }
+    
+    // check if no Signal is over 2.5 sec
+    if (interval >= 2500) {
+        DEBUG("No Signal over 2.5sec\r\n");
+        reset();
+        LastBeatTime = timer.read_ms();
+        for (int i = 0; i < N; ++i) {
+            Rate[i] = -1;
+        }
+    }
+}
+
+#ifdef __cplusplus
+}
+#endif
\ No newline at end of file