First implementation of the HR sensor using 100% foss code (ported from waspos)

1 files changed, 21 insertions, 0 deletions

diff --git a/src/components/heartrate/Ptagc.cpp b/src/components/heartrate/Ptagc.cpp
new file mode 100644
index 00000000..9302af97
--- /dev/null
+++ b/src/components/heartrate/Ptagc.cpp
@@ -0,0 +1,21 @@
+#include <cmath>
+#include "Ptagc.h"
+
+using namespace Pinetime::Controllers;
+
+Ptagc::Ptagc(float start, float decay, float threshold) : peak{start}, decay{decay}, boost{1.0f/decay}, threshold{threshold} {
+
+}
+
+float Ptagc::Step(float spl) {
+  if(std::abs(spl) > peak)
+    peak *= boost;
+  else
+    peak *= decay;
+
+  if((spl > (peak * threshold)) || (spl < (peak * -threshold)))
+    return 0.0f;
+
+  spl = 100.0f * spl / (2.0f * peak);
+  return spl;
+}