From 8438c1071055b265ea20a6d138977d2d493255a9 Mon Sep 17 00:00:00 2001 From: mark9064 <30447455+mark9064@users.noreply.github.com> Date: Sun, 9 Nov 2025 18:15:12 +0000 Subject: [PATCH 1/3] PPGv2 --- .gitmodules | 3 - src/CMakeLists.txt | 6 +- src/components/heartrate/FFT.h | 125 ++++ .../heartrate/HeartRateController.cpp | 14 +- .../heartrate/HeartRateController.h | 7 +- src/components/heartrate/IIR.h | 49 ++ src/components/heartrate/Ppg.cpp | 551 ++++++++++-------- src/components/heartrate/Ppg.h | 209 +++++-- src/components/heartrate/RLS.h | 147 +++++ src/displayapp/screens/HeartRate.cpp | 34 +- src/displayapp/screens/HeartRate.h | 4 +- .../screens/WatchFaceCasioStyleG7710.cpp | 2 +- src/displayapp/screens/WatchFaceDigital.cpp | 2 +- src/displayapp/screens/WatchFaceTerminal.cpp | 2 +- src/drivers/Bma421.cpp | 2 +- src/drivers/Hrs3300.cpp | 21 +- src/heartratetask/HeartRateTask.cpp | 93 ++- src/heartratetask/HeartRateTask.h | 15 +- src/libs/arduinoFFT | 1 - src/main.cpp | 2 +- 20 files changed, 867 insertions(+), 422 deletions(-) create mode 100644 src/components/heartrate/FFT.h create mode 100644 src/components/heartrate/IIR.h create mode 100644 src/components/heartrate/RLS.h delete mode 160000 src/libs/arduinoFFT diff --git a/.gitmodules b/.gitmodules index a6b4d5fd6f..ec390e3a7d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,6 +4,3 @@ [submodule "src/libs/littlefs"] path = src/libs/littlefs url = https://github.com/littlefs-project/littlefs.git -[submodule "src/libs/arduinoFFT"] - path = src/libs/arduinoFFT - url = https://github.com/kosme/arduinoFFT.git diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e4a354df64..ee743590fe 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -675,9 +675,9 @@ set(INCLUDE_FILES heartratetask/HeartRateTask.h components/heartrate/Ppg.h components/heartrate/HeartRateController.h - libs/arduinoFFT/src/arduinoFFT.h - libs/arduinoFFT/src/defs.h - libs/arduinoFFT/src/types.h + components/heartrate/FFT.h + components/heartrate/IIR.h + components/heartrate/RLS.h components/motor/MotorController.h buttonhandler/ButtonHandler.h touchhandler/TouchHandler.h diff --git a/src/components/heartrate/FFT.h b/src/components/heartrate/FFT.h new file mode 100644 index 0000000000..ff85de7282 --- /dev/null +++ b/src/components/heartrate/FFT.h @@ -0,0 +1,125 @@ +#include +#include +#include +#include + +namespace Pinetime { + namespace Utility { + // Fast Fourier transform + // Implements in-place N to N point complex-to-complex FFT + // Implements in-place 2N to N point real-to-complex FFT + // Performing these transforms requires some "twiddling" constants to be known + // These constants depend only on the size of the transform + // Since they are expensive to compute, they can only be computed at compile time (consteval) + class FFT { + public: + static consteval std::size_t IntegerLog2(std::size_t n) { + return std::bit_width(n) - 1; + } + + template + static consteval std::array, IntegerLog2(N)> GenComplexTwiddle() { + using namespace std::complex_literals; + + std::array, IntegerLog2(N)> result; + for (std::size_t i = 0; i < IntegerLog2(N); i++) { + result[i] = exp_consteval(-2.i * std::numbers::pi / static_cast(1 << (i + 1))); + } + return result; + } + + template + static consteval std::array, (N / 4) - 1> GenRealTwiddle() { + using namespace std::complex_literals; + + std::array, (N / 4) - 1> result; + for (std::size_t i = 0; i < (N / 4) - 1; i++) { + result[i] = exp_consteval(-2.i * std::numbers::pi * static_cast(i + 1) / static_cast(N)); + } + return result; + } + + template + static void ComplexFFT(std::array, N>& array, const std::array, IntegerLog2(N)>& twiddle) { + // In-place Cooley-Tukey + InplaceBitReverse(array); + for (std::size_t s = 1; s < IntegerLog2(N) + 1; s++) { + std::size_t m = 1 << s; + for (std::size_t k = 0; k < N; k += m) { + std::complex omega = 1.f; + for (std::size_t j = 0; j < m / 2; j++) { + std::complex t = omega * array[k + j + (m / 2)]; + std::complex u = array[k + j]; + array[k + j] = u + t; + array[k + j + (m / 2)] = u - t; + omega *= twiddle[s - 1]; + } + } + } + } + + template + static void RealFFT(std::array, N>& array, + const std::array, (N / 2) - 1>& realTwiddle, + const std::array, IntegerLog2(N)>& complexTwiddle) { + using namespace std::complex_literals; + + // See https://www.robinscheibler.org/2013/02/13/real-fft.html for how this works + FFT::ComplexFFT(array, complexTwiddle); + // Compute DC bin directly (xe/xo simplify) + // Nyquist bin ignored as unneeded + array[0] = array[0].real() + array[0].imag(); + // Since computations depend on the inverse of the index (mirrored) + // compute two at once, outside to inside + for (std::size_t index = 1; index < N / 2; index++) { + std::size_t indexInv = N - index; + std::complex xeLo = (array[index] + std::conj(array[indexInv])) / 2.f; + std::complex xoLo = -1.if * ((array[index] - std::conj(array[indexInv])) / 2.f); + std::complex xeHi = (array[indexInv] + std::conj(array[index])) / 2.f; + std::complex xoHi = -1.if * ((array[indexInv] - std::conj(array[index])) / 2.f); + array[index] = xeLo + (xoLo * realTwiddle[index - 1]); + array[indexInv] = xeHi + (xoHi * -std::conj(realTwiddle[index - 1])); + } + // Middle element not computed by above loop + // Since index == indexInv + // the middle simplifies to the conjugate as the twiddle is always -i + std::size_t middle = N / 2; + array[middle] = std::conj(array[middle]); + } + + private: + // consteval wrappers of builtins + template + static consteval std::complex<_Tp> exp_consteval(const std::complex<_Tp>& __z) { + return polar_consteval<_Tp>(__builtin_exp(__z.real()), __z.imag()); + } + + template + static consteval std::complex<_Tp> polar_consteval(const _Tp& __rho, const _Tp& __theta) { + return std::complex<_Tp>(__rho * __builtin_cos(__theta), __rho * __builtin_sin(__theta)); + } + + template + static void InplaceBitReverse(std::array& array) { + // Gold-Rader algorithm + // Faster algorithms exist, but this is sufficient + std::size_t swapTarget = 0; + for (std::size_t index = 0; index < N - 1; index++) { + // Only swap in one direction + // Otherwise entire array gets swapped twice + if (index < swapTarget) { + T temp = array[index]; + array[index] = array[swapTarget]; + array[swapTarget] = temp; + } + std::size_t kFactor = N / 2; + while (kFactor <= swapTarget) { + swapTarget -= kFactor; + kFactor /= 2; + } + swapTarget += kFactor; + } + } + }; + } +} diff --git a/src/components/heartrate/HeartRateController.cpp b/src/components/heartrate/HeartRateController.cpp index c365e8659a..65bc08e608 100644 --- a/src/components/heartrate/HeartRateController.cpp +++ b/src/components/heartrate/HeartRateController.cpp @@ -1,11 +1,15 @@ #include "components/heartrate/HeartRateController.h" -#include -#include + +#include +#include "heartratetask/HeartRateTask.h" using namespace Pinetime::Controllers; -void HeartRateController::Update(HeartRateController::States newState, uint8_t heartRate) { +void HeartRateController::UpdateState(HeartRateController::States newState) { this->state = newState; +} + +void HeartRateController::UpdateHeartRate(uint8_t heartRate) { if (this->heartRate != heartRate) { this->heartRate = heartRate; service->OnNewHeartRateValue(heartRate); @@ -14,14 +18,14 @@ void HeartRateController::Update(HeartRateController::States newState, uint8_t h void HeartRateController::Enable() { if (task != nullptr) { - state = States::NotEnoughData; + state = States::Stopped; task->PushMessage(Pinetime::Applications::HeartRateTask::Messages::Enable); } } void HeartRateController::Disable() { if (task != nullptr) { - state = States::Stopped; + state = States::Disabled; task->PushMessage(Pinetime::Applications::HeartRateTask::Messages::Disable); } } diff --git a/src/components/heartrate/HeartRateController.h b/src/components/heartrate/HeartRateController.h index 5bd3a8ef54..685ecd4480 100644 --- a/src/components/heartrate/HeartRateController.h +++ b/src/components/heartrate/HeartRateController.h @@ -15,12 +15,13 @@ namespace Pinetime { namespace Controllers { class HeartRateController { public: - enum class States : uint8_t { Stopped, NotEnoughData, NoTouch, Running }; + enum class States : uint8_t { Disabled, Stopped, NotEnoughData, Searching, Ready, NoTouch }; HeartRateController() = default; void Enable(); void Disable(); - void Update(States newState, uint8_t heartRate); + void UpdateState(States newState); + void UpdateHeartRate(uint8_t heartRate); void SetHeartRateTask(Applications::HeartRateTask* task); @@ -36,7 +37,7 @@ namespace Pinetime { private: Applications::HeartRateTask* task = nullptr; - States state = States::Stopped; + States state = States::Disabled; uint8_t heartRate = 0; Pinetime::Controllers::HeartRateService* service = nullptr; }; diff --git a/src/components/heartrate/IIR.h b/src/components/heartrate/IIR.h new file mode 100644 index 0000000000..448833b222 --- /dev/null +++ b/src/components/heartrate/IIR.h @@ -0,0 +1,49 @@ +#include + +namespace Pinetime { + namespace Utility { + struct SOSCoeffs { + float b0; + float b1; + float b2; + float a1; + float a2; + }; + + struct FilterState { + float s1; + float s2; + }; + + // Infinite impulse response digital filter + // Implemented as cascaded second order sections (SOS) + template + class IIRFilter { + public: + IIRFilter(const std::array& coeffs, const std::array& zi) : coeffs {coeffs}, zi {zi} {}; + + float FilterStep(float input) { + for (std::size_t i = 0; i < NSections; i++) { + // Transposed form II + float output = (coeffs[i].b0 * input) + filterStates[i].s1; + filterStates[i].s1 = ((-coeffs[i].a1) * output) + (coeffs[i].b1 * input) + filterStates[i].s2; + filterStates[i].s2 = ((-coeffs[i].a2) * output) + (coeffs[i].b2 * input); + input = output; + } + return input; + } + + void Prime(float value) { + for (std::size_t i = 0; i < NSections; i++) { + filterStates[i].s1 = zi[i].s1 * value; + filterStates[i].s2 = zi[i].s2 * value; + } + } + + private: + std::array filterStates; + const std::array& coeffs; + const std::array& zi; + }; + } +} diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp index 25be6237d2..f2c4915517 100644 --- a/src/components/heartrate/Ppg.cpp +++ b/src/components/heartrate/Ppg.cpp @@ -1,297 +1,354 @@ #include "components/heartrate/Ppg.h" #include -#include using namespace Pinetime::Controllers; namespace { - 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); + // Computes a mod b + // fmod is a remainder rather than a true mathematical modulo + float MathematicalModulus(float a, float b) { + return std::fmod((std::fmod(a, b) + b), b); + } +} + +Ppg::Ppg() { + Reset(); +} + +void Ppg::Reset() { + ready = false; + hrsCount = 0; + filteredHrsTailIndex = 0; + filteredHrsArray.fill(0.f); + accAdaptive.Reset(); + lockedHrBin = std::nullopt; +} + +bool Ppg::SufficientData() const { + return ready; +} - return (pointY0 * (1 - mu) + pointY1 * mu); +void Ppg::Ingest(uint16_t hrs, int16_t accX, int16_t accY, int16_t accZ) { + // Acceleration is normalised to 1024=1G in the BMA driver + float accXNorm = static_cast(accX) / 1024.f; + float accYNorm = static_cast(accY) / 1024.f; + float accZNorm = static_cast(accZ) / 1024.f; + // Filter initial states assume the previous input was zero + // Instead prime the filters with a real input to avoid + // ringing caused by a large jump from zero + if (hrsCount == 0) { + ppgFilter.Prime(hrs); + accXFilter.Prime(accXNorm); + accYFilter.Prime(accYNorm); + accZFilter.Prime(accZNorm); } + // Highpass the hrs to remove dc baseline + float hrsFilt = ppgFilter.FilterStep(hrs); + // Invert phase: normalise to positive pulse + hrsFilt *= -1.f; - 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; - } - if (peaks != 1) { - width = 0.0f; - peakCenter = 0.0f; + filteredHrsArray[filteredHrsTailIndex] = std::abs(hrsFilt); + filteredHrsTailIndex = (filteredHrsTailIndex + 1) % adaptiveResetWindow; + + // Highpass all the acceleration channels to remove dc baseline + float accXFilt = accXFilter.FilterStep(accXNorm); + float accYFilt = accYFilter.FilterStep(accYNorm); + float accZFilt = accZFilter.FilterStep(accZNorm); + std::optional resetThresh = std::nullopt; + if (hrsCount > adaptiveResetWindow) { + resetThresh = 0.f; + for (size_t i = 0; i < adaptiveResetWindow; i++) { + resetThresh = std::max(resetThresh.value(), filteredHrsArray[i]); } - return peakCenter; + resetThresh.value() *= adaptiveResetThresh; } + std::array filteredAcc {accXFilt, accYFilt, accZFilt}; + // Motion adaptive filtering to remove components in the hrs signal caused by motion + float hrsAdaptive = accAdaptive.FilterStep(hrsFilt, filteredAcc, resetThresh); + StoreHrs(hrsAdaptive); +} - 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++; +std::optional Ppg::HeartRate() { + if (hrsCount == inputLength) { + ready = true; + RunAlg(); + // Roll back by overlapWindow + for (size_t index = 0; index < inputLength - overlapWindow; index++) { + adaptiveHrsArray[index] = adaptiveHrsArray[index + overlapWindow]; } - if (total > 0) { - mean /= static_cast(total); + hrsCount -= overlapWindow; + } + if (lockedHrBin.has_value() && rollingEnergy > rollingEnergyDisplayMinimum) { + return std::round((lockedHrBin.value() + minFrequencyBin) * binWidth * 60.f); + } + return {}; +} + +void Ppg::StoreHrs(float filteredHrs) { + if (hrsCount < inputLength) { + adaptiveHrsArray[hrsCount] = filteredHrs; + hrsCount++; + } else { + for (size_t index = 0; index < inputLength - 1; index++) { + adaptiveHrsArray[index] = adaptiveHrsArray[index + 1]; } - return mean; + adaptiveHrsArray[inputLength - 1] = filteredHrs; } +} - float SignalToNoise(const std::array& signal, int start, int end, float max) { - float mean = SpectrumMean(signal, start, end); - return max / mean; +void Ppg::RunAlg() { + complexFftArray.fill(0.f); + float mean = 0.f; + for (size_t index = 0; index < inputLength; index++) { + mean += adaptiveHrsArray[index]; } + mean /= static_cast(inputLength); + for (size_t index = 0; index < inputLength; index++) { + fftArray[index] = adaptiveHrsArray[index] - mean; + } + // Normalise energy of each segment so algorithm is input amplitude invariant + SegmentRMSNorm(); + // Transform to frequency domain so we can do spectral processing + Utility::FFT::RealFFT(complexFftArray, realTwiddle, complexTwiddle); + // Find peaks in the spectral magnitudes so we can identify possible heart rate peaks + ProminenceFilter(); + // Filter peaks by how likely they are to be heart rates using harmonic information + HarmonicFilter(); - // 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/ + // Then identify and track peaks over time + // Readers note: I suggest collapsing the lambdas immediately below and reading the tracking logic first + // then come back to these after - 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; + // Largest element in array + auto FindPeakIndex = [](std::span array) { + float largest = array[0]; + size_t largestIndex = 0; + for (size_t index = 1; index < array.size(); index++) { + if (array[index] > largest) { + largest = array[index]; + largestIndex = index; } } - 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; - } + return largestIndex; + }; + // Total energy of a peak taking into account energy spread into neighbour bins + auto CalculateLogPeakEnergy = [](size_t bin, std::span roi) { + size_t lowerBound = std::max(0, static_cast(bin) - 1); + size_t upperBound = std::min(bin + 2, roiLength); + float acc = 0.f; + for (size_t index = lowerBound; index < upperBound; index++) { + acc += roi[index]; } - } - - 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 std::log(acc); + }; + // Peak energy relative to total energy + auto PeakProminence = [](float peakEnergy, std::span roi) { + float acc = 0.f; + for (auto item : roi) { + acc += item; } - return max; - } - - void Detrend(std::array& signal) { - int size = signal.size(); - float offset = signal.front(); - float slope = (signal.at(size - 1) - offset) / static_cast(size - 1); - - for (int idx = 0; idx < size; idx++) { - signal[idx] -= (slope * static_cast(idx) + offset); + return peakEnergy / acc; + }; + // Exact peak location by interpolating with neighbour bins + auto PeakSearch = [](size_t bin, std::span roi) { + std::optional upperEnergy = std::nullopt; + std::optional lowerEnergy = std::nullopt; + float centralEnergy = roi[bin]; + if (bin < roi.size() - 1) { + upperEnergy = roi[bin + 1]; } - for (int idx = 0; idx < size - 1; idx++) { - signal[idx] = signal[idx + 1] - signal[idx]; + if (bin > 0) { + lowerEnergy = roi[bin - 1]; } - } + float peakLocation = bin; + if (upperEnergy.has_value()) { + peakLocation += (upperEnergy.value() / centralEnergy) / 2.f; + } + if (lowerEnergy.has_value()) { + peakLocation -= (lowerEnergy.value() / centralEnergy) / 2.f; + } + return peakLocation; + }; + // Exponential moving average + auto EmaStep = [](float state, float next, float alpha) { + return (state * (1.f - alpha)) + (next * alpha); + }; + // Strongest peak near to the previous peak + auto TrackHr = [FindPeakIndex, PeakSearch](float lockedHrBin, std::span roi) { + ssize_t centre = std::round(lockedHrBin); + float deviation = ((lockedHrBin + minFrequencyBin) * 2.f * 0.1f) + 1.f; - // 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}; -} + // Need an odd number of bins so window is centred + // Avoid subsampling bins + auto RoundToOdd = [](float val) { + return (2 * static_cast(std::floor(val / 2))) + 1; + }; -Ppg::Ppg() { - dataAverage.fill(0.0f); - spectrum.fill(0.0f); -} + ssize_t width = std::max(RoundToOdd(deviation), 3U); + ssize_t offset = (width - 1) / 2; + size_t minBin = std::max(0, -offset + centre); + size_t maxBin = std::min(static_cast(roiLength), -offset + centre + width); + std::span searchRoi = roi.subspan(minBin, maxBin - minBin); + size_t peak = FindPeakIndex(searchRoi); + return PeakSearch(peak, searchRoi) + minBin; + }; -int8_t Ppg::Preprocess(uint16_t hrs, uint16_t als) { - if (dataIndex < dataLength) { - dataHRS[dataIndex++] = hrs; - } - alsValue = als; - if (alsValue > alsThreshold) { - return 1; - } - return 0; -} + std::span roi = fftArray.subspan(minFrequencyBin, roiLength); + size_t bestIndex = FindPeakIndex(roi); + float bestPeakEnergy = CalculateLogPeakEnergy(bestIndex, roi); + std::optional strongBin = std::nullopt; -int Ppg::HeartRate() { - if (dataIndex < dataLength) { - if (!enoughData) { - return -2; + if (bestPeakEnergy > strongPeakEnergyMinimum) { + if (PeakProminence(std::exp(bestPeakEnergy), roi) > strongPeakProminenceMinimum) { + strongBin = bestIndex; } - return 0; - } - enoughData = true; - 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; -} - -void Ppg::Reset(bool resetDaqBuffer) { - if (resetDaqBuffer) { - dataIndex = 0; - enoughData = false; - } - avgIndex = 0; - dataAverage.fill(0.0f); - lastPeakLocation = 0.0f; - alsThreshold = UINT16_MAX; - alsValue = 0; - resetSpectralAvg = true; - spectrum.fill(0.0f); -} -// 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++; + // If there's a strong peak and no heart rate locked, or if the current locked peak is weak + // accept the new peak and begin tracking + // Otherwise continue tracking the existing peak + if ((!lockedHrBin.has_value() || rollingEnergy < rollingEnergyWeak) && strongBin.has_value()) { + lockedHrBin = PeakSearch(strongBin.value(), roi); + rollingEnergy = bestPeakEnergy; + } else if (lockedHrBin.has_value()) { + float newLockBin = TrackHr(lockedHrBin.value(), roi); + float lockEnergy = CalculateLogPeakEnergy(std::round(newLockBin), roi); + // Only update the estimated heart rate if the new peak location has substantial energy + // This helps avoid tracking noise when the signal quality transiently degrades + if (lockEnergy > hrUpdateEnergyMinimum) { + lockedHrBin = EmaStep(lockedHrBin.value(), newLockBin, hrMovementAlpha); } } - // 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; + + // Update rolling energy (if currently tracking a heart rate) + // The rolling energy provides information on the energy of the peak over time + // If the peak becomes consistently too weak we stop tracking it + if (lockedHrBin.has_value()) { + float lockEnergy = CalculateLogPeakEnergy(std::round(lockedHrBin.value()), roi); + rollingEnergy = EmaStep(rollingEnergy, lockEnergy, rollingEnergyAlpha); + + if (rollingEnergy < trackingEnergyMinimum) { + lockedHrBin = std::nullopt; } - peakLocation = PeakSearch(vImag.data(), - spectrum.data(), - threshold, - peakWidth, - static_cast(hrROIbegin), - static_cast(hrROIend), - specLen); - peakLocation *= freqResolution; } - // Peak too wide? (broad spectrum noise or large, rapid HR change) - if (peakWidth > maxPeakWidth) { - peakLocation = 0.0f; +} + +[[gnu::noinline]] void Ppg::ProminenceFilter() { + std::array prominence; + for (size_t index = minFrequencyBin - 1; index < fftLength / 2; index++) { + prominence[index] = std::log(std::max(std::abs(complexFftArray[index]), logFloor)); } - // Check HR limits - if (peakLocation < minHR || peakLocation > maxHR) { - peakLocation = 0.0f; + prominenceFilter.Prime(prominence[minFrequencyBin - 1] * primingFactor); + + // Zero phase filter: forwards + backwards + // IIR filters delay the signal, and we want to avoid shifting peaks up or down in frequency + // So we run the filter both forwards and backwards so the delay in each direction cancels out + // The result is zero delay (phase shift) and twice the attenuation + + // forwards pass + for (size_t index = minFrequencyBin; index < fftLength / 2; index++) { + prominence[index] = prominenceFilter.FilterStep(prominence[index]); } - // Reset spectral averaging if bad reading - if (peakLocation == 0.0f) { - resetSpectralAvg = true; + // backwards pass + for (size_t index = (fftLength / 2) - 1; index > minFrequencyBin - 1; index--) { + prominence[index] = prominenceFilter.FilterStep(prominence[index]); } - // 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); + for (size_t index = minFrequencyBin; index < fftLength / 2; index++) { + float val = (prominence[index] - minProminence) / (maxProminence - minProminence); + val = std::max(std::min(1.f, val), logFloor); + complexFftArray[index] *= val; } - return rtn; } -void Ppg::SpectrumAverage(const float* data, float* spectrum, int length, bool reset) { - if (reset) { - spectralAvgCount = 0; +[[gnu::noinline]] void Ppg::SegmentRMSNorm() { + float rollSum = 0.0; + for (size_t index = 0; index < segmentNormWindow; index++) { + rollSum += std::pow(fftArray[index], 2); } - float count = static_cast(spectralAvgCount); - for (int idx = 0; idx < length; idx++) { - spectrum[idx] = (spectrum[idx] * count + data[idx]) / (count + 1); + std::array outputs; + outputs[0] = std::sqrt(rollSum / segmentNormWindow) * 2; + for (size_t index = segmentNormWindow; index < inputLength; index++) { + rollSum -= std::pow(fftArray[index - segmentNormWindow], 2); + rollSum += std::pow(fftArray[index], 2); + outputs[index - segmentNormWindow + 1] = std::sqrt(rollSum / segmentNormWindow) * 2; } - if (spectralAvgCount < spectralAvgMax) { - spectralAvgCount++; + size_t halfNorm = segmentNormWindow / 2; + for (size_t index = 0; index < inputLength; index++) { + if (index <= halfNorm) { + fftArray[index] /= outputs[0]; + } else if (index >= inputLength - halfNorm) { + fftArray[index] /= outputs[outputs.size() - 1]; + } else { + fftArray[index] /= outputs[index - halfNorm]; + } } } -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++; +void Ppg::HarmonicFilter() { + // These arrays are precomputed from clean PPG signals sampled from the sensor + // These constants encode the shape of a heart beat on a PPG trace (beat morphology) + // We use these constants to penalise components in the signal that don't correspond to a wave + // of the right shape, allowing us to eliminate noise + static constexpr std::array, Ppg::roiLength> magArray { + {{0.3486803888990022, 0.255141508669598}, {0.36864722158239355, 0.25164382316780665}, {0.36771410549818545, 0.246948695573225}, + {0.35975580379324007, 0.24714886736121827}, {0.3658843568236732, 0.24650278235091835}, {0.3648928604195185, 0.24415607374450915}, + {0.3751222377393388, 0.23988295745228638}, {0.3921596966191289, 0.23210350117630843}, {0.40638499674260403, 0.22516855732998384}, + {0.4232127690875433, 0.21887312064931116}, {0.41564130033821856, 0.20673749156382631}, {0.38906619325409764, 0.1936140541005159}, + {0.3672394339754485, 0.18042745108982736}, {0.35275401585397503, 0.17151514618926156}, {0.3354326028852867, 0.15978169786664942}, + {0.32178572197981903, 0.1488045144540834}, {0.31281592316734735, 0.13991117977784315}, {0.299797871716369, 0.12788875993747936}, + {0.2966269594265519, 0.11875935055909558}, {0.2925247464787818, 0.11318066458803003}, {0.2964994671856146, 0.10580432119722558}, + {0.29770787961626416, 0.09839859522862732}, {0.3007591380586962, 0.09394035889667726}, {0.31104783374357775, 0.08933500974545709}, + {0.31486127213961, 0.08685088306152053}, {0.3313907046528911, 0.0924341145096451}, {0.30897754578100955, 0.08563613561250377}, + {0.3205154251103728, 0.08487605022457582}, {0.32285950835530364, 0.08122541502849505}, {0.31912839897477796, 0.08362307999351845}, + {0.30726200775051915, 0.08424021081449604}, {0.32024159984649864, 0.08405258601108509}, {0.33036938914308, 0.08392491126815374}, + {0.30531854817881166, 0.08146558813586019}, {0.3308979795151278, 0.08381011591996054}, {0.30632137960394173, 0.08313313923993279}}}; + + static constexpr std::array phaseArray { + {0.2705229340875336, 0.36779124535955804, 0.45434779541743203, 0.5138242784968899, 0.5537065717483055, 0.5891055187012357, + 0.6221592074197645, 0.6558273339604292, 0.6849101620718369, 0.7083511816762154, 0.7167869997820329, 0.7112028603271641, + 0.7005327637607478, 0.6910573759719243, 0.6803636809415491, 0.6647598810363644, 0.6514913727959067, 0.6366402208133636, + 0.6210659369643988, 0.6057153725313422, 0.5894965013729228, 0.5757024639742967, 0.57176394709517514, 0.5881247512605522, + 0.6067800339551624, 0.621127310629737, 0.6243531002213607, 0.6276813318132224, 0.6314029448574778, 0.634138691540482, + 0.6372739733495546, 0.6398513675481557, 0.642109540006218, 0.6449694089531159, 0.6465457224589797, 0.649148754973601}}; + + for (size_t frequencyBin = minFrequencyBin; frequencyBin < maxFrequencyBin; frequencyBin++) { + float f0Mag = std::abs(complexFftArray[frequencyBin]); + float f1Mag = (0.5f * std::abs(complexFftArray[(frequencyBin * 2) - 1])) + std::abs(complexFftArray[frequencyBin * 2]) + + (0.5f * std::abs(complexFftArray[(frequencyBin * 2) + 1])); + float f2Mag = std::abs(complexFftArray[(frequencyBin * 3) - 1]) + std::abs(complexFftArray[frequencyBin * 3]) + + std::abs(complexFftArray[(frequencyBin * 3) + 1]); + + float f1MagRel = f1Mag / f0Mag; + float f2MagRel = f2Mag / f0Mag; + float f1MagPriorRel = f1MagRel / magArray[frequencyBin - minFrequencyBin][0]; + + // Returns a phase between 0 and 1 + auto ComplexToPhaseNorm = [](std::complex value) { + return (std::arg(value) / (2.f * static_cast(std::numbers::pi))) + 0.5f; + }; + // Calculates the distance between two phases, 0 being in-phase and 1 being antiphase + auto PhaseDistance = [](float p1, float p2) { + return 1.f - (std::abs(0.5f - std::abs(p1 - p2)) * 2.f); + }; + + float f0Phase = ComplexToPhaseNorm(complexFftArray[frequencyBin]); + float f1Phase = ComplexToPhaseNorm(complexFftArray[frequencyBin * 2]); + float f1PhaseOffset = MathematicalModulus(f1Phase - (2 * f0Phase), 1.f); + + float filteredEnergy = f0Mag; + if (f1MagPriorRel > f1MagMinimum) { + float f1PhaseDist = PhaseDistance(f1PhaseOffset, phaseArray[frequencyBin - minFrequencyBin]); + filteredEnergy *= std::exp(-f1ExpAttenuationFactor * f1PhaseDist); + if (f1MagRel > f1MagHigh) { + filteredEnergy = std::min(filteredEnergy, fnMagHighEnergyCeiling); + } + } else { + filteredEnergy *= noF1Penalty; + filteredEnergy = std::min(filteredEnergy, noF1EnergyCeiling); } + if (f2MagRel > f2MagHigh) { + filteredEnergy = std::min(filteredEnergy, fnMagHighEnergyCeiling); + } + fftArray[frequencyBin] = filteredEnergy; } - 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 7893538207..c88bf5a680 100644 --- a/src/components/heartrate/Ppg.h +++ b/src/components/heartrate/Ppg.h @@ -1,81 +1,162 @@ #pragma once #include +#include +#include #include #include -// Note: Change internal define 'sqrt_internal sqrt' to -// 'sqrt_internal sqrtf' to save ~3KB of flash. -#define sqrt_internal sqrtf -#define FFT_SPEED_OVER_PRECISION -#include "libs/arduinoFFT/src/arduinoFFT.h" +#include +#include + +#include "components/heartrate/FFT.h" +#include "components/heartrate/IIR.h" +#include "components/heartrate/RLS.h" namespace Pinetime { namespace Controllers { class Ppg { public: Ppg(); - int8_t Preprocess(uint16_t hrs, uint16_t als); - int HeartRate(); - 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; + void Ingest(uint16_t hrs, int16_t accX, int16_t accY, int16_t accZ); + std::optional HeartRate(); + void Reset(); + [[nodiscard]] bool SufficientData() const; + static constexpr float sampleDuration = 0.048f; private: - // 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; + static constexpr float sampleRate = 1.f / sampleDuration; + // FFT length + static constexpr uint16_t fftLength = 256; + // Data going into FFT length (FFT is zero padded) + static constexpr uint16_t inputLength = std::round(8 * sampleRate); + // Uupdate rate + static constexpr float overlapTime = 0.5f; // 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; - bool enoughData = false; - - int ProcessHeartRate(bool init); - float HeartRateAverage(float hr); - void SpectrumAverage(const float* data, float* spectrum, int length, bool reset); + static constexpr uint16_t overlapWindow = std::round(sampleRate * overlapTime); + // Frequency resolution (Hz) + static constexpr float binWidth = 1.f / (fftLength * sampleDuration); + + // Input filtering coefficients + // Generated with scipy butter(4, 0.5, btype="highpass", output="sos", fs=sampleRate) + static constexpr std::array inputCoeffs { + Utility::SOSCoeffs {.b0 = 0.8209900772315549, + .b1 = -1.6419801544631099, + .b2 = 0.8209900772315549, + .a1 = -1.7363191518098462, + .a2 = 0.7562495196628967}, + Utility::SOSCoeffs {.b0 = 1., .b1 = -2., .b2 = 1., .a1 = -1.8698102590459458, .a2 = 0.89127290676215}}; + // Input filtering initial state + static constexpr std::array inputInitialState {Utility::FilterState {.s1 = -0.82099008, .s2 = 0.82099008}, + Utility::FilterState {.s1 = 0., .s2 = 0.}}; + + // Adaptive filtering strength + static constexpr float adaptiveMu = 0.99f; + // Number of noise channels + static constexpr size_t noiseChannels = 3; + // Adaptive filter length + static constexpr size_t adaptiveLength = std::round(0.25f * sampleRate) * noiseChannels; + // Adaptive filter reset consideration window + static constexpr size_t adaptiveResetWindow = 1.f * sampleRate; + // Adaptive filter reset threshold + static constexpr float adaptiveResetThresh = 2.f; + + // FFT constants (computed at compile time to avoid expensive runtime calculations) + static constexpr auto complexTwiddle = Utility::FFT::GenComplexTwiddle(); + static constexpr auto realTwiddle = Utility::FFT::GenRealTwiddle(); + + // Prominence filter constants + // Generated with scipy butter(2, (0.07, 0.65), btype="bandpass", output="sos") + static constexpr std::array prominenceCoeffs { + Utility::SOSCoeffs {.b0 = 0.3705549357629432, + .b1 = 0.7411098715258864, + .b2 = 0.3705549357629432, + .a1 = 0.5064122272787734, + .a2 = 0.2534300618617984}, + Utility::SOSCoeffs {.b0 = 1., .b1 = -2., .b2 = 1., .a1 = -1.6907167610529146, .a2 = 0.7379546730609459}}; + // Prominence filtering initial state + static constexpr std::array prominenceInitialState { + Utility::FilterState {.s1 = 0.47169084512212833, .s2 = 0.15710453541040081}, + Utility::FilterState {.s1 = -0.8422457808850721, .s2 = 0.8422457808850718}}; + static constexpr float primingFactor = 2.f; + + // Threshold after bandpass filtering at which signal is allowed through + static constexpr float minProminence = 0.5f; + // Threshold after bandpass filtering at which all signal is allowed through + static constexpr float maxProminence = 0.8f; + // Clip values below floor to prevent huge negative logs + static constexpr float logFloor = 1e-4f; + + // First harmonic minimum relative magnitude to be considered + static constexpr float f1MagMinimum = 0.5f; + // First harmonic phase penalty exponent + static constexpr float f1ExpAttenuationFactor = 4.f; + // First harmonic magnitude high threshold + static constexpr float f1MagHigh = 1.f; + // Second harmonic magnitude high threshold + static constexpr float f2MagHigh = 0.75f; + // First harmonic absent energy penality + static constexpr float noF1Penalty = 0.2f; + // First harmonic absent energy limit + static constexpr float noF1EnergyCeiling = 6.f; + // Harmonic magnitude high energy ceiling + static constexpr float fnMagHighEnergyCeiling = 3.f; + + // Minimum frequency to consider + static constexpr float minFrequency = 30.f / 60.f; + // Maximum frequency to consider + // Cannot go higher as sample rate puts 2nd harmonic out of range + static constexpr float maxFrequency = 207.f / 60.f; + // Minimum frequency FFT bin + static constexpr size_t minFrequencyBin = (minFrequency / binWidth) + 1; + // Maximum frequency FFT bin + static constexpr size_t maxFrequencyBin = (maxFrequency / binWidth) + 1; + // Length of the region of interest + static constexpr size_t roiLength = maxFrequencyBin - minFrequencyBin; + + // Minimum log energy to consider a peak strong + static constexpr float strongPeakEnergyMinimum = 3.f; + // Minimum prominence of a strong peak to be accepted + static constexpr float strongPeakProminenceMinimum = 0.8f; + // Minimum log energy to keep tracking a peak + static constexpr float trackingEnergyMinimum = -1.f; + // Minimum log energy to update the HR prediction + static constexpr float hrUpdateEnergyMinimum = 0.f; + // Exponential moving average alpha factor for rolling energy + static constexpr float rollingEnergyAlpha = 0.1f * overlapTime; + // Exponential moving average alpha factor for heart rate prediction + static constexpr float hrMovementAlpha = 0.25f * overlapTime; + // Minimum rolling energy to return the prediction + static constexpr float rollingEnergyDisplayMinimum = 0.f; + // Maximum energy at which new strong peaks will replace the current peak + static constexpr float rollingEnergyWeak = 1.f; + + // Window size for pre-FFT segment RMS normalisation + static constexpr size_t segmentNormWindow = std::round((1.f / minFrequency) * sampleRate); + + Utility::IIRFilter ppgFilter = Utility::IIRFilter(inputCoeffs, inputInitialState); + Utility::IIRFilter accXFilter = Utility::IIRFilter(inputCoeffs, inputInitialState); + Utility::IIRFilter accYFilter = Utility::IIRFilter(inputCoeffs, inputInitialState); + Utility::IIRFilter accZFilter = Utility::IIRFilter(inputCoeffs, inputInitialState); + std::array adaptiveHrsArray; + std::array filteredHrsArray; + uint8_t hrsCount; + uint8_t filteredHrsTailIndex; + Utility::RLS accAdaptive = Utility::RLS(adaptiveMu); + + std::array, fftLength / 2> complexFftArray; + std::span fftArray {reinterpret_cast(complexFftArray.data()), fftLength}; + + Utility::IIRFilter prominenceFilter = Utility::IIRFilter(prominenceCoeffs, prominenceInitialState); + + std::optional lockedHrBin; + float rollingEnergy; + bool ready; + + void StoreHrs(float filteredHrs); + void RunAlg(); + void ProminenceFilter(); + void SegmentRMSNorm(); + void HarmonicFilter(); }; } } diff --git a/src/components/heartrate/RLS.h b/src/components/heartrate/RLS.h new file mode 100644 index 0000000000..07ad122589 --- /dev/null +++ b/src/components/heartrate/RLS.h @@ -0,0 +1,147 @@ +#include +#include +#include +#include +#include + +namespace Pinetime { + namespace Utility { + // Recursive least squares adaptive filter + // Supports N noise channels concurrently (NoiseStep) + // See https://en.wikipedia.org/wiki/Recursive_least_squares_filter + // and https://matousc89.github.io/padasip/sources/filters/rls.html + template + class RLS { + public: + explicit RLS(float mu) { + this->mu = mu; + Reset(); + } + + void Reset(bool hard = true) { + if (hard) { + startupRemaining = FIRLength; + } + for (std::size_t i = 0; i < FIRLength; i++) { + for (std::size_t j = 0; j < FIRLength; j++) { + correlationInv[i][j] = 0.f; + } + correlationInv[i][i] = 1.f / eps; + weights[i] = 0.f; + } + } + + float FilterStep(float dataPoint, std::array noisePoints, std::optional resetThresh) { + for (std::size_t index = 0; index < FIRLength - NoiseStep; index++) { + noiseHistory[index] = noiseHistory[index + NoiseStep]; + } + for (std::size_t index = FIRLength - NoiseStep; index < FIRLength; index++) { + noiseHistory[index] = noisePoints[index - FIRLength + NoiseStep]; + } + // Don't filter until noise points ready + if (startupRemaining > 0) { + startupRemaining -= NoiseStep; + if (startupRemaining > 0) { + return dataPoint; + } + } + + float noiseEstimate = DotProduct(weights, noiseHistory); + float denoisedDataPoint = dataPoint - noiseEstimate; + + // Resetting is useful to recover from filter divergence + // If the denoised output exceeds the chosen threshold, reset the filter state + if (resetThresh.has_value()) { + if (std::abs(denoisedDataPoint) > resetThresh) { + Reset(false); + denoisedDataPoint = dataPoint; + } + } + + std::array, FIRLength> correlationUpdate; + for (auto& arr : correlationUpdate) { + arr.fill(0.f); + } + + MatrixMulDynamic( + correlationInv, + [this](std::size_t x, std::size_t y) { + return noiseHistory[x] * noiseHistory[y]; + }, + correlationUpdate); + + MatrixMulInplace(correlationUpdate, correlationInv); + + float normFactor = mu + CalculateNormFactor(); + + for (std::size_t i = 0; i < FIRLength; i++) { + for (std::size_t j = 0; j < FIRLength; j++) { + correlationInv[i][j] = (1.f / mu) * (correlationInv[i][j] - (correlationUpdate[i][j] / normFactor)); + } + } + + DotMatrixInplaceAdd(correlationInv, noiseHistory, weights, denoisedDataPoint); + + return denoisedDataPoint; + } + + private: + // noinline to bound stack use more tightly + [[gnu::noinline]] float CalculateNormFactor() { + std::array temp; + temp.fill(0.f); + DotMatrixInplaceAdd(correlationInv, noiseHistory, temp); + return DotProduct(temp, noiseHistory); + } + + [[gnu::noinline]] static void MatrixMulInplace(std::array, FIRLength>& a, + const std::array, FIRLength>& b) { + // Uses 1 row of extra space + for (std::size_t i = 0; i < FIRLength; i++) { + std::array aPrior {a[i]}; + for (std::size_t j = 0; j < FIRLength; j++) { + float acc = 0.f; + for (std::size_t k = 0; k < FIRLength; k++) { + acc += aPrior[k] * b[k][j]; + } + a[i][j] = acc; + } + } + } + + static float DotProduct(const std::array& a, const std::array& b) { + return std::inner_product(a.begin(), a.end(), b.begin(), 0.f); + } + + static void DotMatrixInplaceAdd(std::array, FIRLength>& mat, + const std::array& vec, + std::array& out, + float norm = 1.f) { + for (std::size_t i = 0; i < FIRLength; i++) { + out[i] += DotProduct(mat[i], vec) * norm; + } + } + + static void MatrixMulDynamic(const std::array, FIRLength>& mat, + const std::function& matGen, + std::array, FIRLength>& out) { + for (std::size_t i = 0; i < FIRLength; i++) { + for (std::size_t j = 0; j < FIRLength; j++) { + float acc = 0.f; + for (std::size_t k = 0; k < FIRLength; k++) { + acc += mat[i][k] * matGen(k, j); + } + out[i][j] = acc; + } + } + } + + static constexpr float eps = 1.f; + std::array weights; + std::array noiseHistory; + std::array, FIRLength> correlationInv; + float mu; + std::size_t startupRemaining; + }; + } +} diff --git a/src/displayapp/screens/HeartRate.cpp b/src/displayapp/screens/HeartRate.cpp index 14c873e201..7c80f8c11d 100644 --- a/src/displayapp/screens/HeartRate.cpp +++ b/src/displayapp/screens/HeartRate.cpp @@ -1,6 +1,6 @@ #include "displayapp/screens/HeartRate.h" -#include #include +#include #include "displayapp/DisplayApp.h" #include "displayapp/InfiniTimeTheme.h" @@ -12,12 +12,16 @@ namespace { switch (s) { case Pinetime::Controllers::HeartRateController::States::NotEnoughData: return "Not enough data,\nplease wait..."; + case Pinetime::Controllers::HeartRateController::States::Searching: + return "Searching..."; case Pinetime::Controllers::HeartRateController::States::NoTouch: return "No touch detected"; - case Pinetime::Controllers::HeartRateController::States::Running: + case Pinetime::Controllers::HeartRateController::States::Ready: return "Measuring..."; case Pinetime::Controllers::HeartRateController::States::Stopped: return "Stopped"; + case Pinetime::Controllers::HeartRateController::States::Disabled: + return "Disabled"; } return ""; } @@ -30,7 +34,7 @@ namespace { HeartRate::HeartRate(Controllers::HeartRateController& heartRateController, System::SystemTask& systemTask) : heartRateController {heartRateController}, wakeLock(systemTask) { - bool isHrRunning = heartRateController.State() != Controllers::HeartRateController::States::Stopped; + bool isHrRunning = heartRateController.State() != Controllers::HeartRateController::States::Disabled; label_hr = lv_label_create(lv_scr_act(), nullptr); lv_obj_set_style_local_text_font(label_hr, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, &jetbrains_mono_76); @@ -50,7 +54,7 @@ HeartRate::HeartRate(Controllers::HeartRateController& heartRateController, Syst label_status = lv_label_create(lv_scr_act(), nullptr); lv_obj_set_style_local_text_color(label_status, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, LV_COLOR_GRAY); - lv_label_set_text_static(label_status, ToString(Pinetime::Controllers::HeartRateController::States::NotEnoughData)); + lv_label_set_text_static(label_status, ToString(Pinetime::Controllers::HeartRateController::States::Disabled)); lv_obj_align(label_status, label_hr, LV_ALIGN_OUT_BOTTOM_MID, 0, 10); @@ -77,18 +81,10 @@ HeartRate::~HeartRate() { void HeartRate::Refresh() { auto state = heartRateController.State(); - switch (state) { - case Controllers::HeartRateController::States::NoTouch: - case Controllers::HeartRateController::States::NotEnoughData: - // case Controllers::HeartRateController::States::Stopped: - lv_label_set_text_static(label_hr, "---"); - break; - default: - if (heartRateController.HeartRate() == 0) { - lv_label_set_text_static(label_hr, "---"); - } else { - lv_label_set_text_fmt(label_hr, "%03d", heartRateController.HeartRate()); - } + if (heartRateController.HeartRate() == 0) { + lv_label_set_text_static(label_hr, "---"); + } else { + lv_label_set_text_fmt(label_hr, "%03d", heartRateController.HeartRate()); } lv_label_set_text_static(label_status, ToString(state)); @@ -97,14 +93,14 @@ void HeartRate::Refresh() { void HeartRate::OnStartStopEvent(lv_event_t event) { if (event == LV_EVENT_CLICKED) { - if (heartRateController.State() == Controllers::HeartRateController::States::Stopped) { + if (heartRateController.State() == Controllers::HeartRateController::States::Disabled) { heartRateController.Enable(); - UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Stopped); + UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Disabled); wakeLock.Lock(); lv_obj_set_style_local_text_color(label_hr, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, Colors::highlight); } else { heartRateController.Disable(); - UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Stopped); + UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Disabled); wakeLock.Release(); lv_obj_set_style_local_text_color(label_hr, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, Colors::lightGray); } diff --git a/src/displayapp/screens/HeartRate.h b/src/displayapp/screens/HeartRate.h index 69c935de08..c85662dd72 100644 --- a/src/displayapp/screens/HeartRate.h +++ b/src/displayapp/screens/HeartRate.h @@ -1,13 +1,11 @@ #pragma once -#include -#include #include "displayapp/screens/Screen.h" #include "systemtask/SystemTask.h" #include "systemtask/WakeLock.h" #include "Symbols.h" -#include #include +#include namespace Pinetime { namespace Controllers { diff --git a/src/displayapp/screens/WatchFaceCasioStyleG7710.cpp b/src/displayapp/screens/WatchFaceCasioStyleG7710.cpp index c695f852fe..8a0998a48f 100644 --- a/src/displayapp/screens/WatchFaceCasioStyleG7710.cpp +++ b/src/displayapp/screens/WatchFaceCasioStyleG7710.cpp @@ -290,7 +290,7 @@ void WatchFaceCasioStyleG7710::Refresh() { } heartbeat = heartRateController.HeartRate(); - heartbeatRunning = heartRateController.State() != Controllers::HeartRateController::States::Stopped; + heartbeatRunning = heartRateController.State() != Controllers::HeartRateController::States::Disabled; if (heartbeat.IsUpdated() || heartbeatRunning.IsUpdated()) { if (heartbeatRunning.Get()) { lv_obj_set_style_local_text_color(heartbeatIcon, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, color_text); diff --git a/src/displayapp/screens/WatchFaceDigital.cpp b/src/displayapp/screens/WatchFaceDigital.cpp index 3163c6e750..3fe1abddb1 100644 --- a/src/displayapp/screens/WatchFaceDigital.cpp +++ b/src/displayapp/screens/WatchFaceDigital.cpp @@ -151,7 +151,7 @@ void WatchFaceDigital::Refresh() { } heartbeat = heartRateController.HeartRate(); - heartbeatRunning = heartRateController.State() != Controllers::HeartRateController::States::Stopped; + heartbeatRunning = heartRateController.State() != Controllers::HeartRateController::States::Disabled; if (heartbeat.IsUpdated() || heartbeatRunning.IsUpdated()) { if (heartbeatRunning.Get()) { lv_obj_set_style_local_text_color(heartbeatIcon, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, lv_color_hex(0xCE1B1B)); diff --git a/src/displayapp/screens/WatchFaceTerminal.cpp b/src/displayapp/screens/WatchFaceTerminal.cpp index 96d77741fd..1babcce544 100644 --- a/src/displayapp/screens/WatchFaceTerminal.cpp +++ b/src/displayapp/screens/WatchFaceTerminal.cpp @@ -135,7 +135,7 @@ void WatchFaceTerminal::Refresh() { } heartbeat = heartRateController.HeartRate(); - heartbeatRunning = heartRateController.State() != Controllers::HeartRateController::States::Stopped; + heartbeatRunning = heartRateController.State() != Controllers::HeartRateController::States::Disabled; if (heartbeat.IsUpdated() || heartbeatRunning.IsUpdated()) { if (heartbeatRunning.Get()) { lv_label_set_text_fmt(heartbeatValue, "[L_HR]#ee3311 %d bpm#", heartbeat.Get()); diff --git a/src/drivers/Bma421.cpp b/src/drivers/Bma421.cpp index 74d47d0667..b25075b9ca 100644 --- a/src/drivers/Bma421.cpp +++ b/src/drivers/Bma421.cpp @@ -85,7 +85,7 @@ void Bma421::Init() { return; accel_conf.odr = BMA4_OUTPUT_DATA_RATE_100HZ; - accel_conf.range = BMA4_ACCEL_RANGE_2G; + accel_conf.range = BMA4_ACCEL_RANGE_8G; accel_conf.bandwidth = BMA4_ACCEL_NORMAL_AVG4; accel_conf.perf_mode = BMA4_CIC_AVG_MODE; ret = bma4_set_accel_config(&accel_conf, &bma); diff --git a/src/drivers/Hrs3300.cpp b/src/drivers/Hrs3300.cpp index a4b724796d..fa7420cc96 100644 --- a/src/drivers/Hrs3300.cpp +++ b/src/drivers/Hrs3300.cpp @@ -16,7 +16,7 @@ using namespace Pinetime::Drivers; namespace { - static constexpr uint8_t ledDriveCurrentValue = 0x2f; + static constexpr uint8_t ledDriveCurrentValue = 0x2e; } /** Driver for the HRS3300 heart rate sensor. @@ -33,21 +33,22 @@ void Hrs3300::Init() { Disable(); vTaskDelay(100); - // HRS disabled, 50ms wait time between ADC conversion period, current 12.5mA - WriteRegister(static_cast(Registers::Enable), 0x50); + // HRS disabled, 0ms wait time between ADC conversion period, current 12.5mA + WriteRegister(static_cast(Registers::Enable), 0x70); // Current 12.5mA and low nibble 0xF. // Note: Setting low nibble to 0x8 per the datasheet results in - // modulated LED driver output. Setting to 0xF results in clean, - // steady output during the ADC conversion period. + // modulated LED driver output (50% PWM) + // Saves some power at the cost of some resolution + // 0xF sets 100% duty WriteRegister(static_cast(Registers::PDriver), ledDriveCurrentValue); - // HRS and ALS both in 15-bit mode results in ~50ms LED drive period - // and presumably ~50ms ADC conversion period. - WriteRegister(static_cast(Registers::Res), 0x77); + // HRS in 15-bit and ALS in 16-bit mode result in 48ms period + // See https://github.com/wasp-os/wasp-os/pull/363#issuecomment-1279733038 + WriteRegister(static_cast(Registers::Res), 0x78); - // Gain set to 1x - WriteRegister(static_cast(Registers::Hgain), 0x00); + // Gain set to 8x + WriteRegister(static_cast(Registers::Hgain), 0x0e); } void Hrs3300::Enable() { diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp index e9bc11a30a..e5457ba304 100644 --- a/src/heartratetask/HeartRateTask.cpp +++ b/src/heartratetask/HeartRateTask.cpp @@ -1,9 +1,12 @@ #include "heartratetask/HeartRateTask.h" -#include #include +#include +#include #include +#include using namespace Pinetime::Applications; +using ControllerStates = Pinetime::Controllers::HeartRateController::States; namespace { constexpr TickType_t backgroundMeasurementTimeLimit = 30 * configTICK_RATE_HZ; @@ -43,14 +46,14 @@ TickType_t HeartRateTask::CurrentTaskDelay() { // To avoid the number of milliseconds overflowing a u32, we take a factor of 2 out of the divisor and dividend // (1024 / 2) * 65536 * 100 = 3355443200 which is less than 2^32 + constexpr uint16_t deltaTms = Controllers::Ppg::sampleDuration * 1000; // Guard against future tick rate changes - static_assert((configTICK_RATE_HZ / 2ULL) * (std::numeric_limits::max() + 1ULL) * - static_cast((Pinetime::Controllers::Ppg::deltaTms)) < + static_assert((configTICK_RATE_HZ / 2ULL) * (std::numeric_limits::max() + 1ULL) * static_cast((deltaTms)) < std::numeric_limits::max(), "Overflow"); - TickType_t elapsedTarget = RoundedDiv(static_cast(configTICK_RATE_HZ / 2) * (static_cast(count) + 1U) * - static_cast((Pinetime::Controllers::Ppg::deltaTms)), - static_cast(1000 / 2)); + TickType_t elapsedTarget = + RoundedDiv(static_cast(configTICK_RATE_HZ / 2) * (static_cast(count) + 1U) * static_cast((deltaTms)), + static_cast(1000 / 2)); // On count overflow, reset both count and start time // Count is 16bit to avoid overflow in elapsedTarget @@ -90,15 +93,16 @@ TickType_t HeartRateTask::CurrentTaskDelay() { HeartRateTask::HeartRateTask(Drivers::Hrs3300& heartRateSensor, Controllers::HeartRateController& controller, - Controllers::Settings& settings) - : heartRateSensor {heartRateSensor}, controller {controller}, settings {settings} { + Controllers::Settings& settings, + Drivers::Bma421& motionSensor) + : heartRateSensor {heartRateSensor}, controller {controller}, settings {settings}, motionSensor {motionSensor} { } void HeartRateTask::Start() { messageQueue = xQueueCreate(10, 1); controller.SetHeartRateTask(this); - if (pdPASS != xTaskCreate(HeartRateTask::Process, "Heartrate", 500, this, 1, &taskHandle)) { + if (xTaskCreate(HeartRateTask::Process, "HRM", 400, this, 1, &taskHandle) != pdPASS) { APP_ERROR_HANDLER(NRF_ERROR_NO_MEM); } } @@ -146,10 +150,10 @@ void HeartRateTask::Work() { // If this constraint is somehow violated, the unexpected state // will self-resolve at the next screen on event newState = States::ForegroundMeasuring; - valueCurrentlyShown = false; break; case Messages::Disable: newState = States::Disabled; + SendHeartRate(ControllerStates::Disabled, 0); break; } } @@ -184,50 +188,43 @@ void HeartRateTask::PushMessage(HeartRateTask::Messages msg) { void HeartRateTask::StartMeasurement() { heartRateSensor.Enable(); - ppg.Reset(true); - vTaskDelay(100); - measurementSucceeded = false; + ppg.Reset(); count = 0; measurementStartTime = xTaskGetTickCount(); } void HeartRateTask::StopMeasurement() { heartRateSensor.Disable(); - ppg.Reset(true); - vTaskDelay(100); + ppg.Reset(); + controller.UpdateState(ControllerStates::Stopped); } void HeartRateTask::HandleSensorData() { auto sensorData = heartRateSensor.ReadHrsAls(); - int8_t ambient = ppg.Preprocess(sensorData.hrs, sensorData.als); - int bpm = ppg.HeartRate(); - - // Ambient light detected - if (ambient > 0) { - // Reset all DAQ buffers - ppg.Reset(true); - controller.Update(Controllers::HeartRateController::States::NotEnoughData, bpm); - bpm = 0; - valueCurrentlyShown = false; + auto motionValues = motionSensor.Process(); + // Sensor starting up + if (sensorData.hrs == 0) { + return; } - // Reset requested, or not enough data - if (bpm == -1) { - // Reset all DAQ buffers except HRS buffer - ppg.Reset(false); - // Set HR to zero and update - bpm = 0; - controller.Update(Controllers::HeartRateController::States::Running, bpm); - valueCurrentlyShown = false; - } else if (bpm == -2) { - // Not enough data - bpm = 0; - if (!valueCurrentlyShown) { - controller.Update(Controllers::HeartRateController::States::NotEnoughData, bpm); + ppg.Ingest(sensorData.hrs, motionValues.x, motionValues.y, motionValues.z); + + std::optional bpm = ppg.HeartRate(); + if (bpm.has_value()) { + SendHeartRate(ControllerStates::Ready, bpm.value()); + } else if (ppg.SufficientData()) { + SendHeartRate(ControllerStates::Searching, 0); + } else { + // If there's currently a value shown, don't clear it + // But still update the algorithm state + if (valueCurrentlyShown) { + controller.UpdateState(ControllerStates::NotEnoughData); + } else { + SendHeartRate(ControllerStates::NotEnoughData, 0); } } - if (bpm != 0) { + if (bpm.has_value()) { // Maintain constant frequency acquisition in background mode // If the last measurement time is set to the start time, then the next measurement // will start exactly one background period after this one @@ -237,9 +234,6 @@ void HeartRateTask::HandleSensorData() { } else { lastMeasurementTime = xTaskGetTickCount(); } - measurementSucceeded = true; - valueCurrentlyShown = true; - controller.Update(Controllers::HeartRateController::States::Running, bpm); return; } // If been measuring for longer than the time limit, set the last measurement time @@ -247,15 +241,6 @@ void HeartRateTask::HandleSensorData() { // and also means that background measurement won't begin immediately after // an unsuccessful long foreground measurement if (xTaskGetTickCount() - measurementStartTime > backgroundMeasurementTimeLimit) { - // When measuring, propagate failure if no value within the time limit - // Prevents stale heart rates from being displayed for >1 background period - // Or more than the time limit after switching to screen on (where the last background measurement was successful) - // Note: Once a successful measurement is recorded in screen on it will never be cleared - // without some other state change e.g. ambient light reset - if (!measurementSucceeded) { - controller.Update(Controllers::HeartRateController::States::Running, 0); - valueCurrentlyShown = false; - } if (state == States::BackgroundMeasuring) { lastMeasurementTime = xTaskGetTickCount() - backgroundMeasurementTimeLimit; } else { @@ -263,3 +248,9 @@ void HeartRateTask::HandleSensorData() { } } } + +void HeartRateTask::SendHeartRate(ControllerStates state, int bpm) { + valueCurrentlyShown = bpm != 0; + controller.UpdateState(state); + controller.UpdateHeartRate(bpm); +} diff --git a/src/heartratetask/HeartRateTask.h b/src/heartratetask/HeartRateTask.h index e00bc4d638..e148b26d0a 100644 --- a/src/heartratetask/HeartRateTask.h +++ b/src/heartratetask/HeartRateTask.h @@ -4,26 +4,24 @@ #include #include #include -#include +#include "components/heartrate/Ppg.h" #include "components/settings/Settings.h" +#include "components/heartrate/HeartRateController.h" namespace Pinetime { namespace Drivers { class Hrs3300; - } - - namespace Controllers { - class HeartRateController; + class Bma421; } namespace Applications { class HeartRateTask { public: enum class Messages : uint8_t { GoToSleep, WakeUp, Enable, Disable }; - explicit HeartRateTask(Drivers::Hrs3300& heartRateSensor, Controllers::HeartRateController& controller, - Controllers::Settings& settings); + Controllers::Settings& settings, + Pinetime::Drivers::Bma421& motionSensor); void Start(); void Work(); void PushMessage(Messages msg); @@ -38,16 +36,17 @@ namespace Pinetime { [[nodiscard]] bool BackgroundMeasurementNeeded() const; [[nodiscard]] std::optional BackgroundMeasurementInterval() const; TickType_t CurrentTaskDelay(); + void SendHeartRate(Controllers::HeartRateController::States state, int bpm); TaskHandle_t taskHandle; QueueHandle_t messageQueue; bool valueCurrentlyShown; - bool measurementSucceeded; States state = States::Disabled; uint16_t count; Drivers::Hrs3300& heartRateSensor; Controllers::HeartRateController& controller; Controllers::Settings& settings; + Drivers::Bma421& motionSensor; Controllers::Ppg ppg; TickType_t lastMeasurementTime; TickType_t measurementStartTime; diff --git a/src/libs/arduinoFFT b/src/libs/arduinoFFT deleted file mode 160000 index 419d7b044e..0000000000 --- a/src/libs/arduinoFFT +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 419d7b044e56b87de8efbcf76f09c04759628fb4 diff --git a/src/main.cpp b/src/main.cpp index d0ab3e4887..1c91b423a6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -99,7 +99,7 @@ Pinetime::Controllers::Settings settingsController {fs}; Pinetime::Controllers::MotorController motorController {}; Pinetime::Controllers::HeartRateController heartRateController; -Pinetime::Applications::HeartRateTask heartRateApp(heartRateSensor, heartRateController, settingsController); +Pinetime::Applications::HeartRateTask heartRateApp(heartRateSensor, heartRateController, settingsController, motionSensor); Pinetime::Controllers::DateTime dateTimeController {settingsController}; Pinetime::Drivers::Watchdog watchdog; From 3c056cfcd293a9af841b4525092fba6e2c0a990c Mon Sep 17 00:00:00 2001 From: mark9064 <30447455+mark9064@users.noreply.github.com> Date: Sun, 9 Nov 2025 17:48:15 +0000 Subject: [PATCH 2/3] Auto gain control --- src/components/heartrate/IIR.h | 7 + src/components/heartrate/Ppg.cpp | 4 + src/components/heartrate/Ppg.h | 1 + src/drivers/Hrs3300.cpp | 314 ++++++++++++++++++++++++---- src/drivers/Hrs3300.h | 45 +++- src/heartratetask/HeartRateTask.cpp | 43 ++-- src/heartratetask/HeartRateTask.h | 1 + 7 files changed, 351 insertions(+), 64 deletions(-) diff --git a/src/components/heartrate/IIR.h b/src/components/heartrate/IIR.h index 448833b222..601b05778f 100644 --- a/src/components/heartrate/IIR.h +++ b/src/components/heartrate/IIR.h @@ -40,6 +40,13 @@ namespace Pinetime { } } + void Scale(float scaleFactor) { + for (std::size_t i = 0; i < NSections; i++) { + filterStates[i].s1 *= scaleFactor; + filterStates[i].s2 *= scaleFactor; + } + } + private: std::array filterStates; const std::array& coeffs; diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp index f2c4915517..0eb3efb669 100644 --- a/src/components/heartrate/Ppg.cpp +++ b/src/components/heartrate/Ppg.cpp @@ -28,6 +28,10 @@ bool Ppg::SufficientData() const { return ready; } +void Ppg::ScaleHrs(float scaleFactor) { + ppgFilter.Scale(scaleFactor); +} + void Ppg::Ingest(uint16_t hrs, int16_t accX, int16_t accY, int16_t accZ) { // Acceleration is normalised to 1024=1G in the BMA driver float accXNorm = static_cast(accX) / 1024.f; diff --git a/src/components/heartrate/Ppg.h b/src/components/heartrate/Ppg.h index c88bf5a680..20e3f2f329 100644 --- a/src/components/heartrate/Ppg.h +++ b/src/components/heartrate/Ppg.h @@ -21,6 +21,7 @@ namespace Pinetime { std::optional HeartRate(); void Reset(); [[nodiscard]] bool SufficientData() const; + void ScaleHrs(float scaleFactor); static constexpr float sampleDuration = 0.048f; private: diff --git a/src/drivers/Hrs3300.cpp b/src/drivers/Hrs3300.cpp index fa7420cc96..c965a6b791 100644 --- a/src/drivers/Hrs3300.cpp +++ b/src/drivers/Hrs3300.cpp @@ -6,7 +6,7 @@ #include "drivers/Hrs3300.h" #include -#include +#include #include #include @@ -16,69 +16,104 @@ using namespace Pinetime::Drivers; namespace { - static constexpr uint8_t ledDriveCurrentValue = 0x2e; + constexpr uint16_t hrsHighThresh = 15000; + constexpr uint16_t hrsLowThresh = 3000; + constexpr uint16_t alsHighThresh = 1500; + constexpr uint16_t alsLowThresh = 300; + constexpr uint16_t touchRegainedThresh = 70; + constexpr uint16_t touchLostThresh = 1000; + // The proprietary driver uses this and it seems to be required to re-initialise the sensor + constexpr std::array, 18> bootupRegisters = {{ + {0x01, 0xd0}, + {0x0c, 0x4e}, + {0x16, 0x78}, + {0x17, 0x0d}, + {0x02, 0x80}, + {0x03, 0x00}, + {0x04, 0x00}, + {0x05, 0x00}, + {0x06, 0x00}, + {0x07, 0x00}, + {0x08, 0x74}, + {0x09, 0x00}, + {0x0a, 0x08}, + {0x0b, 0x00}, + {0x0c, 0x6e}, + {0x0d, 0x02}, + {0x0e, 0x07}, + {0x0f, 0x0f}, + }}; + + constexpr uint8_t powerChangeDelay = .25f / Pinetime::Controllers::Ppg::sampleDuration; + constexpr uint8_t noTouchDelay = 2 / Pinetime::Controllers::Ppg::sampleDuration; + // Duration ambient light must be low for gain up to trigger + // Long to prevent gaining up when high ambient light only occurs occasionally + // Much better to be in too low a gain than too high and saturate the ADC + constexpr uint16_t gainUpWaitTime = 30 / Pinetime::Controllers::Ppg::sampleDuration; + // If just gained up, wait at least this long before considering another gain up + constexpr uint16_t gainUpCascadeWaitTime = 5 / Pinetime::Controllers::Ppg::sampleDuration; } /** Driver for the HRS3300 heart rate sensor. * Original implementation from wasp-os : https://github.com/wasp-os/wasp-os/blob/master/wasp/drivers/hrs3300.py * * Experimentaly derived changes to improve signal/noise (see comments below) - Ceimour + * + * Refactored to include automatic drive and gain control using proprietary driver techniques - mark */ Hrs3300::Hrs3300(TwiMaster& twiMaster, uint8_t twiAddress) : twiMaster {twiMaster}, twiAddress {twiAddress} { } void Hrs3300::Init() { - nrf_gpio_cfg_input(30, NRF_GPIO_PIN_NOPULL); - - Disable(); - vTaskDelay(100); - - // HRS disabled, 0ms wait time between ADC conversion period, current 12.5mA - WriteRegister(static_cast(Registers::Enable), 0x70); - - // Current 12.5mA and low nibble 0xF. - // Note: Setting low nibble to 0x8 per the datasheet results in - // modulated LED driver output (50% PWM) - // Saves some power at the cost of some resolution - // 0xF sets 100% duty - WriteRegister(static_cast(Registers::PDriver), ledDriveCurrentValue); - - // HRS in 15-bit and ALS in 16-bit mode result in 48ms period - // See https://github.com/wasp-os/wasp-os/pull/363#issuecomment-1279733038 + for (auto bootupRegister : bootupRegisters) { + WriteRegister(bootupRegister[0], bootupRegister[1]); + } + r7f = ReadRegister(0x7f); + r80 = ReadRegister(0x80); + r81 = ReadRegister(0x81); + r82 = ReadRegister(0x82); + // Top 4 bits of r80 clear and bottom 3 bits of r81 clear + if (((r80 & 0xf0) | (r81 & 0x07)) == 0) { + r81 |= 0x04; + } WriteRegister(static_cast(Registers::Res), 0x78); - - // Gain set to 8x - WriteRegister(static_cast(Registers::Hgain), 0x0e); + Disable(); } void Hrs3300::Enable() { NRF_LOG_INFO("ENABLE"); - auto value = ReadRegister(static_cast(Registers::Enable)); - value |= 0x80; - WriteRegister(static_cast(Registers::Enable), value); - WriteRegister(static_cast(Registers::PDriver), ledDriveCurrentValue); + ppgDrive = PPGDrive::Current13mALow; + ppgGain = PPGGain::Gain8x; + ppgState = PPGState::Running; + actionDelay = powerChangeDelay; + hrsLowCount = 0; + hrsHighCount = 0; + alsLowCount = 0; + alsHighCount = 0; + + // Needs to be initialised twice or sensor gets stuck in weird state + SetPower(); + SetPower(); } void Hrs3300::Disable() { NRF_LOG_INFO("DISABLE"); - auto value = ReadRegister(static_cast(Registers::Enable)); - value &= ~0x80; - WriteRegister(static_cast(Registers::Enable), value); - - WriteRegister(static_cast(Registers::PDriver), 0); + WriteRegister(static_cast(Registers::Enable), 0x00); + WriteRegister(static_cast(Registers::PDriver), 0x00); + ppgState = PPGState::Off; } Hrs3300::PackedHrsAls Hrs3300::ReadHrsAls() { - constexpr Registers dataRegisters[] = - {Registers::C1dataM, Registers::C0DataM, Registers::C0DataH, Registers::C1dataH, Registers::C1dataL, Registers::C0dataL}; - // Calculate smallest register address - constexpr uint8_t baseOffset = static_cast(*std::min_element(std::begin(dataRegisters), std::end(dataRegisters))); - // Calculate largest address to determine length of read needed - // Add one to largest relative index to find the length - constexpr uint8_t length = static_cast(*std::max_element(std::begin(dataRegisters), std::end(dataRegisters))) - baseOffset + 1; - - Hrs3300::PackedHrsAls res; + static constexpr Registers dataRegisters[] = + {Registers::C1DataM, Registers::C0DataM, Registers::C0DataH, Registers::C1DataH, Registers::C1DataL, Registers::C0DataL}; + + // Calculate smallest/largest register address + constexpr auto bounds = std::ranges::minmax(dataRegisters); + constexpr uint8_t baseOffset = static_cast(bounds.min); + constexpr uint8_t length = static_cast(bounds.max) - baseOffset + 1; + + PackedHrsAls res; uint8_t buf[length]; auto ret = twiMaster.Read(twiAddress, baseOffset, buf, length); if (ret != TwiMaster::ErrorCodes::NoError) { @@ -87,16 +122,16 @@ Hrs3300::PackedHrsAls Hrs3300::ReadHrsAls() { // hrs uint8_t m = static_cast(Registers::C0DataM) - baseOffset; uint8_t h = static_cast(Registers::C0DataH) - baseOffset; - uint8_t l = static_cast(Registers::C0dataL) - baseOffset; + uint8_t l = static_cast(Registers::C0DataL) - baseOffset; // There are two extra bits (17 and 18) but they are not read here // as resolutions >16bit aren't practically useful (too slow) and // all hrs values throughout InfiniTime are 16bit res.hrs = (buf[m] << 8) | ((buf[h] & 0x0f) << 4) | (buf[l] & 0x0f); // als - m = static_cast(Registers::C1dataM) - baseOffset; - h = static_cast(Registers::C1dataH) - baseOffset; - l = static_cast(Registers::C1dataL) - baseOffset; + m = static_cast(Registers::C1DataM) - baseOffset; + h = static_cast(Registers::C1DataH) - baseOffset; + l = static_cast(Registers::C1DataL) - baseOffset; res.als = ((buf[h] & 0x3f) << 11) | (buf[m] << 3) | (buf[l] & 0x07); return res; @@ -104,14 +139,201 @@ Hrs3300::PackedHrsAls Hrs3300::ReadHrsAls() { void Hrs3300::WriteRegister(uint8_t reg, uint8_t data) { auto ret = twiMaster.Write(twiAddress, reg, &data, 1); - if (ret != TwiMaster::ErrorCodes::NoError) + if (ret != TwiMaster::ErrorCodes::NoError) { NRF_LOG_INFO("WRITE ERROR"); + } } uint8_t Hrs3300::ReadRegister(uint8_t reg) { uint8_t value; auto ret = twiMaster.Read(twiAddress, reg, &value, 1); - if (ret != TwiMaster::ErrorCodes::NoError) + if (ret != TwiMaster::ErrorCodes::NoError) { NRF_LOG_INFO("READ ERROR"); + } return value; } + +void Hrs3300::SetPower() { + uint8_t r80Temp = r80; + uint8_t r81Temp = r81; + uint8_t r7aTemp = 0x00; + + // All in MSB->LSB order + // It's just what the proprietary driver does + if (ppgDrive == PPGDrive::Current13mALow) { + r80Temp = (r81 & 0x01) << 7 | (r80 & 0xe0) >> 1 | (r80 & 0x0f); + r81Temp = (r81 & 0xf8) | (r81 & 0x06) >> 1; + } else if (ppgDrive == PPGDrive::Current40mAHigh) { + r80Temp = r80 | 0xf0; + r81Temp = r81 | 0x07; + } else if (ppgDrive == PPGDrive::NoTouchPowerSave) { + r80Temp = (r80 & 0x8f) | 0x30; + r81Temp = r81 & 0xf8; + r7aTemp = 0x02; + } + // Set special power registers + WriteRegister(0x85, 0x20); + WriteRegister(0x7f, r7f); + WriteRegister(0x80, r80Temp); + WriteRegister(0x81, r81Temp); + WriteRegister(0x82, r82); + WriteRegister(0x85, 0x00); + WriteRegister(0x7a, r7aTemp); + + uint8_t gain = 0x01; + switch (ppgGain) { + case PPGGain::Gain1x: + break; + case PPGGain::Gain2x: + gain |= 0x04; + break; + case PPGGain::Gain4x: + gain |= 0x08; + break; + case PPGGain::Gain8x: + gain |= 0x0c; + break; + } + + // HRS enabled, 800ms wait + uint8_t enable = 0x80; + // OSC on, 50% duty cycle (last 4 bits) + uint8_t drive = 0x2e; + // See https://github.com/wasp-os/wasp-os/pull/363#issuecomment-1279733038 for acquisition period details + switch (ppgDrive) { + // 800ms, 13mA (exact current unknown due to special power registers) + case PPGDrive::NoTouchPowerSave: + break; + // 48ms, 13mA/Lower + case PPGDrive::Current13mALow: + case PPGDrive::Current13mA: + enable |= 0x70; + break; + // 48ms, 20mA + case PPGDrive::Current20mA: + enable |= 0x70; + drive |= 0x40; + break; + // 48ms, 30mA + case PPGDrive::Current30mA: + enable |= 0x78; + break; + // 48ms, 40mA/Higher + case PPGDrive::Current40mA: + case PPGDrive::Current40mAHigh: + enable |= 0x78; + drive |= 0x40; + break; + } + WriteRegister(static_cast(Registers::HGain), gain); + WriteRegister(static_cast(Registers::Enable), enable); + WriteRegister(static_cast(Registers::PDriver), drive); +} + +Hrs3300::PPGState Hrs3300::AutoGain(uint16_t hrs, uint16_t als) { + PPGDrive previousPpgDrive = ppgDrive; + PPGGain previousPpgGain = ppgGain; + + if (hrs > hrsHighThresh) { + hrsHighCount = std::max(hrsHighCount, static_cast(hrsHighCount + 1)); + } else { + hrsHighCount = 0; + } + if (hrs < hrsLowThresh) { + hrsLowCount = std::max(hrsLowCount, static_cast(hrsLowCount + 1)); + } else { + hrsLowCount = 0; + } + + if (als > alsHighThresh) { + alsHighCount = std::max(alsHighCount, static_cast(alsHighCount + 1)); + } else { + alsHighCount = 0; + } + if (als < alsLowThresh) { + alsLowCount = std::max(alsLowCount, static_cast(alsLowCount + 1)); + } else { + alsLowCount = 0; + } + + if (actionDelay > 0) { + actionDelay--; + return ppgState; + } + + // No touch handling + if (ppgState == PPGState::NoTouch) { + // If touch regained, reinitialise + if (hrs >= touchRegainedThresh) { + // Disable then enable to avoid waiting for the 800ms inter-sample delay in no touch mode + Disable(); + Enable(); + return PPGState::Reset; + } + // Otherwise return early as waiting until touch is regained + return ppgState; + } + + // Drive changes + if (hrsHighCount > powerChangeDelay) { + if (ppgDrive == PPGDrive::Current40mAHigh) { + ppgDrive = PPGDrive::Current40mA; + } else if (ppgDrive == PPGDrive::Current40mA) { + ppgDrive = PPGDrive::Current30mA; + } else if (ppgDrive == PPGDrive::Current30mA) { + ppgDrive = PPGDrive::Current20mA; + } else if (ppgDrive == PPGDrive::Current20mA) { + ppgDrive = PPGDrive::Current13mA; + } else if (ppgDrive == PPGDrive::Current13mA) { + ppgDrive = PPGDrive::Current13mALow; + } + } else if (hrsLowCount > powerChangeDelay) { + if (ppgDrive == PPGDrive::Current13mALow) { + ppgDrive = PPGDrive::Current13mA; + } else if (ppgDrive == PPGDrive::Current13mA) { + ppgDrive = PPGDrive::Current20mA; + } else if (ppgDrive == PPGDrive::Current20mA) { + ppgDrive = PPGDrive::Current30mA; + } else if (ppgDrive == PPGDrive::Current30mA) { + ppgDrive = PPGDrive::Current40mA; + } else if (ppgDrive == PPGDrive::Current40mA) { + ppgDrive = PPGDrive::Current40mAHigh; + } + } + + // Gain changes + if (alsHighCount > powerChangeDelay) { + if (ppgGain == PPGGain::Gain8x) { + ppgGain = PPGGain::Gain4x; + } else if (ppgGain == PPGGain::Gain4x) { + ppgGain = PPGGain::Gain2x; + } else if (ppgGain == PPGGain::Gain2x) { + ppgGain = PPGGain::Gain1x; + } + } else if (alsLowCount > gainUpWaitTime) { + if (ppgGain == PPGGain::Gain1x) { + ppgGain = PPGGain::Gain2x; + } else if (ppgGain == PPGGain::Gain2x) { + ppgGain = PPGGain::Gain4x; + } else if (ppgGain == PPGGain::Gain4x) { + ppgGain = PPGGain::Gain8x; + } + if (ppgGain != previousPpgGain) { + alsLowCount = gainUpWaitTime - gainUpCascadeWaitTime; + } + } + + // Transition into no touch if maximum drive and hrs low + // Otherwise apply any gain/drive changes + if (ppgDrive == PPGDrive::Current40mAHigh && previousPpgDrive == PPGDrive::Current40mAHigh && hrs < touchLostThresh) { + actionDelay = noTouchDelay; + ppgState = PPGState::NoTouch; + ppgDrive = PPGDrive::NoTouchPowerSave; + ppgGain = PPGGain::Gain8x; + SetPower(); + } else if (ppgDrive != previousPpgDrive || ppgGain != previousPpgGain) { + actionDelay = powerChangeDelay; + SetPower(); + } + return ppgState; +} diff --git a/src/drivers/Hrs3300.h b/src/drivers/Hrs3300.h index 6f72144847..53c7eea7cc 100644 --- a/src/drivers/Hrs3300.h +++ b/src/drivers/Hrs3300.h @@ -1,6 +1,8 @@ #pragma once #include "drivers/TwiMaster.h" +#include +#include namespace Pinetime { namespace Drivers { @@ -9,17 +11,19 @@ namespace Pinetime { enum class Registers : uint8_t { Id = 0x00, Enable = 0x01, - EnableHen = 0x80, - C1dataM = 0x08, + C1DataM = 0x08, C0DataM = 0x09, C0DataH = 0x0a, PDriver = 0x0c, - C1dataH = 0x0d, - C1dataL = 0x0e, - C0dataL = 0x0f, + C1DataH = 0x0d, + C1DataL = 0x0e, + C0DataL = 0x0f, Res = 0x16, - Hgain = 0x17 + HGain = 0x17 }; + // Reset is not used internally, it exists to inform the heart + // rate task that it should reset its state + enum class PPGState : uint8_t { Off, Running, NoTouch, Reset }; struct PackedHrsAls { uint16_t hrs; @@ -36,13 +40,42 @@ namespace Pinetime { void Enable(); void Disable(); PackedHrsAls ReadHrsAls(); + PPGState AutoGain(uint16_t hrsAvg, uint16_t currentAls); private: + enum class PPGDrive : uint8_t { + NoTouchPowerSave, + // Special registers set + Current13mALow, + Current13mA, + Current20mA, + Current30mA, + Current40mA, + // Special registers set + Current40mAHigh, + }; + enum class PPGGain : uint8_t { Gain1x, Gain2x, Gain4x, Gain8x }; TwiMaster& twiMaster; uint8_t twiAddress; + uint8_t r7f; + uint8_t r80; + uint8_t r81; + uint8_t r82; + PPGDrive ppgDrive; + PPGGain ppgGain; + PPGState ppgState; + + uint8_t actionDelay; + + uint8_t hrsLowCount; + uint8_t hrsHighCount; + uint16_t alsLowCount; + uint8_t alsHighCount; + void WriteRegister(uint8_t reg, uint8_t data); uint8_t ReadRegister(uint8_t reg); + void SetPower(); }; } } diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp index e5457ba304..aeff5d48f7 100644 --- a/src/heartratetask/HeartRateTask.cpp +++ b/src/heartratetask/HeartRateTask.cpp @@ -207,23 +207,42 @@ void HeartRateTask::HandleSensorData() { return; } + // If there are large discontinuities in the heart rate signal, scale the heart rate signal filter + // These discontinuities will be due to sensor gain or drive changes + static constexpr float discontinuityThreshold = 0.2f; + if (lastHrs != 0 && std::abs(static_cast(sensorData.hrs) - static_cast(lastHrs)) > + std::min(lastHrs, sensorData.hrs) * discontinuityThreshold) { + ppg.ScaleHrs(static_cast(sensorData.hrs) / static_cast(lastHrs)); + } + lastHrs = sensorData.hrs; ppg.Ingest(sensorData.hrs, motionValues.x, motionValues.y, motionValues.z); - std::optional bpm = ppg.HeartRate(); - if (bpm.has_value()) { - SendHeartRate(ControllerStates::Ready, bpm.value()); - } else if (ppg.SufficientData()) { - SendHeartRate(ControllerStates::Searching, 0); - } else { - // If there's currently a value shown, don't clear it - // But still update the algorithm state - if (valueCurrentlyShown) { - controller.UpdateState(ControllerStates::NotEnoughData); + auto ppgState = heartRateSensor.AutoGain(sensorData.hrs, sensorData.als); + + std::optional bpm = std::nullopt; + if (ppgState == Drivers::Hrs3300::PPGState::NoTouch) { + SendHeartRate(ControllerStates::NoTouch, 0); + } else if (ppgState == Drivers::Hrs3300::PPGState::Reset) { + ppg.Reset(); + count = 0; + lastHrs = 0; + SendHeartRate(ControllerStates::NotEnoughData, 0); + } else if (ppgState == Drivers::Hrs3300::PPGState::Running) { + bpm = ppg.HeartRate(); + if (bpm.has_value()) { + SendHeartRate(ControllerStates::Ready, bpm.value()); + } else if (ppg.SufficientData()) { + SendHeartRate(ControllerStates::Searching, 0); } else { - SendHeartRate(ControllerStates::NotEnoughData, 0); + // If there's currently a value shown, don't clear it + // But still update the algorithm state + if (valueCurrentlyShown) { + controller.UpdateState(ControllerStates::NotEnoughData); + } else { + SendHeartRate(ControllerStates::NotEnoughData, 0); + } } } - if (bpm.has_value()) { // Maintain constant frequency acquisition in background mode // If the last measurement time is set to the start time, then the next measurement diff --git a/src/heartratetask/HeartRateTask.h b/src/heartratetask/HeartRateTask.h index e148b26d0a..81f36fb8f8 100644 --- a/src/heartratetask/HeartRateTask.h +++ b/src/heartratetask/HeartRateTask.h @@ -43,6 +43,7 @@ namespace Pinetime { bool valueCurrentlyShown; States state = States::Disabled; uint16_t count; + uint16_t lastHrs = 0; Drivers::Hrs3300& heartRateSensor; Controllers::HeartRateController& controller; Controllers::Settings& settings; From fb03e22a694c9b31735f443bd95a446fc849ae34 Mon Sep 17 00:00:00 2001 From: mark9064 <30447455+mark9064@users.noreply.github.com> Date: Fri, 21 Nov 2025 23:49:36 +0000 Subject: [PATCH 3/3] Correct Stopped state handling --- src/heartratetask/HeartRateTask.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp index aeff5d48f7..dc040ab8a5 100644 --- a/src/heartratetask/HeartRateTask.cpp +++ b/src/heartratetask/HeartRateTask.cpp @@ -153,7 +153,6 @@ void HeartRateTask::Work() { break; case Messages::Disable: newState = States::Disabled; - SendHeartRate(ControllerStates::Disabled, 0); break; } } @@ -170,6 +169,10 @@ void HeartRateTask::Work() { } else if ((newState == States::Waiting || newState == States::Disabled) && (state == States::ForegroundMeasuring || state == States::BackgroundMeasuring)) { StopMeasurement(); + controller.UpdateState(ControllerStates::Stopped); + } + if (newState == States::Disabled) { + SendHeartRate(ControllerStates::Disabled, 0); } state = newState; @@ -196,7 +199,6 @@ void HeartRateTask::StartMeasurement() { void HeartRateTask::StopMeasurement() { heartRateSensor.Disable(); ppg.Reset(); - controller.UpdateState(ControllerStates::Stopped); } void HeartRateTask::HandleSensorData() {