apps: heart: Introduce simple app for the heart rate sensor

The heart rate analysis step is still a work in progress but the current
app allows us to visualize the the results of the signal conditioning.

Signed-off-by: Daniel Thompson <daniel@redfelineninja.org.uk>

4 files changed, 179 insertions, 1 deletions

diff --git a/wasp/apps/heart.py b/wasp/apps/heart.py
new file mode 100644
index 0000000..d88efd4
--- /dev/null
+++ b/wasp/apps/heart.py
@@ -0,0 +1,139 @@
+# SPDX-License-Identifier: LGPL-3.0-or-later
+# Copyright (C) 2020 Daniel Thompson
+
+import wasp
+import machine
+
+class Biquad():
+    """Direct Form II Biquad Filter"""
+
+    def __init__(self, b0, b1, b2, a1, a2):
+        self._coeff = (b0, b1, b2, a1, a2)
+        self._v1 = 0
+        self._v2 = 0
+
+    def step(self, x):
+        c = self._coeff
+        v1 = self._v1
+        v2 = self._v2
+
+        v = x - (c[3] * v1) - (c[4] * v2)
+        y = (c[0] * v) + (c[1] * v1) + (c[2] * v2)
+
+        self._v2 = v1
+        self._v1 = v
+        return y
+
+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
+    spikes but needs an extra 1k for sample storage which isn't
+    really plausible for a microcontroller.
+    """
+    def __init__(self, start, decay, threshold):
+        self._peak = start
+        self._decay = decay
+        self._boost = 1 / decay
+        self._threshold = threshold
+
+    def step(self, spl):        
+        # peak tracking
+        peak = self._peak
+        if abs(spl) > peak:
+            peak *= self._boost
+        else:
+            peak *= self._decay
+        self._peak = peak
+
+        # rejection filter (clipper)
+        threshold = self._threshold
+        if spl > (peak * threshold) or spl < (peak * -threshold):
+            return 0
+        
+        # booster
+        spl = 100 * spl / (2 * peak)
+        
+        return spl
+
+class HeartApp():
+    """Heart Rate Sensing application.
+
+    """
+    NAME = 'Heart'
+
+    def foreground(self):
+        """Activate the application."""
+        wasp.watch.hrs.enable()
+
+        # There is no delay after the enable because the redraw should
+        # take long enough it is not needed
+        draw = wasp.watch.drawable
+        draw.fill()
+        draw.string('PPG graph', 0, 6, width=240)
+
+        self._hpf = Biquad(0.87518309, -1.75036618,  0.87518309, -1.73472577,  0.7660066)
+        self._agc = PTAGC(20, 0.971, 2)
+        self._lpf = Biquad(0.10873253, 0.21746505, 0.10873253, -0.76462555,  0.19955565)
+
+        self._x = 0
+        self._offset = wasp.watch.hrs.read_hrs()
+
+        wasp.system.request_tick(1000 // 8)
+
+    def background(self):
+        wasp.watch.hrs.disable()
+        del self._hpf
+        del self._agc
+        del self._lpf
+
+    def _tick(self, ticks):
+        """Notify the application that its periodic tick is due."""
+        spl = wasp.watch.hrs.read_hrs()
+        spl -= self._offset
+        spl = self._hpf.step(spl)
+        spl = self._agc.step(spl)
+        spl = self._lpf.step(spl)
+
+        color = 0xffc0
+
+        # If the maths goes wrong lets show it in the chart!
+        if spl > 100 or spl < -100:
+            color = 0xffff
+        if spl > 104 or spl < -104:
+            spl = 0
+        spl = int(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
+        self._x = x
+
+    def tick(self, ticks):
+        """This is an outrageous hack but, at present, the RTC can only
+        wake us up every 125ms so we implement sub-ticks using a regular
+        timer to ensure we can read the sensor at 24Hz.
+        """
+        t = machine.Timer(id=1, period=8000000)
+        t.start()
+        self._tick(1)
+        wasp.system.keep_awake()
+
+        while t.time() < 41666:
+            pass
+        self._tick(1)
+
+        while t.time() < 83332:
+            pass
+        self._tick(1)
+
+        t.stop()
+        del t
diff --git a/wasp/boards/pinetime/manifest.py b/wasp/boards/pinetime/manifest.py
index 59061b6..02b5649 100644
--- a/wasp/boards/pinetime/manifest.py
+++ b/wasp/boards/pinetime/manifest.py
@@ -6,6 +6,7 @@ freeze('../..',
     (
         'apps/clock.py',
         'apps/flashlight.py',
+        'apps/heart.py',
         'apps/launcher.py',
         'apps/pager.py',
         'apps/settings.py',
diff --git a/wasp/boards/simulator/watch.py b/wasp/boards/simulator/watch.py
index 48d7122..9f9920c 100644
--- a/wasp/boards/simulator/watch.py
+++ b/wasp/boards/simulator/watch.py
@@ -124,6 +124,41 @@ class RTC(object):
     def get_uptime_ms(self):
         return int(self.uptime * 1000)
 
+class HRS():
+    DATA = (
+9084,9084,9025,9025,9009,9009,9009,9015,9015,9024,9024,9024,9073,9073,9074,9074,
+9074,9100,9100,9097,9097,9097,9045,9045,9023,9023,9023,9035,9035,9039,9039,9039,
+9049,9049,9052,9052,9052,9066,9066,9070,9070,9070,9078,9078,9081,9081,9081,9092,
+9092,9093,9093,9093,9094,9094,9108,9108,9108,9124,9124,9122,9122,9122,9100,9100,
+9110,9110,9110,9112,9112,9118,9118,9118,9127,9127,9136,9136,9136,9147,9147,9154,
+9154,9154,9156,9156,9153,9153,9153,9152,9152,9156,9156,9156,9161,9161,9161,9177,
+9177,9186,9186,9196,9196,9196,9201,9201,9201,9189,9189,9176,9176,9176,9176,9176,
+9175,9175,9175,9175,9175,9180,9180,9180,9189,9189,9202,9202,9202,9207,9207,9181,
+9181,9181,9167,9167,9169,9169,9169,9163,9163,9164,9164,9164,9165,9165,9172,9172,
+9172,9180,9180,9192,9192,9192,9178,9178,9161,9161,9161,9163,9163,9173,9173,9173,
+9170,9170,9179,9179,9183,9183,9183,9196,9196,9207,9207,9207,9208,9208,9186,9186,
+9186,9182,9182,9193,9193,9193,9197,9197,9188,9204,9204,9212,9212,9212,9223,9223,
+9228,9228,9228,9235,9235,9215,9215,9215,9217,9217,9225,9225,9225,9230,9230,9237,
+9237,9237,9246,9246,9260,9260,9260,9270,9270,9269,9269,9269,9256,9256,9256,9256,
+9256,9263,9263,9274,9274,9274,9288,9288,9292,9292,9292,9307,9307,9310,9310,9310,
+9292,9292,9291,9291,9291,9297,9297,9298,9298,9298
+)
+    def __init__(self):
+        self._i = 0
+
+    def enable(self):
+        pass
+
+    def disable(self):
+        pass
+
+    def read_hrs(self):
+        d = self.DATA[self._i]
+        self._i += 1
+        if self._i >= len(self.DATA):
+            self._i = 0
+        return d
+
 backlight = Backlight()
 spi = SPI(0)
 spi.init(polarity=1, phase=1, baudrate=8000000)
@@ -136,6 +171,7 @@ drawable = draw565.Draw565(display)
 accel = Accelerometer()
 battery = Battery()
 button = Pin('BUTTON', Pin.IN, quiet=True)
+hrs = HRS()
 rtc = RTC()
 touch = CST816S(I2C(0), Pin('TP_INT', Pin.IN, quiet=True), Pin('TP_RST', Pin.OUT, quiet=True))
 vibrator = Vibrator(Pin('MOTOR', Pin.OUT, value=0), active_low=True)
diff --git a/wasp/wasp.py b/wasp/wasp.py
index c4a565c..092ad29 100644
--- a/wasp/wasp.py
+++ b/wasp/wasp.py
@@ -22,6 +22,7 @@ import widgets
 
 from apps.clock import ClockApp
 from apps.flashlight import FlashlightApp
+from apps.heart import HeartApp
 from apps.launcher import LauncherApp
 from apps.pager import PagerApp, CrashApp
 from apps.settings import SettingsApp
@@ -108,8 +109,9 @@ class Manager():
 
         # TODO: Eventually these should move to main.py
         self.register(ClockApp(), True)
-        self.register(StopwatchApp(), True)
         self.register(StepCounterApp(), True)
+        self.register(StopwatchApp(), True)
+        self.register(HeartApp(), True)
         self.register(FlashlightApp(), False)
         self.register(SettingsApp(), False)
         self.register(TestApp(), False)