27 lines
653 B
C++
27 lines
653 B
C++
/*
|
|
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;
|
|
}
|