Merge branch 'develop' into upstream-dev

1 files changed, 97 insertions, 0 deletions

diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp
new file mode 100644
index 00000000..5a6d2a5e
--- /dev/null
+++ b/src/heartratetask/HeartRateTask.cpp
@@ -0,0 +1,97 @@
+#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;
+          if(measurementStarted) {
+            lastBpm = 0;
+            StartMeasurement();
+          }
+          break;
+        case Messages::StartMeasurement:
+          if(measurementStarted) break;
+          lastBpm = 0;
+          StartMeasurement();
+          measurementStarted = true;
+          break;
+        case Messages::StopMeasurement:
+          if(!measurementStarted) break;
+          StopMeasurement();
+          measurementStarted = false;
+          break;
+      }
+    }
+
+    if (measurementStarted) {
+      auto hrs = heartRateSensor.ReadHrs();
+      ppg.Preprocess(hrs);
+      auto bpm = ppg.HeartRate();
+
+      if (lastBpm == 0 && bpm == 0) controller.Update(Controllers::HeartRateController::States::NotEnoughData, 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();
+  vTaskDelay(100);
+  ppg.SetOffset(static_cast<float>(heartRateSensor.ReadHrs()));
+}
+
+void HeartRateTask::StopMeasurement() {
+  heartRateSensor.Disable();
+  vTaskDelay(100);
+}