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

@@ -2,6 +2,7 @@
 # Copyright (C) 2020 Daniel Thompson
 
 import wasp
+import array
 import machine
 
 class Biquad():
@@ -58,6 +59,56 @@ class PTAGC():
 
         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