diff options
Diffstat (limited to 'src/components/heartrate/Biquad.cpp')
-rw-r--r-- | src/components/heartrate/Biquad.cpp | 27 |
1 files changed, 27 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; +} |