diff options
Diffstat (limited to 'src/components/heartrate')
-rw-r--r-- | src/components/heartrate/Biquad.cpp | 27 | ||||
-rw-r--r-- | src/components/heartrate/Biquad.h | 22 | ||||
-rw-r--r-- | src/components/heartrate/HeartRateController.cpp | 41 | ||||
-rw-r--r-- | src/components/heartrate/HeartRateController.h | 38 | ||||
-rw-r--r-- | src/components/heartrate/Ppg.cpp | 106 | ||||
-rw-r--r-- | src/components/heartrate/Ppg.h | 31 | ||||
-rw-r--r-- | src/components/heartrate/Ptagc.cpp | 28 | ||||
-rw-r--r-- | src/components/heartrate/Ptagc.h | 18 |
8 files changed, 311 insertions, 0 deletions
diff --git a/src/components/heartrate/Biquad.cpp b/src/components/heartrate/Biquad.cpp new file mode 100644 index 00000000..6a4b8181 --- /dev/null +++ b/src/components/heartrate/Biquad.cpp @@ -0,0 +1,27 @@ +/* + SPDX-License-Identifier: LGPL-3.0-or-later + Original work Copyright (C) 2020 Daniel Thompson + C++ port Copyright (C) 2021 Jean-François Milants +*/ + +#include "Biquad.h" + +using namespace Pinetime::Controllers; + +/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */ +Biquad::Biquad(float b0, float b1, float b2, float a1, float a2) : b0{b0}, b1{b1}, b2{b2}, a1{a1}, a2{a2} { + +} + +float Biquad::Step(float x) { + auto v1 = this->v1; + auto v2 = this->v2; + + auto v = x - (a1 * v1) - (a2 * v2); + auto y = (b0 * v) + (b1 * v1) + (b2 * v2); + + this->v2 = v1; + this->v1 = v; + + return y; +} diff --git a/src/components/heartrate/Biquad.h b/src/components/heartrate/Biquad.h new file mode 100644 index 00000000..dc9b97f6 --- /dev/null +++ b/src/components/heartrate/Biquad.h @@ -0,0 +1,22 @@ +#pragma once + +namespace Pinetime { + namespace Controllers { + /// Direct Form II Biquad Filter + class Biquad { + public: + Biquad(float b0, float b1, float b2, float a1, float a2); + float Step(float x); + + private: + float b0; + float b1; + float b2; + float a1; + float a2; + + float v1 = 0.0f; + float v2 = 0.0f; + }; + } +} diff --git a/src/components/heartrate/HeartRateController.cpp b/src/components/heartrate/HeartRateController.cpp new file mode 100644 index 00000000..d0b0d513 --- /dev/null +++ b/src/components/heartrate/HeartRateController.cpp @@ -0,0 +1,41 @@ +#include "HeartRateController.h" +#include <heartratetask/HeartRateTask.h> +#include <systemtask/SystemTask.h> + +using namespace Pinetime::Controllers; + +HeartRateController::HeartRateController(Pinetime::System::SystemTask &systemTask) : systemTask{systemTask} { + +} + + +void HeartRateController::Update(HeartRateController::States newState, uint8_t heartRate) { + this->state = newState; + if(this->heartRate != heartRate) { + this->heartRate = heartRate; + service->OnNewHeartRateValue(heartRate); + } +} + +void HeartRateController::Start() { + if(task != nullptr) { + state = States::NotEnoughData; + task->PushMessage(Pinetime::Applications::HeartRateTask::Messages::StartMeasurement); + } +} + +void HeartRateController::Stop() { + if(task != nullptr) { + state = States::Stopped; + task->PushMessage(Pinetime::Applications::HeartRateTask::Messages::StopMeasurement); + } +} + +void HeartRateController::SetHeartRateTask(Pinetime::Applications::HeartRateTask *task) { + this->task = task; +} + +void HeartRateController::SetService(Pinetime::Controllers::HeartRateService *service) { + this->service = service; +} + diff --git a/src/components/heartrate/HeartRateController.h b/src/components/heartrate/HeartRateController.h new file mode 100644 index 00000000..001111b5 --- /dev/null +++ b/src/components/heartrate/HeartRateController.h @@ -0,0 +1,38 @@ +#pragma once + +#include <cstdint> +#include <components/ble/HeartRateService.h> + +namespace Pinetime { + namespace Applications { + class HeartRateTask; + } + namespace System { + class SystemTask; + } + namespace Controllers { + class HeartRateController { + public: + enum class States { Stopped, NotEnoughData, NoTouch, Running}; + + explicit HeartRateController(System::SystemTask& systemTask); + + void Start(); + void Stop(); + void Update(States newState, uint8_t heartRate); + + void SetHeartRateTask(Applications::HeartRateTask* task); + States State() const { return state; } + uint8_t HeartRate() const { return heartRate; } + + void SetService(Pinetime::Controllers::HeartRateService *service); + + private: + System::SystemTask& systemTask; + Applications::HeartRateTask* task = nullptr; + States state = States::Stopped; + uint8_t heartRate = 0; + Pinetime::Controllers::HeartRateService* service = nullptr; + }; + } +}
\ No newline at end of file diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp new file mode 100644 index 00000000..233c3003 --- /dev/null +++ b/src/components/heartrate/Ppg.cpp @@ -0,0 +1,106 @@ +/* + SPDX-License-Identifier: LGPL-3.0-or-later + Original work Copyright (C) 2020 Daniel Thompson + C++ port Copyright (C) 2021 Jean-François Milants +*/ + +#include <vector> +#include <nrf_log.h> +#include "Ppg.h" +using namespace Pinetime::Controllers; + +/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */ +namespace { + int Compare(int* d1, int* d2, size_t count) { + int e = 0; + for(int i = 0; i < count; i++) { + auto d = d1[i] - d2[i]; + e += d * d; + } + return e; + } + + int CompareShift(int* d, int shift, size_t count) { + return Compare(d +shift, d, count - shift); + } + + int Trough(int* d, size_t size, float mn, float mx) { + auto z2 = CompareShift(d, mn-2, size); + auto z1 = CompareShift(d, mn-1, size); + for(int i = mn; i < mx + 1; i++) { + auto z = CompareShift(d, i, size); + if(z2 > z1 && z1 < z) + return i; + z2 = z1; + z1 = z; + } + return -1; + } +} + +Ppg::Ppg(float spl) : offset{spl}, + hpf{0.87033078, -1.74066156, 0.87033078,-1.72377617, 0.75754694}, + agc{20, 0.971, 2}, + lpf{0.11595249, 0.23190498, 0.11595249,-0.72168143, 0.18549138} { + +} + +int Ppg::Preprocess(float spl) { + spl -= offset; + spl = hpf.Step(spl); + spl = agc.Step(spl); + spl = lpf.Step(spl); + + auto spl_int = static_cast<int>(spl); + + if(dataIndex < 200) + data[dataIndex++] = spl_int; + return spl_int; +} + +float Ppg::HeartRate() { + if(dataIndex < 200) + return 0; + + NRF_LOG_INFO("PREPROCESS, offset = %d", offset); + auto hr = ProcessHeartRate(); + dataIndex = 0; + return hr; +} + +int cccount = 0; +float Ppg::ProcessHeartRate() { + + if(cccount > 2) + asm("nop"); + cccount ++; + auto t0 = Trough(data.data(), dataIndex, 7, 48); + if(t0 < 0) + return 0; + + float t1 = t0 * 2; + t1 = Trough(data.data(), dataIndex, t1-5, t1+5); + if(t1 < 0) + return 0; + + float t2 = static_cast<int>(t1 * 3) / 2; + t2 = Trough(data.data(), dataIndex, t2 - 5, t2 + 5); + if(t2 < 0) + return 0; + + float t3 = static_cast<int>(t2 * 4) / 3; + t3 = Trough(data.data(), dataIndex, t3 - 4, t3 + 4); + if(t3 < 0) + return static_cast<int>(60 * 24 * 3) / static_cast<int>(t2); + + return static_cast<int>(60 * 24 * 4) / static_cast<int>(t3); +} + +void Ppg::SetOffset(uint16_t offset) { + this->offset = offset; + dataIndex = 0; +} + +void Ppg::Reset() { + dataIndex = 0; +} diff --git a/src/components/heartrate/Ppg.h b/src/components/heartrate/Ppg.h new file mode 100644 index 00000000..747ae019 --- /dev/null +++ b/src/components/heartrate/Ppg.h @@ -0,0 +1,31 @@ +#pragma once + +#include <array> +#include "Biquad.h" +#include "Ptagc.h" + +namespace Pinetime { + namespace Controllers { + class Ppg { + public: + explicit Ppg(float spl); + + int Preprocess(float spl); + float HeartRate(); + + void SetOffset(uint16_t i); + void Reset(); + + private: + std::array<int, 200> data; + size_t dataIndex = 0; + float offset; + Biquad hpf; + Ptagc agc; + Biquad lpf; + + + float ProcessHeartRate(); + }; + } +}
\ No newline at end of file diff --git a/src/components/heartrate/Ptagc.cpp b/src/components/heartrate/Ptagc.cpp new file mode 100644 index 00000000..dd7c4411 --- /dev/null +++ b/src/components/heartrate/Ptagc.cpp @@ -0,0 +1,28 @@ +/* + SPDX-License-Identifier: LGPL-3.0-or-later + Original work Copyright (C) 2020 Daniel Thompson + C++ port Copyright (C) 2021 Jean-François Milants +*/ + +#include <cmath> +#include "Ptagc.h" + +using namespace Pinetime::Controllers; + +/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */ +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; +} diff --git a/src/components/heartrate/Ptagc.h b/src/components/heartrate/Ptagc.h new file mode 100644 index 00000000..c20de4c0 --- /dev/null +++ b/src/components/heartrate/Ptagc.h @@ -0,0 +1,18 @@ +#pragma once + +namespace Pinetime { + namespace Controllers { + class Ptagc { + public: + Ptagc(float start, float decay, float threshold); + float Step(float spl); + + private: + float peak; + float decay; + float boost; + float threshold; + + }; + } +} |