Refactored Ppg for frequency based algorithm. (#1486)

New implementation of the heart rate sensor data processing using a frequency based PPG algorithm.
The HRS3300 settings are fine-tuned for better signal to noise at 10Hz.
The measurement delay is now set to 100ms.
Enable and use the ambient light sensor.
FFT implementation based on ArduinoFFT (https://github.com/kosme/arduinoFFT, GPLv3.0).

6 files changed, 317 insertions, 178 deletions

diff --git a/src/components/heartrate/Biquad.cpp b/src/components/heartrate/Biquad.cpp
deleted file mode 100644
index b7edd403..00000000
--- a/src/components/heartrate/Biquad.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
-  SPDX-License-Identifier: LGPL-3.0-or-later
-  Original work Copyright (C) 2020 Daniel Thompson
-  C++ port Copyright (C) 2021 Jean-François Milants
-*/
-
-#include "components/heartrate/Biquad.h"
-
-using namespace Pinetime::Controllers;
-
-/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */
-Biquad::Biquad(float b0, float b1, float b2, float a1, float a2) : b0 {b0}, b1 {b1}, b2 {b2}, a1 {a1}, a2 {a2} {
-}
-
-float Biquad::Step(float x) {
-  auto v1 = this->v1;
-  auto v2 = this->v2;
-
-  auto v = x - (a1 * v1) - (a2 * v2);
-  auto y = (b0 * v) + (b1 * v1) + (b2 * v2);
-
-  this->v2 = v1;
-  this->v1 = v;
-
-  return y;
-}
diff --git a/src/components/heartrate/Biquad.h b/src/components/heartrate/Biquad.h
deleted file mode 100644
index 7c8ca58f..00000000
--- a/src/components/heartrate/Biquad.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#pragma once
-
-namespace Pinetime {
-  namespace Controllers {
-    /// Direct Form II Biquad Filter
-    class Biquad {
-    public:
-      Biquad(float b0, float b1, float b2, float a1, float a2);
-      float Step(float x);
-
-    private:
-      float b0;
-      float b1;
-      float b2;
-      float a1;
-      float a2;
-
-      float v1 = 0.0f;
-      float v2 = 0.0f;
-    };
-  }
-}
diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp
index 900b1c22..3a6988ae 100644
--- a/src/components/heartrate/Ppg.cpp
+++ b/src/components/heartrate/Ppg.cpp
@@ -1,107 +1,292 @@
-/*
-  SPDX-License-Identifier: LGPL-3.0-or-later
-  Original work Copyright (C) 2020 Daniel Thompson
-  C++ port Copyright (C) 2021 Jean-François Milants
-*/
-
 #include "components/heartrate/Ppg.h"
-#include <vector>
 #include <nrf_log.h>
+#include <vector>
+
 using namespace Pinetime::Controllers;
 
-/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */
 namespace {
-  int Compare(int8_t* d1, int8_t* d2, size_t count) {
-    int e = 0;
-    for (size_t i = 0; i < count; i++) {
-      auto d = d1[i] - d2[i];
-      e += d * d;
+  float LinearInterpolation(const float* xValues, const float* yValues, int length, float pointX) {
+    if (pointX > xValues[length - 1]) {
+      return yValues[length - 1];
+    } else if (pointX <= xValues[0]) {
+      return yValues[0];
+    }
+    int index = 0;
+    while (pointX > xValues[index] && index < length - 1) {
+      index++;
+    }
+    float pointX0 = xValues[index - 1];
+    float pointX1 = xValues[index];
+    float pointY0 = yValues[index - 1];
+    float pointY1 = yValues[index];
+    float mu = (pointX - pointX0) / (pointX1 - pointX0);
+
+    return (pointY0 * (1 - mu) + pointY1 * mu);
+  }
+
+  float PeakSearch(float* xVals, float* yVals, float threshold, float& width, float start, float end, int length) {
+    int peaks = 0;
+    bool enabled = false;
+    float minBin = 0.0f;
+    float maxBin = 0.0f;
+    float peakCenter = 0.0f;
+    float prevValue = LinearInterpolation(xVals, yVals, length, start - 0.01f);
+    float currValue = LinearInterpolation(xVals, yVals, length, start);
+    float idx = start;
+    while (idx < end) {
+      float nextValue = LinearInterpolation(xVals, yVals, length, idx + 0.01f);
+      if (currValue < threshold) {
+        enabled = true;
+      }
+      if (currValue >= threshold and enabled) {
+        if (prevValue < threshold) {
+          minBin = idx;
+        } else if (nextValue <= threshold) {
+          maxBin = idx;
+          peaks++;
+          width = maxBin - minBin;
+          peakCenter = width / 2.0f + minBin;
+        }
+      }
+      prevValue = currValue;
+      currValue = nextValue;
+      idx += 0.01f;
     }
-    return e;
+    if (peaks != 1) {
+      width = 0.0f;
+      peakCenter = 0.0f;
+    }
+    return peakCenter;
   }
 
-  int CompareShift(int8_t* d, int shift, size_t count) {
-    return Compare(d + shift, d, count - shift);
+  float SpectrumMean(const std::array<float, Ppg::spectrumLength>& signal, int start, int end) {
+    int total = 0;
+    float mean = 0.0f;
+    for (int idx = start; idx < end; idx++) {
+      mean += signal.at(idx);
+      total++;
+    }
+    if (total > 0) {
+      mean /= static_cast<float>(total);
+    }
+    return mean;
   }
 
-  int Trough(int8_t* d, size_t size, uint8_t mn, uint8_t mx) {
-    auto z2 = CompareShift(d, mn - 2, size);
-    auto z1 = CompareShift(d, mn - 1, size);
-    for (int i = mn; i < mx + 1; i++) {
-      auto z = CompareShift(d, i, size);
-      if (z2 > z1 && z1 < z) {
-        return i;
+  float SignalToNoise(const std::array<float, Ppg::spectrumLength>& signal, int start, int end, float max) {
+    float mean = SpectrumMean(signal, start, end);
+    return max / mean;
+  }
+
+  // Simple bandpass filter using exponential moving average
+  void Filter30to240(std::array<float, Ppg::dataLength>& signal) {
+    // From:
+    // https://www.norwegiancreations.com/2016/03/arduino-tutorial-simple-high-pass-band-pass-and-band-stop-filtering/
+
+    int length = signal.size();
+    // 0.268 is ~0.5Hz and 0.816 is ~4Hz cutoff at 10Hz sampling
+    float expAlpha = 0.816f;
+    float expAvg = 0.0f;
+    for (int loop = 0; loop < 4; loop++) {
+      expAvg = signal.front();
+      for (int idx = 0; idx < length; idx++) {
+        expAvg = (expAlpha * signal.at(idx)) + ((1 - expAlpha) * expAvg);
+        signal[idx] = expAvg;
+      }
+    }
+    expAlpha = 0.268f;
+    for (int loop = 0; loop < 4; loop++) {
+      expAvg = signal.front();
+      for (int idx = 0; idx < length; idx++) {
+        expAvg = (expAlpha * signal.at(idx)) + ((1 - expAlpha) * expAvg);
+        signal[idx] -= expAvg;
       }
-      z2 = z1;
-      z1 = z;
     }
-    return -1;
   }
-}
 
-Ppg::Ppg()
-  : hpf {0.87033078, -1.74066156, 0.87033078, -1.72377617, 0.75754694},
-    agc {20, 0.971, 2},
-    lpf {0.11595249, 0.23190498, 0.11595249, -0.72168143, 0.18549138} {
-}
+  float SpectrumMax(const std::array<float, Ppg::spectrumLength>& data, int start, int end) {
+    float max = 0.0f;
+    for (int idx = start; idx < end; idx++) {
+      if (data.at(idx) > max) {
+        max = data.at(idx);
+      }
+    }
+    return max;
+  }
 
-int8_t Ppg::Preprocess(float spl) {
-  spl -= offset;
-  spl = hpf.Step(spl);
-  spl = agc.Step(spl);
-  spl = lpf.Step(spl);
+  void Detrend(std::array<float, Ppg::dataLength>& signal) {
+    int size = signal.size();
+    float offset = signal.front();
+    float slope = (signal.at(size - 1) - offset) / static_cast<float>(size - 1);
 
-  auto spl_int = static_cast<int8_t>(spl);
+    for (int idx = 0; idx < size; idx++) {
+      signal[idx] -= (slope * static_cast<float>(idx) + offset);
+    }
+    for (int idx = 0; idx < size - 1; idx++) {
+      signal[idx] = signal[idx + 1] - signal[idx];
+    }
+  }
 
-  if (dataIndex < 200) {
-    data[dataIndex++] = spl_int;
+  // Hanning Coefficients from numpy: python -c 'import numpy;print(numpy.hanning(64))'
+  // Note: Harcoded and must be updated if constexpr dataLength is changed. Prevents the need to
+  // use cosf() which results in an extra ~5KB in storage.
+  // This data is symetrical so just using the first half (saves 128B when dataLength is 64).
+  static constexpr float hanning[Ppg::dataLength >> 1] {
+    0.0f,        0.00248461f, 0.00991376f, 0.0222136f,  0.03926189f, 0.06088921f, 0.08688061f, 0.11697778f,
+    0.15088159f, 0.1882551f,  0.22872687f, 0.27189467f, 0.31732949f, 0.36457977f, 0.41317591f, 0.46263495f,
+    0.51246535f, 0.56217185f, 0.61126047f, 0.65924333f, 0.70564355f, 0.75f,       0.79187184f, 0.83084292f,
+    0.86652594f, 0.89856625f, 0.92664544f, 0.95048443f, 0.96984631f, 0.98453864f, 0.99441541f, 0.99937846f};
+}
+
+Ppg::Ppg() {
+  dataAverage.fill(0.0f);
+  spectrum.fill(0.0f);
+}
+
+int8_t Ppg::Preprocess(uint32_t hrs, uint32_t als) {
+  if (dataIndex < dataLength) {
+    dataHRS[dataIndex++] = hrs;
   }
-  return spl_int;
+  alsValue = als;
+  if (alsValue > alsThreshold) {
+    return 1;
+  }
+  return 0;
 }
 
 int Ppg::HeartRate() {
-  if (dataIndex < 200) {
+  if (dataIndex < dataLength) {
     return 0;
   }
-
-  NRF_LOG_INFO("PREPROCESS, offset = %d", offset);
-  auto hr = ProcessHeartRate();
-  dataIndex = 0;
+  int hr = 0;
+  hr = ProcessHeartRate(resetSpectralAvg);
+  resetSpectralAvg = false;
+  // Make room for overlapWindow number of new samples
+  for (int idx = 0; idx < dataLength - overlapWindow; idx++) {
+    dataHRS[idx] = dataHRS[idx + overlapWindow];
+  }
+  dataIndex = dataLength - overlapWindow;
   return hr;
 }
 
-int Ppg::ProcessHeartRate() {
-  int t0 = Trough(data.data(), dataIndex, 7, 48);
-  if (t0 < 0) {
-    return 0;
+void Ppg::Reset(bool resetDaqBuffer) {
+  if (resetDaqBuffer) {
+    dataIndex = 0;
   }
+  avgIndex = 0;
+  dataAverage.fill(0.0f);
+  lastPeakLocation = 0.0f;
+  alsThreshold = UINT16_MAX;
+  alsValue = 0;
+  resetSpectralAvg = true;
+  spectrum.fill(0.0f);
+}
 
-  int t1 = t0 * 2;
-  t1 = Trough(data.data(), dataIndex, t1 - 5, t1 + 5);
-  if (t1 < 0) {
-    return 0;
+// Pass init == true to reset spectral averaging.
+// Returns -1 (Reset Acquisition), 0 (Unable to obtain HR) or HR (BPM).
+int Ppg::ProcessHeartRate(bool init) {
+  std::copy(dataHRS.begin(), dataHRS.end(), vReal.begin());
+  Detrend(vReal);
+  Filter30to240(vReal);
+  vImag.fill(0.0f);
+  // Apply Hanning Window
+  int hannIdx = 0;
+  for (int idx = 0; idx < dataLength; idx++) {
+    if (idx >= dataLength >> 1) {
+      hannIdx--;
+    }
+    vReal[idx] *= hanning[hannIdx];
+    if (idx < dataLength >> 1) {
+      hannIdx++;
+    }
   }
-
-  int t2 = (t1 * 3) / 2;
-  t2 = Trough(data.data(), dataIndex, t2 - 5, t2 + 5);
-  if (t2 < 0) {
-    return 0;
+  // Compute in place power spectrum
+  ArduinoFFT<float> FFT = ArduinoFFT<float>(vReal.data(), vImag.data(), dataLength, sampleFreq);
+  FFT.compute(FFTDirection::Forward);
+  FFT.complexToMagnitude();
+  FFT.~ArduinoFFT();
+  SpectrumAverage(vReal.data(), spectrum.data(), spectrum.size(), init);
+  peakLocation = 0.0f;
+  float threshold = peakDetectionThreshold;
+  float peakWidth = 0.0f;
+  int specLen = spectrum.size();
+  float max = SpectrumMax(spectrum, hrROIbegin, hrROIend);
+  float signalToNoiseRatio = SignalToNoise(spectrum, hrROIbegin, hrROIend, max);
+  if (signalToNoiseRatio > signalToNoiseThreshold && spectrum.at(0) < dcThreshold) {
+    threshold *= max;
+    // Reuse VImag for interpolation x values passed to PeakSearch
+    for (int idx = 0; idx < dataLength; idx++) {
+      vImag[idx] = idx;
+    }
+    peakLocation = PeakSearch(vImag.data(),
+                              spectrum.data(),
+                              threshold,
+                              peakWidth,
+                              static_cast<float>(hrROIbegin),
+                              static_cast<float>(hrROIend),
+                              specLen);
+    peakLocation *= freqResolution;
   }
-
-  int t3 = (t2 * 4) / 3;
-  t3 = Trough(data.data(), dataIndex, t3 - 4, t3 + 4);
-  if (t3 < 0) {
-    return (60 * 24 * 3) / t2;
+  // Peak too wide? (broad spectrum noise or large, rapid HR change)
+  if (peakWidth > maxPeakWidth) {
+    peakLocation = 0.0f;
   }
-
-  return (60 * 24 * 4) / t3;
+  // Check HR limits
+  if (peakLocation < minHR || peakLocation > maxHR) {
+    peakLocation = 0.0f;
+  }
+  // Reset spectral averaging if bad reading
+  if (peakLocation == 0.0f) {
+    resetSpectralAvg = true;
+  }
+  // Set the ambient light threshold and return HR in BPM
+  alsThreshold = static_cast<uint16_t>(alsValue * alsFactor);
+  // Get current average HR. If HR reduced to zero, return -1 (reset) else HR
+  peakLocation = HeartRateAverage(peakLocation);
+  int rtn = -1;
+  if (peakLocation == 0.0f && lastPeakLocation > 0.0f) {
+    lastPeakLocation = 0.0f;
+  } else {
+    lastPeakLocation = peakLocation;
+    rtn = static_cast<int>((peakLocation * 60.0f) + 0.5f);
+  }
+  return rtn;
 }
 
-void Ppg::SetOffset(uint16_t offset) {
-  this->offset = offset;
-  dataIndex = 0;
+void Ppg::SpectrumAverage(const float* data, float* spectrum, int length, bool reset) {
+  if (reset) {
+    spectralAvgCount = 0;
+  }
+  float count = static_cast<float>(spectralAvgCount);
+  for (int idx = 0; idx < length; idx++) {
+    spectrum[idx] = (spectrum[idx] * count + data[idx]) / (count + 1);
+  }
+  if (spectralAvgCount < spectralAvgMax) {
+    spectralAvgCount++;
+  }
 }
 
-void Ppg::Reset() {
-  dataIndex = 0;
+float Ppg::HeartRateAverage(float hr) {
+  avgIndex++;
+  avgIndex %= dataAverage.size();
+  dataAverage[avgIndex] = hr;
+  float avg = 0.0f;
+  float total = 0.0f;
+  float min = 300.0f;
+  float max = 0.0f;
+  for (const float& value : dataAverage) {
+    if (value > 0.0f) {
+      avg += value;
+      if (value < min)
+        min = value;
+      if (value > max)
+        max = value;
+      total++;
+    }
+  }
+  if (total > 0) {
+    avg /= total;
+  } else {
+    avg = 0.0f;
+  }
+  return avg;
 }
diff --git a/src/components/heartrate/Ppg.h b/src/components/heartrate/Ppg.h
index 1f709bab..2f8a1faa 100644
--- a/src/components/heartrate/Ppg.h
+++ b/src/components/heartrate/Ppg.h
@@ -3,29 +3,77 @@
 #include <array>
 #include <cstddef>
 #include <cstdint>
-#include "components/heartrate/Biquad.h"
-#include "components/heartrate/Ptagc.h"
+// Note: Change internal define 'sqrt_internal sqrt' to
+// 'sqrt_internal sqrtf' to save ~3KB of flash.
+#define FFT_SPEED_OVER_PRECISION
+#include "libs/arduinoFFT-develop/src/arduinoFFT.h"
 
 namespace Pinetime {
   namespace Controllers {
     class Ppg {
     public:
       Ppg();
-      int8_t Preprocess(float spl);
+      int8_t Preprocess(uint32_t hrs, uint32_t als);
       int HeartRate();
-
-      void SetOffset(uint16_t offset);
-      void Reset();
+      void Reset(bool resetDaqBuffer);
+      static constexpr int deltaTms = 100;
+      // Daq dataLength: Must be power of 2
+      static constexpr uint16_t dataLength = 64;
+      static constexpr uint16_t spectrumLength = dataLength >> 1;
 
     private:
-      std::array<int8_t, 200> data;
-      size_t dataIndex = 0;
-      float offset;
-      Biquad hpf;
-      Ptagc agc;
-      Biquad lpf;
+      // The sampling frequency (Hz) based on sampling time in milliseconds (DeltaTms)
+      static constexpr float sampleFreq = 1000.0f / static_cast<float>(deltaTms);
+      // The frequency resolution (Hz)
+      static constexpr float freqResolution = sampleFreq / dataLength;
+      // Number of samples before each analysis
+      // 0.5 second update rate at 10Hz
+      static constexpr uint16_t overlapWindow = 5;
+      // Maximum number of spectrum running averages
+      // Note: actual number of spectra averaged = spectralAvgMax + 1
+      static constexpr uint16_t spectralAvgMax = 2;
+      // Multiple Peaks above this threshold (% of max) are rejected
+      static constexpr float peakDetectionThreshold = 0.6f;
+      // Maximum peak width (bins) at threshold for valid peak.
+      static constexpr float maxPeakWidth = 2.5f;
+      // Metric for spectrum noise level.
+      static constexpr float signalToNoiseThreshold = 3.0f;
+      // Heart rate Region Of Interest begin (bins)
+      static constexpr uint16_t hrROIbegin = static_cast<uint16_t>((30.0f / 60.0f) / freqResolution + 0.5f);
+      // Heart rate Region Of Interest end (bins)
+      static constexpr uint16_t hrROIend = static_cast<uint16_t>((240.0f / 60.0f) / freqResolution + 0.5f);
+      // Minimum HR (Hz)
+      static constexpr float minHR = 40.0f / 60.0f;
+      // Maximum HR (Hz)
+      static constexpr float maxHR = 230.0f / 60.0f;
+      // Threshold for high DC level after filtering
+      static constexpr float dcThreshold = 0.5f;
+      // ALS detection factor
+      static constexpr float alsFactor = 2.0f;
+
+      // Raw ADC data
+      std::array<uint16_t, dataLength> dataHRS;
+      // Stores Real numbers from FFT
+      std::array<float, dataLength> vReal;
+      // Stores Imaginary numbers from FFT
+      std::array<float, dataLength> vImag;
+      // Stores power spectrum calculated from FFT real and imag values
+      std::array<float, (spectrumLength)> spectrum;
+      // Stores each new HR value (Hz). Non zero values are averaged for HR output
+      std::array<float, 20> dataAverage;
+
+      uint16_t avgIndex = 0;
+      uint16_t spectralAvgCount = 0;
+      float lastPeakLocation = 0.0f;
+      uint16_t alsThreshold = UINT16_MAX;
+      uint16_t alsValue = 0;
+      uint16_t dataIndex = 0;
+      float peakLocation;
+      bool resetSpectralAvg = true;
 
-      int ProcessHeartRate();
+      int ProcessHeartRate(bool init);
+      float HeartRateAverage(float hr);
+      void SpectrumAverage(const float* data, float* spectrum, int length, bool reset);
     };
   }
 }
diff --git a/src/components/heartrate/Ptagc.cpp b/src/components/heartrate/Ptagc.cpp
deleted file mode 100644
index 221be460..00000000
--- a/src/components/heartrate/Ptagc.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
-  SPDX-License-Identifier: LGPL-3.0-or-later
-  Original work Copyright (C) 2020 Daniel Thompson
-  C++ port Copyright (C) 2021 Jean-François Milants
-*/
-
-#include "components/heartrate/Ptagc.h"
-#include <cmath>
-
-using namespace Pinetime::Controllers;
-
-/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */
-Ptagc::Ptagc(float start, float decay, float threshold) : peak {start}, decay {decay}, boost {1.0f / decay}, threshold {threshold} {
-}
-
-float Ptagc::Step(float spl) {
-  if (std::abs(spl) > peak) {
-    peak *= boost;
-  } else {
-    peak *= decay;
-  }
-
-  if ((spl > (peak * threshold)) || (spl < (peak * -threshold))) {
-    return 0.0f;
-  }
-
-  spl = 100.0f * spl / (2.0f * peak);
-  return spl;
-}
diff --git a/src/components/heartrate/Ptagc.h b/src/components/heartrate/Ptagc.h
deleted file mode 100644
index 3476636b..00000000
--- a/src/components/heartrate/Ptagc.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#pragma once
-
-namespace Pinetime {
-  namespace Controllers {
-    class Ptagc {
-    public:
-      Ptagc(float start, float decay, float threshold);
-      float Step(float spl);
-
-    private:
-      float peak;
-      float decay;
-      float boost;
-      float threshold;
-    };
-  }
-}