diff options
| -rw-r--r-- | wasp/apps/heart.py | 87 |
1 files 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 |