significantly improved beat detection using peak finding *and* zero c…

…rossing
espruino · Jan 24, 2022 · 407dd17 · 407dd17
1 parent c654955
commit 407dd17
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 11 deletions.
diff --git a/libs/banglejs/jswrap_bangle.c b/libs/banglejs/jswrap_bangle.c
@@ -3465,6 +3465,7 @@ bool jswrap_banglejs_idle() {
       if (o) {
         jsvObjectSetChildAndUnLock(o,"raw",jsvNewFromInteger(hrmInfo.raw));
         jsvObjectSetChildAndUnLock(o,"filt",jsvNewFromInteger(hrmInfo.filtered));
+        jsvObjectSetChildAndUnLock(o,"avg",jsvNewFromInteger(hrmInfo.avg));
         jsvObjectSetChildAndUnLock(o,"isBeat",jsvNewFromBool(hrmInfo.isBeat));
         jsvObjectSetChildAndUnLock(o,"bpm",jsvNewFromFloat(hrmInfo.bpm10 / 10.0));
         jsvObjectSetChildAndUnLock(o,"confidence",jsvNewFromInteger(hrmInfo.confidence));

diff --git a/libs/misc/heartrate.c b/libs/misc/heartrate.c
@@ -277,9 +277,7 @@ HrmInfo hrmInfo;
 /// Initialise heart rate monitoring
 void hrm_init() {
   memset(&hrmInfo, 0, sizeof(hrmInfo));
-  hrmInfo.filtered = 0;
-  hrmInfo.filtered1 = 0;
-  hrmInfo.filtered2 = 0;
+  hrmInfo.wasLow = false;
   hrmInfo.lastBeatTime = jshGetSystemTime();
   HRMFilter_init(&hrmFilter);
 }
@@ -357,16 +355,22 @@ bool hrm_new(int hrmValue) {
   if (h<=-32768) h=-32768;
   if (h>32767) h=32767;
   hrmInfo.filtered2 = hrmInfo.filtered1;
-  hrmInfo.filtered1 = hrmInfo.filtered;
+  hrmInfo.filtered1 = hrmInfo.filtered;  
   hrmInfo.filtered = h;
 
   // check for step counter
   bool hadBeat = false;
   hrmInfo.isBeat = false;
-  if ((hrmInfo.filtered1 >= hrmInfo.filtered) && (hrmInfo.filtered1 >= hrmInfo.filtered2)) {
+
+  if (h < hrmInfo.avg)
+    hrmInfo.wasLow = true;
+  else if (hrmInfo.wasLow && (hrmInfo.filtered1 >= hrmInfo.filtered) && (hrmInfo.filtered1 >= hrmInfo.filtered2)) {
+    hrmInfo.wasLow = false; // peak detected, and had previously gone below average
     hrmInfo.isBeat = true;
     hadBeat = hrm_had_beat();
   }
 
+  hrmInfo.avg = ((hrmInfo.avg*31) + h) >> 5;
+
   return hadBeat;
 }
diff --git a/libs/misc/heartrate.h b/libs/misc/heartrate.h
@@ -18,6 +18,7 @@
 #define HRM_HIST_LEN 16 // how many BPM values do we keep a history of
 #define HRM_MEDIAN_LEN 8 // how many BPM values do we average in our median filter to get a BPM reading
 
+// Do we use 8 or 16 bits for data storage?
 #ifdef HEARTRATE_DEVICE_VC31
 #define HrmValueType int16_t
 #define HRMVALUE_MIN -32768
@@ -33,7 +34,10 @@ typedef struct {
   int16_t filtered;
   int16_t filtered1; // before filtered
   int16_t filtered2; // before filtered1
-  bool isBeat;
+  int16_t avg; // average signal value, moving average
+
+  bool wasLow; // has the signal gone below the average? set =false when a beat detected
+  bool isBeat; // was this sample classified as a detected beat?
   JsSysTime lastBeatTime; // timestamp of last heartbeat
   uint8_t times[HRM_HIST_LEN]; // times of previous beats, in 1/100th secs
   uint8_t timeIdx; // index in times

diff --git a/libs/misc/hrm_vc31.c b/libs/misc/hrm_vc31.c
@@ -70,6 +70,7 @@ uint16_t hrmPollInterval = HRM_POLL_INTERVAL_DEFAULT; // in msec, so 20 = 50hz
 #define VC31B_REG19        0x19 // LED current, 0x30=10mA,0x50=40mA,0x5A=60mA,0xE0=80mA
 #define VC31B_REG20        0x1A // SLOT0 ENV sensitivity - 0x77 = PDResMax
 #define VC31B_REG21        0x1B // SLOT1 ENV sensitivity - 0x77 = PDResMax
+#define VC31B_REG22        0x1C // ? set to 0x67 for HRM mode
 
 // Interrupts
 // 0x10 = WearStatusDetection (ENV sensor IRQ?)
@@ -272,7 +273,11 @@ void vc31_new_ppg(uint16_t value) {
   else if (vcInfo.ppgOffset < -offsetAdjustment) vcInfo.ppgOffset += offsetAdjustment;
   else vcInfo.ppgOffset = 0;
 
-  hrmCallback(vcInfo.ppgValue + vcInfo.ppgOffset);
+  int v = vcInfo.ppgValue + vcInfo.ppgOffset;
+  if (vcType == VC31B_DEVICE)
+    v <<= 1; // on VC31B the PPG doesn't vary as much with pulse so try and bulk it up here a bit
+
+  hrmCallback(v);
 }
 
 
@@ -754,20 +759,31 @@ void hrm_sensor_on(HrmCallback callback) {
         0x40,      // VC31B_REG14 0x40 + FIFO Interrupt length in bottom 6 bits
         0x03,0x1F, // VC31B_REG15 (2 bytes) 16 bit counter prescaler
         0x00,      // VC31B_REG16 SLOT2 ENV sample rate - 6
-        0x00,0x80, // VC31B_REG17/18 SLOT0/1 LED current
+        0x00,      // VC31B_REG17 SLOT0 LED current
+        0x80,      // VC31B_REG18 SLOT1 LED current
         0x00,      // VC31B_REG19 - LED current
-        0x57,0x37, // VC31B_REG20/21 ENV sensitivity?
-        0x07,0x16,
+        0x57,0x37, // VC31B_REG20/21 SLOT 0/1 ENV sensitivity?
+        0x07,0x16, // 22,23
         0x56,0x16,0x00
     };
+    /* for SPO2
+
+      regConfig[0] = 0x47; // enable SLOT1
+      regConfig[1] = 0x2F; // different IRQs..
+      regConfig[6] = 0; // SLOT2 samplerate = every sample
+      regConfig[7] = 0x80; // SLOT1? LED current same as normal slot
+      regConfig[13] = 0x96; // was 0x16
+      regConfig[16] = 0x04; // was 4 - OverSample?
+
+     */
     memcpy(vcbInfo.regConfig, _regConfig, sizeof(_regConfig));
     //vcbInfo.regConfig[3]&0x3F==0 for FIFO disable, so FIFO is off now
 
     // vcbInfo.regConfig[3] |= vcbInfo.vcHr02SampleRate - 6; // Enable FIFO
     vcbInfo.regConfig[6] = vcbInfo.vcHr02SampleRate - 6; // VC31B_REG16 how often should ENV fire
 
     vcbInfo.regConfig[9] = 0xE0; //CUR = 80mA//write Hs equal to 1
-    vcbInfo.regConfig[12] = 0x67;
+    vcbInfo.regConfig[12] = 0x67; // VC31B_REG22
     vcbInfo.regConfig[0] = 0x45; // VC31B_REG11 heart rate calculation - SLOT2(env) and SLOT0(hr)
     // write all registers in one go
     vc31_wx(VC31B_REG11, vcbInfo.regConfig, 17);