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

2 files changed, 132 insertions, 0 deletions

diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp
new file mode 100644
index 0000000..5ce2745
--- /dev/null
+++ b/src/heartratetask/HeartRateTask.cpp
@@ -0,0 +1,89 @@
+#include "HeartRateTask.h"
+#include <drivers/Hrs3300.h>
+#include <components/heartrate/HeartRateController.h>
+#include <nrf_log.h>
+
+using namespace Pinetime::Applications;
+
+HeartRateTask::HeartRateTask(Drivers::Hrs3300 &heartRateSensor, Controllers::HeartRateController& controller) :
+      heartRateSensor{heartRateSensor},
+      controller{controller},
+      ppg{static_cast<float>(heartRateSensor.ReadHrs())} {
+  messageQueue = xQueueCreate(10, 1);
+  controller.SetHeartRateTask(this);
+}
+
+void HeartRateTask::Start() {
+  if (pdPASS != xTaskCreate(HeartRateTask::Process, "Heartrate", 500, this, 0, &taskHandle))
+    APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
+}
+
+void HeartRateTask::Process(void *instance) {
+  auto *app = static_cast<HeartRateTask *>(instance);
+  app->Work();
+}
+
+void HeartRateTask::Work() {
+  int lastBpm = 0;
+  while (true) {
+    Messages msg;
+    uint32_t delay;
+    if (state == States::Running) {
+      if (measurementStarted) delay = 40;
+      else delay = 100;
+    } else
+      delay = portMAX_DELAY;
+
+    if (xQueueReceive(messageQueue, &msg, delay)) {
+      switch (msg) {
+        case Messages::GoToSleep:
+          StopMeasurement();
+          state = States::Idle;
+          break;
+        case Messages::WakeUp:
+          state = States::Running;
+          break;
+        case Messages::StartMeasurement:
+          lastBpm = 0;
+          StartMeasurement();
+          ppg.SetOffset(heartRateSensor.ReadHrs());
+          break;
+        case Messages::StopMeasurement:
+          StopMeasurement();
+          break;
+      }
+    }
+
+    if (measurementStarted) {
+      auto hrs = heartRateSensor.ReadHrs();
+      ppg.Preprocess(hrs);
+      auto bpm = ppg.HeartRate();
+
+      if (lastBpm == 0 && bpm == 0) controller.Update(Controllers::HeartRateController::States::NoTouch, 0);
+      if(bpm != 0) {
+        lastBpm = bpm;
+        controller.Update(Controllers::HeartRateController::States::Running, lastBpm);
+      }
+    }
+  }
+}
+
+void HeartRateTask::PushMessage(HeartRateTask::Messages msg) {
+  BaseType_t xHigherPriorityTaskWoken;
+  xHigherPriorityTaskWoken = pdFALSE;
+  xQueueSendFromISR(messageQueue, &msg, &xHigherPriorityTaskWoken);
+  if (xHigherPriorityTaskWoken) {
+    /* Actual macro used here is port specific. */
+    // TODO : should I do something here?
+  }
+}
+
+void HeartRateTask::StartMeasurement() {
+  heartRateSensor.Enable();
+  measurementStarted = true;
+}
+
+void HeartRateTask::StopMeasurement() {
+  heartRateSensor.Disable();
+  measurementStarted = false;
+}
diff --git a/src/heartratetask/HeartRateTask.h b/src/heartratetask/HeartRateTask.h
new file mode 100644
index 0000000..f142af1
--- /dev/null
+++ b/src/heartratetask/HeartRateTask.h
@@ -0,0 +1,43 @@
+#pragma once
+#include <FreeRTOS.h>
+#include <task.h>
+#include <queue.h>
+#include <components/heartrate/Ppg.h>
+
+
+namespace Pinetime {
+  namespace Drivers {
+    class Hrs3300;
+  }
+  namespace Controllers{
+    class HeartRateController;
+  }
+  namespace Applications {
+    class HeartRateTask {
+    public:
+      enum class Messages : uint8_t {GoToSleep, WakeUp, StartMeasurement, StopMeasurement };
+      enum class States {Idle, Running};
+
+      explicit HeartRateTask(Drivers::Hrs3300& heartRateSensor, Controllers::HeartRateController& controller);
+      void Start();
+      void Work();
+      void PushMessage(Messages msg);
+
+    private:
+      static void Process(void* instance);
+      void StartMeasurement();
+      void StopMeasurement();
+
+      TaskHandle_t taskHandle;
+      QueueHandle_t messageQueue;
+      States state = States::Running;
+      Drivers::Hrs3300 &heartRateSensor;
+      Controllers::HeartRateController& controller;
+      Controllers::Ppg ppg;
+      bool measurementStarted = false;
+
+    };
+
+  }
+}
+