Skip to content

Ring Buffer for Heart Rate Data #1092

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
watertrainer wants to merge 8 commits into from
Closed

Ring Buffer for Heart Rate Data #1092

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
979f0f9
replace magic number 200 with const DATA_SIZE
watertrainer Apr 5, 2022
254fb84
ring buffer impl for processing of hr sensor data
watertrainer Apr 14, 2022
c59f9b5
variable to change the freq of heart rate update
watertrainer Apr 15, 2022
efde7e2
Merge branch 'InfiniTimeOrg:develop' into heartrate-ring-buffer
watertrainer Apr 15, 2022
ef84a22
some clang-tidy cleanups/autoformat
watertrainer Apr 15, 2022
c6d7673
replaced DATA_SIZE variable with data.size()
watertrainer Apr 25, 2022
faf2faa
removed unnecessary parenthesis
watertrainer Apr 25, 2022
6fb413a
introduced boolean, if data was filled up at least once
watertrainer Apr 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
110 changes: 60 additions & 50 deletions src/components/heartrate/Ppg.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,45 +5,42 @@
*/

#include "components/heartrate/Ppg.h"
#include <vector>
#include <nrf_log.h>
#include <vector>
using namespace Pinetime::Controllers;

/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */
namespace {
int Compare(int8_t* d1, int8_t* d2, size_t count) {
int e = 0;
for (size_t i = 0; i < count; i++) {
auto d = d1[i] - d2[i];
e += d * d;
}
return e;
}

int CompareShift(int8_t* d, int shift, size_t count) {
return Compare(d + shift, d, count - shift);
}

int Trough(int8_t* d, size_t size, uint8_t mn, uint8_t 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()
: 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::Compare(int8_t* d1, int shift, size_t count) {
Copy link
Copy Markdown
Contributor

@Riksu9000 Riksu9000 Apr 19, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why was this function changed, apart from adding getRingIndex()?

Copy link
Copy Markdown
Author

@watertrainer watertrainer Apr 19, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

in the original implementation the pointer was moved forward and then accessed as a 0-based array. That wouldn't work in the ring buffer, because the actual index has to be known to modulo it.

int e = 0;
for (size_t i = 0; i < count; i++) {
auto d = d1[getRingIndex(i + shift)] - d1[getRingIndex(i)];
e += d * d;
}
return e;
}

int Ppg::CompareShift(int8_t* d, int shift, size_t count) {
return Compare(d, shift, count - shift);
}
int Ppg::Trough(int8_t* d, size_t size, uint8_t mn, uint8_t 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;
}

int8_t Ppg::Preprocess(float spl) {
spl -= offset;
spl = hpf.Step(spl);
Expand All @@ -52,48 +49,61 @@ int8_t Ppg::Preprocess(float spl) {

auto spl_int = static_cast<int8_t>(spl);

if (dataIndex < 200)
data[dataIndex++] = spl_int;
data[dataIndex] = spl_int;
dataIndex = (dataIndex + 1) % Ppg::data.size();
if (dataIndex == 0) {
dataReady = true;
}
return spl_int;
}

float Ppg::HeartRate() {
if (dataIndex < 200)
int Ppg::HeartRate() {
if (!dataReady || dataIndex % Ppg::UPDATE_HEARTRATE_AFTER != 0) {
return 0;
}

NRF_LOG_INFO("PREPROCESS, offset = %d", offset);
auto hr = ProcessHeartRate();
dataIndex = 0;
int hr = ProcessHeartRate();
return hr;
}
float Ppg::ProcessHeartRate() {
auto t0 = Trough(data.data(), dataIndex, 7, 48);
if (t0 < 0)
int Ppg::ProcessHeartRate() {
int t0 = Trough(data.data(), data.size(), 7, 48);
if (t0 < 0) {
return 0;
}

float t1 = t0 * 2;
t1 = Trough(data.data(), dataIndex, t1 - 5, t1 + 5);
if (t1 < 0)
int t1 = t0 * 2;
t1 = Trough(data.data(), data.size(), 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)
int t2 = (t1 * 3) / 2;
t2 = Trough(data.data(), data.size(), 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);
int t3 = (t2 * 4) / 3;
t3 = Trough(data.data(), data.size(), t3 - 4, t3 + 4);
if (t3 < 0) {
return (60 * 24 * 3) / t2;
}

return static_cast<int>(60 * 24 * 4) / static_cast<int>(t3);
return (60 * 24 * 4) / t3;
}

// Gets the Index in the Ring Buffer which corresponds to the index, if it was a 0-based Array
// dataIndex points to the next index to write to, so it is the start of the buffer and the last element is at dataIndex-1
int Ppg::getRingIndex(int8_t index) {
return (index + dataIndex) % data.size();
}
void Ppg::SetOffset(uint16_t offset) {
this->offset = offset;
dataIndex = 0;
this->Reset();
}

void Ppg::Reset() {
dataIndex = 0;
dataReady = false;
}
10 changes: 8 additions & 2 deletions src/components/heartrate/Ppg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,20 +12,26 @@ namespace Pinetime {
public:
Ppg();
int8_t Preprocess(float spl);
float HeartRate();
int HeartRate();

void SetOffset(uint16_t i);
void Reset();

private:
static const uint8_t UPDATE_HEARTRATE_AFTER = 200;
std::array<int8_t, 200> data;
size_t dataIndex = 0;
bool dataReady = false;
float offset;
Biquad hpf;
Ptagc agc;
Biquad lpf;

float ProcessHeartRate();
int getRingIndex(int8_t index);
int Compare(int8_t* d1, int shift, size_t count);
int CompareShift(int8_t* d, int shift, size_t count);
int Trough(int8_t* d, size_t size, uint8_t mn, uint8_t mx);
int ProcessHeartRate();
};
}
}