From e6811bb69389bb03de49ebe923672dc1315afb6d Mon Sep 17 00:00:00 2001
From: Daniel Thompson <daniel@redfelineninja.org.uk>
Date: Wed, 24 Jun 2020 21:20:18 +0100
Subject: [PATCH] apps: heart: Initial heart rate detection

The algorithm is fairly crude and the GUI is pretty simple but, if you stay
still for 10 seconds, there's a good chance of an accurate pulse
reading.

Of course if you jog on the spot for ten seconds it more likely to
calculate how many steps per minutes you are performing!

Signed-off-by: Daniel Thompson <daniel@redfelineninja.org.uk>
---
 wasp/apps/heart.py | 87 ++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 73 insertions(+), 14 deletions(-)

diff --git a/wasp/apps/heart.py b/wasp/apps/heart.py
index d88efd4..fdf8336 100644
--- a/wasp/apps/heart.py
+++ b/wasp/apps/heart.py
@@ -2,6 +2,7 @@
 # Copyright (C) 2020 Daniel Thompson
 
 import wasp
+import array
 import machine
 
 class Biquad():
@@ -26,7 +27,7 @@ class Biquad():
 
 class PTAGC():
     """Peak Tracking Automatic Gain Control
-    
+
     In order for the correlation checks to work correctly we must
     aggressively reject spikes caused by fast DC steps. Setting a
     threshold based on the median is very effective at killing
@@ -39,7 +40,7 @@ class PTAGC():
         self._boost = 1 / decay
         self._threshold = threshold
 
-    def step(self, spl):        
+    def step(self, spl):
         # peak tracking
         peak = self._peak
         if abs(spl) > peak:
@@ -52,12 +53,62 @@ class PTAGC():
         threshold = self._threshold
         if spl > (peak * threshold) or spl < (peak * -threshold):
             return 0
-        
+
         # booster
         spl = 100 * spl / (2 * peak)
-        
+
         return spl
 
+def _compare(d1, d2, count, shift):
+    e = 0
+    for i in range(count):
+        d = d1[i] - d2[i]
+        e += d*d
+    return e
+
+def compare(d, shift):
+    return _compare(d[shift:], d[:-shift], len(d)-shift, shift)
+
+def trough(d, mn, mx):
+    z2 = compare(d, mn-2)
+    z1 = compare(d, mn-1)
+    for i in range(mn, mx+1):
+        z = compare(d, i)
+        if z2 > z1 and z1 < z:
+            return i
+        z2 = z1
+        z1 = z
+
+    return -1
+
+def get_hrs(d):
+    din = memoryview(d)
+
+    # Search initially from ~210 to 30 bpm
+    t0 = trough(din, 7, 48)
+    if t0 < 0:
+        return None
+
+    # Check the second cycle ...
+    t1 = t0 * 2
+    t1 = trough(din, t1 - 5, t1 + 5)
+    if t1 < 0:
+        return None
+
+    # ... and the third
+    t2 = (t1 * 3) // 2
+    t2 = trough(din, t2 - 5, t2 + 4)
+    if t2 < 0:
+        return None
+
+    # If we can find a fourth cycle then use that for the extra
+    # precision otherwise report whatever we've found
+    t3 = (t2 * 4) // 3
+    t3 = trough(din, t3 - 4, t3 + 4)
+    if t3 < 0:
+        return (60 * 24 * 3) // t2
+    return (60 * 24 * 4) // t3
+
 class HeartApp():
     """Heart Rate Sensing application.
 
@@ -74,13 +125,15 @@ class HeartApp():
         draw.fill()
         draw.string('PPG graph', 0, 6, width=240)
 
-        self._hpf = Biquad(0.87518309, -1.75036618,  0.87518309, -1.73472577,  0.7660066)
+        self._hpf = Biquad(0.87033078, -1.74066156, 0.87033078, -1.72377617, 0.75754694)
         self._agc = PTAGC(20, 0.971, 2)
-        self._lpf = Biquad(0.10873253, 0.21746505, 0.10873253, -0.76462555,  0.19955565)
+        self._lpf = Biquad(0.11595249, 0.23190498, 0.11595249, -0.72168143, 0.18549138)
 
         self._x = 0
         self._offset = wasp.watch.hrs.read_hrs()
 
+        self._hrdata = array.array('b')
+
         wasp.system.request_tick(1000 // 8)
 
     def background(self):
@@ -89,13 +142,22 @@ class HeartApp():
         del self._agc
         del self._lpf
 
-    def _tick(self, ticks):
+    def _subtick(self, ticks):
         """Notify the application that its periodic tick is due."""
+        draw = wasp.watch.drawable
+
         spl = wasp.watch.hrs.read_hrs()
         spl -= self._offset
         spl = self._hpf.step(spl)
         spl = self._agc.step(spl)
         spl = self._lpf.step(spl)
+        spl = int(spl)
+
+        self._hrdata.append(spl)
+        if len(self._hrdata) >= 240:
+            draw.string('{} bpm'.format(get_hrs(self._hrdata)), 0, 6, width=240)
+            del self._hrdata
+            self._hrdata = array.array('b')
 
         color = 0xffc0
 
@@ -104,14 +166,11 @@ class HeartApp():
             color = 0xffff
         if spl > 104 or spl < -104:
             spl = 0
-        spl = int(spl) + 104
+        spl += 104
 
         x = self._x
-
-        draw = wasp.watch.drawable
         draw.fill(0, x, 32, 1, 208-spl)
         draw.fill(color, x, 239-spl, 1, spl)
-
         x += 2
         if x >= 240:
             x = 0
@@ -124,16 +183,16 @@ class HeartApp():
         """
         t = machine.Timer(id=1, period=8000000)
         t.start()
-        self._tick(1)
+        self._subtick(1)
         wasp.system.keep_awake()
 
         while t.time() < 41666:
             pass
-        self._tick(1)
+        self._subtick(1)
 
         while t.time() < 83332:
             pass
-        self._tick(1)
+        self._subtick(1)
 
         t.stop()
         del t