PPGv2

Author: mark9064

.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

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

src/components/heartrate/FFT.h

@@ -0,0 +1,125 @@
+#include <complex>
+#include <array>
+#include <bit>
+#include <numbers>
+
+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 <std::size_t N>
+      static consteval std::array<std::complex<float>, IntegerLog2(N)> GenComplexTwiddle() {
+        using namespace std::complex_literals;
+
+        std::array<std::complex<float>, IntegerLog2(N)> result;
+        for (std::size_t i = 0; i < IntegerLog2(N); i++) {
+          result[i] = exp_consteval(-2.i * std::numbers::pi / static_cast<double>(1 << (i + 1)));
+        }
+        return result;
+      }
+
+      template <std::size_t N>
+      static consteval std::array<std::complex<float>, (N / 4) - 1> GenRealTwiddle() {
+        using namespace std::complex_literals;
+
+        std::array<std::complex<float>, (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<double>(i + 1) / static_cast<double>(N));
+        }
+        return result;
+      }
+
+      template <std::size_t N>
+      static void ComplexFFT(std::array<std::complex<float>, N>& array, const std::array<std::complex<float>, 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<float> omega = 1.f;
+            for (std::size_t j = 0; j < m / 2; j++) {
+              std::complex<float> t = omega * array[k + j + (m / 2)];
+              std::complex<float> u = array[k + j];
+              array[k + j] = u + t;
+              array[k + j + (m / 2)] = u - t;
+              omega *= twiddle[s - 1];
+            }
+          }
+        }
+      }
+
+      template <std::size_t N>
+      static void RealFFT(std::array<std::complex<float>, N>& array,
+                          const std::array<std::complex<float>, (N / 2) - 1>& realTwiddle,
+                          const std::array<std::complex<float>, 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<float> xeLo = (array[index] + std::conj(array[indexInv])) / 2.f;
+          std::complex<float> xoLo = -1.if * ((array[index] - std::conj(array[indexInv])) / 2.f);
+          std::complex<float> xeHi = (array[indexInv] + std::conj(array[index])) / 2.f;
+          std::complex<float> 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 <typename _Tp>
+      static consteval std::complex<_Tp> exp_consteval(const std::complex<_Tp>& __z) {
+        return polar_consteval<_Tp>(__builtin_exp(__z.real()), __z.imag());
+      }
+
+      template <typename _Tp>
+      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 <class T, std::size_t N>
+      static void InplaceBitReverse(std::array<T, N>& 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;
+        }
+      }
+    };
+  }
+}

src/components/heartrate/HeartRateController.cpp

@@ -1,11 +1,15 @@
 #include "components/heartrate/HeartRateController.h"
-#include <heartratetask/HeartRateTask.h>
-#include <systemtask/SystemTask.h>
+
+#include <cstdint>
+#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);
   }
 }

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;
     };

src/components/heartrate/IIR.h

@@ -0,0 +1,49 @@
+#include <array>
+
+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 <std::size_t NSections>
+    class IIRFilter {
+    public:
+      IIRFilter(const std::array<SOSCoeffs, NSections>& coeffs, const std::array<FilterState, NSections>& 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<FilterState, NSections> filterStates;
+      const std::array<SOSCoeffs, NSections>& coeffs;
+      const std::array<FilterState, NSections>& zi;
+    };
+  }
+}