Merge pull request #14 from lesunb/feat/covid_detection

Inclusao de arquivos uteis e correcoes na entrada de dados

arduino_code_for_real_sensors/ecg_spo2/ecg_spo2.ino

@@ -0,0 +1,215 @@
+#define USE_USBCON
+#include <ros.h>
+#include <MAX3010x.h>
+#include "filters.h"
+#include <std_srvs/SetBool.h>
+
+// Sensor (adjust to your sensor type)
+MAX30105 sensor;
+const auto kSamplingRate = sensor.SAMPLING_RATE_400SPS;
+const float kSamplingFrequency = 400.0;
+
+// Finger Detection Threshold and Cooldown
+const unsigned long kFingerThreshold = 10000;
+const unsigned int kFingerCooldownMs = 500;
+
+// Edge Detection Threshold (decrease for MAX30100)
+const float kEdgeThreshold = -2000.0;
+
+// Filters
+const float kLowPassCutoff = 5.0;
+const float kHighPassCutoff = 0.5;
+
+// Averaging
+const bool kEnableAveraging = false;
+const int kAveragingSamples = 5;
+const int kSampleThreshold = 5;
+
+// ROS
+ros::NodeHandle  nh;
+
+float spo2;
+int bpm;
+
+void bpm_callback(const std_srvs::SetBool::Request & req, std_srvs::SetBool::Response & res){
+  if(req.data == true) {
+    String str = String(bpm);
+    char msg[str.length() + 1];
+    str.toCharArray(msg, str.length() + 1);
+    res.success = true;
+    res.message = msg;
+  }
+  else {
+    res.success = false;
+    res.message = "not ok";
+  }
+}
+
+void spo2_callback(const std_srvs::SetBool::Request & req, std_srvs::SetBool::Response & res){
+  if(req.data == true) {
+    String str = String(spo2, 2);
+    char msg[str.length() + 1];
+    str.toCharArray(msg, str.length() + 1);
+    res.success = true;
+    res.message = msg;
+  }
+  else {
+    res.success = false;
+    res.message = "not ok";
+  }
+}
+
+ros::ServiceServer<std_srvs::SetBool::Request,std_srvs::SetBool::Response> bpm_server("bpm",&bpm_callback);
+ros::ServiceServer<std_srvs::SetBool::Request,std_srvs::SetBool::Response> spo2_server("spo2",&spo2_callback);
+
+void setup() {
+  nh.initNode();
+  nh.advertiseService(bpm_server);
+  nh.advertiseService(spo2_server);
+
+  if(sensor.begin() && sensor.setSamplingRate(kSamplingRate)) { 
+  }
+  else {
+    while(1);
+  }
+}
+
+// Filter Instances
+LowPassFilter low_pass_filter_red(kLowPassCutoff, kSamplingFrequency);
+LowPassFilter low_pass_filter_ir(kLowPassCutoff, kSamplingFrequency);
+HighPassFilter high_pass_filter(kHighPassCutoff, kSamplingFrequency);
+Differentiator differentiator(kSamplingFrequency);
+MovingAverageFilter<kAveragingSamples> averager_bpm;
+MovingAverageFilter<kAveragingSamples> averager_r;
+MovingAverageFilter<kAveragingSamples> averager_spo2;
+
+// Statistic for pulse oximetry
+MinMaxAvgStatistic stat_red;
+MinMaxAvgStatistic stat_ir;
+
+// R value to SpO2 calibration factors
+// See https://www.maximintegrated.com/en/design/technical-documents/app-notes/6/6845.html
+float kSpO2_A = 1.5958422*1.25;
+float kSpO2_B = -34.6596622;
+float kSpO2_C = 112.6898759*1.1;
+
+// Timestamp of the last heartbeat
+long last_heartbeat = 0;
+
+// Timestamp for finger detection
+long finger_timestamp = 0;
+bool finger_detected = false;
+
+// Last diff to detect zero crossing
+float last_diff = NAN;
+bool crossed = false;
+long crossed_time = 0;
+
+void loop() {
+  auto sample = sensor.readSample(1000);
+  float current_value_red = sample.red;
+  float current_value_ir = sample.ir;
+
+  // Detect Finger using raw sensor value
+  if(sample.red > kFingerThreshold) {
+    if(millis() - finger_timestamp > kFingerCooldownMs) {
+      finger_detected = true;
+    }
+  }
+  else {
+    // Reset values if the finger is removed
+    differentiator.reset();
+    averager_bpm.reset();
+    averager_r.reset();
+    averager_spo2.reset();
+    low_pass_filter_red.reset();
+    low_pass_filter_ir.reset();
+    high_pass_filter.reset();
+    stat_red.reset();
+    stat_ir.reset();
+
+    finger_detected = false;
+    finger_timestamp = millis();
+  }
+
+  if(finger_detected) {
+    current_value_red = low_pass_filter_red.process(current_value_red);
+    current_value_ir = low_pass_filter_ir.process(current_value_ir);
+
+    // Statistics for pulse oximetry
+    stat_red.process(current_value_red);
+    stat_ir.process(current_value_ir);
+
+    // Heart beat detection using value for red LED
+    float current_value = high_pass_filter.process(current_value_red);
+    float current_diff = differentiator.process(current_value);
+
+    // Valid values?
+    if(!isnan(current_diff) && !isnan(last_diff)) {
+
+      // Detect Heartbeat - Zero-Crossing
+      if(last_diff > 0 && current_diff < 0) {
+        crossed = true;
+        crossed_time = millis();
+      }
+
+      if(current_diff > 0) {
+        crossed = false;
+      }
+
+      // Detect Heartbeat - Falling Edge Threshold
+      if(crossed && current_diff < kEdgeThreshold) {
+        if(last_heartbeat != 0 && crossed_time - last_heartbeat > 300) {
+          // Show Results
+          bpm = 60000/(crossed_time - last_heartbeat);
+          float rred = (stat_red.maximum()-stat_red.minimum())/stat_red.average();
+          float rir = (stat_ir.maximum()-stat_ir.minimum())/stat_ir.average();
+          float r = rred/rir;
+          spo2 = kSpO2_A * r * r + kSpO2_B * r + kSpO2_C;
+
+
+          if(bpm > 50 && bpm < 250) {
+            // Average?
+            if(kEnableAveraging) {
+              int average_bpm = averager_bpm.process(bpm);
+              int average_r = averager_r.process(r);
+              int average_spo2 = averager_spo2.process(spo2);
+
+              // Show if enough samples have been collected
+              if(averager_bpm.count() >= kSampleThreshold) {
+                //Serial.print("Time (ms): ");
+                //Serial.println(millis()); 
+                //Serial.print("Heart Rate (avg, bpm): ");
+                //Serial.println(average_bpm);
+                //Serial.print("R-Value (avg): ");
+                //Serial.println(average_r);  
+                //Serial.print("SpO2 (avg, %): ");
+                //Serial.println(average_spo2);  
+              }
+            }
+            else {
+              //Serial.print("Time (ms): ");
+              //Serial.println(millis()); 
+              //Serial.print("Heart Rate (current, bpm): ");
+              //Serial.println(bpm);  
+              //Serial.print("R-Value (current): ");
+              //Serial.println(r);
+              //Serial.print("SpO2 (current, %): ");
+              //Serial.println(spo2);   
+            }
+          }
+
+          // Reset statistic
+          stat_red.reset();
+          stat_ir.reset();
+        }
+
+        crossed = false;
+        last_heartbeat = crossed_time;
+      }
+    }
+
+    last_diff = current_diff;
+  }
+  nh.spinOnce();
+}