From c22e30a4a6ef014c7a5086ad47eaab7740a75ff2 Mon Sep 17 00:00:00 2001 From: Ceimour <113631258+Ceimour@users.noreply.github.com> Date: Sun, 30 Apr 2023 08:50:18 -0500 Subject: 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). --- src/components/heartrate/Biquad.cpp | 26 --- src/components/heartrate/Biquad.h | 22 --- src/components/heartrate/Ppg.cpp | 327 ++++++++++++++++++++++++++++-------- src/components/heartrate/Ppg.h | 74 ++++++-- src/components/heartrate/Ptagc.cpp | 29 ---- src/components/heartrate/Ptagc.h | 17 -- 6 files changed, 317 insertions(+), 178 deletions(-) delete mode 100644 src/components/heartrate/Biquad.cpp delete mode 100644 src/components/heartrate/Biquad.h delete mode 100644 src/components/heartrate/Ptagc.cpp delete mode 100644 src/components/heartrate/Ptagc.h (limited to 'src/components') 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 #include +#include + 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& 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(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& 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& 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& 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& signal) { + int size = signal.size(); + float offset = signal.front(); + float slope = (signal.at(size - 1) - offset) / static_cast(size - 1); - auto spl_int = static_cast(spl); + for (int idx = 0; idx < size; idx++) { + signal[idx] -= (slope * static_cast(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 FFT = ArduinoFFT(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(hrROIbegin), + static_cast(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(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((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(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 #include #include -#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 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(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((30.0f / 60.0f) / freqResolution + 0.5f); + // Heart rate Region Of Interest end (bins) + static constexpr uint16_t hrROIend = static_cast((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 dataHRS; + // Stores Real numbers from FFT + std::array vReal; + // Stores Imaginary numbers from FFT + std::array vImag; + // Stores power spectrum calculated from FFT real and imag values + std::array spectrum; + // Stores each new HR value (Hz). Non zero values are averaged for HR output + std::array 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 - -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; - }; - } -} -- cgit v1.2.3-70-g09d2