71 lines
2.5 KiB
C++
71 lines
2.5 KiB
C++
//
|
|
// Created by david on 15.03.2026.
|
|
//
|
|
|
|
#include "step_detector.h"
|
|
|
|
// TODO: we are hardcoding filter coefficients for 60 Hz
|
|
// TODO: this is tolerable for 50 Hz
|
|
|
|
// TODO: check if we can do with floats instead of doubles
|
|
// (check how much the [already bad] accuracy of filtering suffers)
|
|
|
|
// TODO: in Java, check if delta timestamps effectively match FPS
|
|
// TODO: FPS constant should be passed as argument to C++ (but we keep an FPS define to validate the coefficients)
|
|
|
|
// Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
|
|
static std::vector<double> hpf_taps_b {0.91875845, -4.59379227, 9.18758454, -9.18758454, 4.59379227, -0.91875845};
|
|
static std::vector<double> hpf_taps_a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171};
|
|
static size_t upslope_width = 4;
|
|
const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
|
|
|
|
StepDetector::StepDetector(StepListener *listener, bool debug) :
|
|
listener(listener),
|
|
f_highpass(hpf_taps_b, hpf_taps_a),
|
|
f_neg(1, 0, 0, std::vector<double> {-1.0}),
|
|
f_ssf(upslope_width),
|
|
f_ssd(len_refr),
|
|
f_sqi(upslope_width),
|
|
debug(debug)
|
|
{}
|
|
|
|
#if (FPS != 60)
|
|
#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
|
|
#endif
|
|
|
|
void StepDetector::filter(std::vector<float> values) {
|
|
// TODO: later on, we should use a vector projection towards gravity
|
|
auto s0 = (double) values[1]; // take y-axis value for now
|
|
auto s1 = f_highpass.filter(s0);
|
|
auto s2 = f_neg.filter(s1);
|
|
auto s3 = f_ssf.filter(s2);
|
|
auto s4 = f_ssd.filter(s3);
|
|
auto q5 = f_sqi.filter(s2, s3, s4);
|
|
if (debug) {
|
|
buf_ssd.push_back(s4);
|
|
buf_sqi.push_back(q5);
|
|
buf_out.push_back(s4 * (q5 > 0.0 ? 1.0 : 0.0));
|
|
}
|
|
// is step, step quality is OK, and we have a listener?
|
|
if(s4 > 0.0 && q5 > 0.0 && listener != nullptr) {
|
|
listener->playBeat();
|
|
}
|
|
}
|
|
|
|
std::vector<double> StepDetector::getBufSsd() { return buf_ssd; }
|
|
std::vector<double> StepDetector::getBufSqi() { return buf_sqi; }
|
|
std::vector<double> StepDetector::getBufOut() { return buf_out; }
|
|
|
|
void StepDetector::primeFilters(std::vector<double> sig) {
|
|
const size_t N_INIT = SsfStepDetector::initial_samples();
|
|
// initialize: feed for priming the filters
|
|
for (size_t i = 0; i < N_INIT; i++) {
|
|
const auto a_i = static_cast<float>(sig[i]);
|
|
filter(std::vector<float> {0.0f, a_i, 0.0f});
|
|
}
|
|
// clear debug buffers
|
|
buf_ssd.clear();
|
|
buf_sqi.clear();
|
|
buf_out.clear();
|
|
}
|