feat: StepDetector update FPS, more FPS fixes

google-tests/CMakeLists.txt

@@ -15,6 +15,7 @@ add_executable(Google_Tests_run
         test3.cpp
         test4.cpp
         test5.cpp
+        test6.cpp
 )
 
 file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)

google-tests/test1.cpp

@@ -2,7 +2,6 @@
 // Created by david on 28.02.2026.
 //
 #include <gtest/gtest.h>
-#include "library.h"
 #include "iir_filter.h"
 #include "npy.hpp"
 #include <utility>
@@ -10,16 +9,6 @@
 #include <string>
 #include <filesystem>
 
-// Demonstrate some basic assertions.
-TEST(HelloTest, BasicAssertions) {
-    // Expect two strings not to be equal.
-    EXPECT_STRNE("hello", "world from test1.cpp");
-    // Expect equality.
-    EXPECT_EQ(7 * 6, 42);
-    printf("asdf");
-    hello();
-}
-
 TEST(HelloTest, Load_npy_matrix) {
     // "C:\\Users\\david\\Documents\\src\\libpasada\\cmake-build-debug\\google-tests"
     std::cout <<   std::filesystem::current_path() << std::endl;

google-tests/test2.cpp

@@ -79,7 +79,7 @@ TEST(HelloTest, Filter_Delta_U) {
 
 // NOTE: later SSF must be fed -u, not u
 TEST(HelloTest, Filter_SSF) {
-    SsfFilter f_ssf(3);
+    SsfFilter f_ssf(FPS * 3/4); // target upslope_width = 3
     std::vector x { 1.0, 3.0,  2.0, 5.0,  1.0, 1.5 };
     //         du { 1.0, 2.0, -1.0, 3.0, -4.0, 0.5 }
     //        duc { 1.0, 2.0,  0.0, 3.0,  0.0, 0.5 }
@@ -116,8 +116,7 @@ TEST(HelloTest, Zong_SSF_Stage2) {
     auto y_neg = apply_filter(f_neg, y);
 
     // Stage 2: sum slope function
-    const size_t upslope_width = 4;
-    SsfFilter f_ssf(upslope_width);
+    SsfFilter f_ssf(FPS);
     auto ssf = apply_filter(f_ssf, y_neg);
 
     npy_save("test2/ssf_t2_ssf.npy", ssf);
@@ -148,22 +147,20 @@ TEST(HelloTest, Zong_SSF_Stage3) {
     //std::cerr << "before stage 2" << std::endl;
 
     // Stage 2: sum slope function
-    const size_t upslope_width = 4;
-    SsfFilter f_ssf(upslope_width);
+    SsfFilter f_ssf(FPS);
     auto ssf = apply_filter(f_ssf, y_neg);
 
     //std::cerr << "before stage 3" << std::endl;
 
     // Stage 3: threshold detection
-    const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
-    DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
+    DebugSsfStepDetectorThreshold f_ssd_thr(FPS);
     auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
 
     //std::cerr << "before writing results 1 and doing step detection" << std::endl;
 
     npy_save("test2/ssf_t2_ssf_threshold.npy", ssf_threshold);
 
-    SsfStepDetector f_ssd(len_refr);
+    SsfStepDetector f_ssd(FPS);
     auto steps = apply_filter(f_ssd, ssf);
 
     //std::cerr << "before writing results 2" << std::endl;

google-tests/test3.cpp

@@ -94,8 +94,8 @@ protected:
     std::vector<bool> goods;
 
 public:
-    DebugRunningQuality(): lockedAt(-1), locked(false) {}
-    explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
+    DebugRunningQuality(double fps): RunningQuality(fps), lockedAt(-1), locked(false) {}
+    explicit DebugRunningQuality(double fps, bool disableSsf): RunningQuality(fps, disableSsf), locked(false) {}
     virtual ~DebugRunningQuality() {}
     bool isLocked() { return locked; }
     std::vector<double> getCorrs() { return corrs; }
@@ -126,7 +126,8 @@ TEST(SignalTest, resample_same_len) {
 */
 
 TEST(SignalTest, RunningQuality_t1) {
-    DebugRunningQuality sqi(true);
+    double fps = 60.0;
+    DebugRunningQuality sqi(fps, true);
     std::vector a {0.0, 0.3, 0.9, 1.0, 0.7, 0.5, 0.1};
     std::vector b {0.0, 0.3, 0.9, 1.0, 0.5, 0.5, 0.1};
     std::vector c {0.0, 0.3, 0.9, 1.0, 0.9, 0.5, 0.1};
@@ -151,9 +152,7 @@ TEST(SignalTest, RunningQuality_t2) {
 
     std::vector<double> signal = fetch_y_axis(acc);
 
-#if (FPS != 60)
-#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
-#endif
+    double fps = 60.0;
 
     // TODO: SQI: cehck input file
     // TODO: SQI: print debug values corr,idx, checkedSsf
@@ -174,22 +173,20 @@ TEST(SignalTest, RunningQuality_t2) {
     //std::cerr << "before stage 2" << std::endl;
 
     // Stage 2: sum slope function
-    const size_t upslope_width = 4;
-    SsfFilter f_ssf(upslope_width);
+    SsfFilter f_ssf(fps);
     auto ssf = apply_filter(f_ssf, y_neg);
 
     //std::cerr << "before stage 3" << std::endl;
 
     // Stage 3: threshold detection
-    const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
-    DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
+    DebugSsfStepDetectorThreshold f_ssd_thr(fps);
     auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
 
     //std::cerr << "before writing results 1 and doing step detection" << std::endl;
 
     npy_save("test2/ssf_t3_ssf_threshold.npy", ssf_threshold);
 
-    SsfStepDetector f_ssd(len_refr);
+    SsfStepDetector f_ssd(fps);
     auto steps = apply_filter(f_ssd, ssf);
 
     //std::cerr << "before writing results 2" << std::endl;
@@ -198,7 +195,7 @@ TEST(SignalTest, RunningQuality_t2) {
 
     // Debug SQI
 
-    DebugRunningQuality sqi;
+    DebugRunningQuality sqi(fps);
 
     std::vector<double> beat_buf;
     std::vector<double> ssf_buf;

google-tests/test4.cpp

@@ -11,19 +11,22 @@
 TEST(StepDetector, t1_sub_sample_resolution) {
     npy::npy_data s = npy::read_npy<double>("test4/step_150a.npy");
 
+    double fps = 60.0;
+
     std::vector<double> signal = fetch_y_axis(s);
     const size_t N = signal.size();
-    const size_t N_INIT = SsfStepDetector::initial_samples();
+    const size_t N_INIT = SsfStepDetector::initial_samples(fps);
 
-    StepDetector det(nullptr, true);
+    StepDetector det(fps, nullptr, true);
 
     // initialize: feed for priming the filters
-    det.primeFilters(signal);
+    double ts = det.primeFilters(fps, signal);
 
     // feed for actual test
     for (size_t i = 0; i < N; i++) {
         const auto a_i = static_cast<float>(signal[i]);
-        det.filter(std::vector<float> {0.0f, a_i, 0.0f});
+        det.filter(ts * 1e9, std::vector<float> {0.0f, a_i, 0.0f});
+        ts += 1.0 / fps;
     }
 
     std::vector<double> ssd = det.getBufSsd(); // raw SsfStepDetector

google-tests/test5.cpp

@@ -19,9 +19,7 @@ TEST(HelloTest, Zong_SSF_Test5_a1) {
 
     std::vector<double> signal = fetch_y_axis(acc, 2); // (ts, x, y, z) entries -> fetch 'y'
 
-#if (FPS != 60)
-#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
-#endif
+    double fps = 60.0;
 
     // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
     std::vector b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
@@ -40,15 +38,13 @@ TEST(HelloTest, Zong_SSF_Test5_a1) {
     //std::cerr << "before stage 2" << std::endl;
 
     // Stage 2: sum slope function
-    const size_t upslope_width = 4;
-    SsfFilter f_ssf(upslope_width);
+    SsfFilter f_ssf(fps);
     auto ssf = apply_filter(f_ssf, y_neg);
 
     //std::cerr << "before stage 3" << std::endl;
 
     // Stage 3: threshold detection
-    const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
-    DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
+    DebugSsfStepDetectorThreshold f_ssd_thr(fps);
     auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
 
     //std::cerr << "before writing results 1 and doing step detection" << std::endl;
@@ -57,7 +53,7 @@ TEST(HelloTest, Zong_SSF_Test5_a1) {
     npy_save("test5/ssf_a1_ssf.npy", ssf);
     npy_save("test5/ssf_a1_ssf_threshold.npy", ssf_threshold);
 
-    SsfStepDetector f_ssd(len_refr);
+    SsfStepDetector f_ssd(fps);
     auto steps = apply_filter(f_ssd, ssf);
 
     //std::cerr << "before writing results 2" << std::endl;

google-tests/test6.cpp

@@ -0,0 +1,122 @@
+//
+// Created by david on 19.05.2026.
+//
+
+#include <filesystem>
+#include <numeric>
+#include <random>
+#include <gtest/gtest.h>
+#include "pd_resamp.h"
+#include "pd_signal.h"
+#include "test_helpers.h"
+
+#define M_PI 3.14159265358979323846
+
+void make_test_signal_1(int N, std::vector<double> &ts, std::vector<double> &sig) {
+    double f = 10.0;
+    double fs = 100.0;
+    pd_signal::linspace(ts, 0.0, (N-1) / fs * 1e9, N, false);
+    sig.resize(N);
+    for (int i = 0; i < N; i++) {
+        sig[i] = std::cos(2 * M_PI * f * i / fs);
+    }
+}
+
+void add_noise(std::vector<double>& x, double mu, double sigma) {
+    if (sigma < 0.0) { throw std::invalid_argument("sigma must be non-negative"); }
+    std::mt19937 rng { 42 }; /* std::random_device{}() */
+    std::normal_distribution<double> dist(mu, sigma);
+    for (double& v : x) {
+        v += dist(rng);
+    }
+}
+
+int make_spiky_times(int N_hint, std::vector<double>& ts_in, std::vector<double>& ts_out, std::vector<double> &sig_in, std::vector<double> &sig_out) {
+    double fs = 100.0;
+    // note that resulting indices will be 100 + halfdist, because of sampling rate change at i=100  =>  120, 139, 149
+    std::vector<int> spikes {140, 178, 198};
+    std::vector<double> rel_spikes { 1.8, 5.6, 2.51 };
+    int N = N_hint + static_cast<int>(std::accumulate(rel_spikes.begin(), rel_spikes.end(), 0) + 1.0);
+
+    // at certain indices, add a larger time spike
+    ts_out.resize(ts_in.size());
+    std::ranges::copy(ts_in, ts_out.begin());
+    for (int i = 0; i < spikes.size(); i++) {
+        int i_spike = spikes[i];
+        double dt_spike = (rel_spikes[i] - 1.0) * 1.0 / fs * 1e9;
+        for (int j = i_spike; j < N_hint; j++) {
+            ts_out[j] += dt_spike;
+        }
+    }
+
+    // add gaussian noise to times
+    add_noise(ts_out, 0.0, 0.01 / fs * 1e9);
+    std::ranges::sort(ts_out); // make sure they remain sorted
+
+    // reduce sampling rate in second half
+    for (int i = 100; i < 100 + (N_hint - 100) / 2; i++) {
+        ts_out[i] = ts_out[100 + (i-100)*2];
+    }
+    ts_out.resize(100 + (N_hint - 100) / 2);
+
+    // compute signal at times
+    pd_signal::interp(sig_out, ts_out, ts_in, sig_in);
+
+    return N;
+}
+
+TEST(HelloTest, Resampler_Test1) {
+    std::vector<double> ts_orig, ts_spiky;
+    std::vector<double> a_orig, a_spiky;
+    std::vector<double> sig_res;
+
+    make_test_signal_1(207, ts_orig, a_orig); // N = 200+sum(rel_spikes)-len(rel_spikes)
+    double fs = 1e9 / (ts_orig[1]-ts_orig[0]);
+    //std::cout << "fs=" << fs << std::endl;
+    make_spiky_times(200, ts_orig, ts_spiky, a_orig, a_spiky);
+
+    Resampler res;
+
+    const int INITIAL_SAMPLES = 100; // Resampler.INITIAL_SAMPLES;
+    int i;
+    // push - initial samples are buffered
+    for (i = 0; i < INITIAL_SAMPLES - 1; i++) {
+        res.push(ts_spiky[i], a_spiky[i]);
+        ASSERT_FALSE(res.peek());
+    }
+    res.push(ts_spiky[i], a_spiky[i]);
+    //ASSERT_NEAR(res.get_fs(), fs, 1e-7); // should fail
+    ASSERT_NEAR(res.get_fs(), fs, 1e-2);
+
+    // get - initial samples are pushed out
+    sig_res.resize(ts_orig.size()+1); // despite gaussian time noise, sum(ts) should roughly be same length as we guessed initially
+    for (i = 0; i < INITIAL_SAMPLES; i++) {
+        ASSERT_TRUE(res.peek());
+        sig_res[i] = res.get();
+    }
+
+    // push - additional samples are all pushed out
+    int j = INITIAL_SAMPLES;
+    for (i = INITIAL_SAMPLES; i < ts_spiky.size(); i++) {
+        res.push(ts_spiky[i], a_spiky[i]);
+        // potentially get multiple samples
+        while (res.peek())
+            sig_res[j++] = res.get();
+    }
+
+    std::filesystem::create_directories("test6");
+    npy_save("test6/ts_orig_t1.npy", ts_orig);
+    npy_save("test6/a_orig_t1.npy", a_orig);
+
+    npy_save("test6/ts_spiky_t1.npy", ts_spiky);
+    npy_save("test6/a_spiky_t1.npy", a_spiky);
+
+    npy_save("test6/sig_res_t1.npy", sig_res);
+    std::vector<double> fs_t1{res.get_fs()};
+    npy_save("test6/fs_t1.npy", fs_t1);
+    /*
+     * ts_gen = np.arange(sig_res_t1.shape[0]) / fs * 1e9
+     * plt.plot(ts_spiky_t1, a_spiky_t1)
+     * plt.plot(ts_gen, sig_res_t1)
+     */
+}

google-tests/test_helpers.cpp

@@ -46,7 +46,7 @@ std::vector<double> fetch_y_axis(npy::npy_data<double>& acc, int dim) {
     return signal;
 }
 
-DebugSsfStepDetectorThreshold::DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {}
+DebugSsfStepDetectorThreshold::DebugSsfStepDetectorThreshold(double fps) : SsfStepDetector(fps) {}
 double DebugSsfStepDetectorThreshold::filter(double val) {
     this->SsfStepDetector::filter(val);
     return peek_threshold();

google-tests/test_helpers.h

@@ -27,7 +27,7 @@ std::vector<double> fetch_y_axis(npy::npy_data<double>& acc, int dim = 1);
 /** Returns the ssf_threshold as the filter output for debugging. */
 class DebugSsfStepDetectorThreshold : public SsfStepDetector {
 public:
-    DebugSsfStepDetectorThreshold(size_t len_refr);
+    DebugSsfStepDetectorThreshold(double fps);
     double filter(double val);
 };
 

pasada-lib/CMakeLists.txt

@@ -1,11 +1,11 @@
 project(Pasada_Lib)
 
 SET(PASADA_SRC
-        library.cpp
         iir_filter.cpp
         ssf_filter.cpp
         pd_signal.cpp
         step_detector.cpp
+        pd_resamp.cpp
 )
 
 if(PASADA_BUILD_TESTS)

pasada-lib/include/library.h

@@ -1,6 +0,0 @@
-#ifndef LIBPASADA_LIBRARY_H
-#define LIBPASADA_LIBRARY_H
-
-void hello();
-
-#endif // LIBPASADA_LIBRARY_H

pasada-lib/include/pd_resamp.h

@@ -0,0 +1,51 @@
+//
+// Created by david on 17.05.2026.
+//
+
+#ifndef PASADASUPERPROJECT_PD_RESAMP_H
+#define PASADASUPERPROJECT_PD_RESAMP_H
+
+#include "iir_filter.h"
+
+/** Filter that changes sampling rate between input and output. */
+class ResamplingFilter {
+public:
+    ResamplingFilter() {}
+    virtual ~ResamplingFilter() {}
+    virtual void push(double ts, double val) = 0;
+    virtual bool peek() = 0;
+    virtual double get() = 0;
+};
+
+/** Normalizes incoming Android sensor sampling rate. */
+class Resampler : public ResamplingFilter {
+protected:
+    const size_t INITIAL_SAMPLES = 100;
+    std::vector<double> times;
+    std::vector<double> data;
+    /** circular buffer size */
+    size_t N;
+    /** write index */
+    size_t n;
+    /** read index */
+    size_t m;
+    bool initialized;
+    bool read_valid;
+    /** computed sampling frequency, this will be the output rate */
+    double fs;
+    void compute_fs();
+public:
+    Resampler();
+    /**
+     * Push a value into the buffer.
+     * Caller is responsible for polling via peek() and get() afterward.
+     * @param ts  timestamp in nanoseconds
+     * @param val signal sample
+     */
+    void push(double ts, double val) override;
+    bool peek() override;
+    double get() override;
+    double get_fs() const;
+};
+
+#endif //PASADASUPERPROJECT_PD_RESAMP_H

pasada-lib/include/pd_signal.h

@@ -40,6 +40,12 @@ namespace pd_signal {
     /** two-dimensional mean of a collection of signals */
     void mean(std::vector<double> &out, std::deque<std::vector<double> >& m);
 
+    /** simple mean of 1-d signal */
+    double mean(const std::vector<double>& in);
+    void diff(std::vector<double>& out, const std::vector<double>& in);
+
+    std::vector<double> gauss(size_t N, double mu, double sigma);
+
     /**
      * Convolution of two polynomials given in ASCENDING power order.
      * If <c>p = p_0 + p_1 x + ... + p_{P-1} x^{P-1}</c> and likewise for q,

pasada-lib/include/ssf_filter.h

@@ -8,7 +8,7 @@
 #include "iir_filter.h"
 #include <deque>
 
-#define FPS 60
+/** max step rate detected: defines the length of "refractory period" which ignores additional, spurious edges in accelerometer */
 #define MAX_BPM 300
 
 /**
@@ -18,11 +18,10 @@
  */
 class SsfFilter {
 protected:
-    size_t sw;
     Filt f_delta_u;
     Filt f_window;
 public:
-    SsfFilter(size_t upslope_width);
+    SsfFilter(double fps);
     double filter(double val);
 };
 
@@ -34,8 +33,8 @@ public:
  */
 class SsfStepDetector {
 protected:
-    const size_t LEN_INIT;
-    const size_t LEN_TH_WIN;
+    size_t LEN_INIT;
+    size_t LEN_TH_WIN;
     size_t num_samples;
     double ssf_threshold;
     double ssf_threshold_nm1;
@@ -44,16 +43,18 @@ protected:
     size_t n_refr;
     bool is_refr;
     double ssf_nm1;
+    size_t ssf_usw2;
     Filt f_ssf_mean;
 public:
     /**
      * @param len_refr duration of refractory period, in samples
      */
-    SsfStepDetector(size_t len_refr);
+    SsfStepDetector(double fps);
+    ~SsfStepDetector();
     double filter(double val);
     double peek_threshold();
 
-    static size_t initial_samples();
+    static size_t initial_samples(double fps);
 };
 
 /**
@@ -62,18 +63,19 @@ public:
 class RunningQuality {
 protected:
     // TODO: make it a filter (output proper samples)
+    // TODO: use fps info
 
     /** template beat is resampled to this #samples */
-    const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */;
+    int BEAT_LEN;
 
     /** threshold for accepting initial beats */
-    const double BEAT_CORR_THR_1 = 0.9;
+    double BEAT_CORR_THR_1 = 0.9;
     /** threshold for accepting subsequent beats */
-    const double BEAT_CORR_THR_2 = 0.8;
+    double BEAT_CORR_THR_2 = 0.8;
     /** absolute SSF threshold for accepting any beat */
-    const double SSF_THRESHOLD = 5.0;
+    double SSF_THRESHOLD = 5.0;
     /** number of recent beats to use for beat template. must be even (alternating feet have different patterns; make it symmetric) */
-    const int    NUM_BEATS = 4;
+    int    NUM_BEATS = 4;
 
     std::deque<std::vector<double> > beatTemplates;
     std::vector<double> beatTemplate;
@@ -93,8 +95,8 @@ protected:
     virtual void dispatchBeat(int idx, bool good, double posCorr);
 
 public:
-    RunningQuality();
-    explicit RunningQuality(bool disableSsf);
+    RunningQuality(double fps);
+    explicit RunningQuality(double fps, bool disableSsf);
     virtual ~RunningQuality();
 
     // note: arg should be an iterator really, but can do later
@@ -115,7 +117,8 @@ protected:
     std::vector<double> ssf_buf;
     double sqi;
 public:
-    RunningQualityFilter(size_t upslope_width);
+    RunningQualityFilter(double fps);
+    ~RunningQualityFilter();
     double filter(double y, double ssf, double step);
 };
 

pasada-lib/include/step_detector.h

@@ -7,6 +7,7 @@
 
 #include "iir_filter.h"
 #include "ssf_filter.h"
+#include "pd_resamp.h"
 #include <vector>
 
 class StepListener {
@@ -15,6 +16,19 @@ public:
     virtual void playBeat() = 0;
 };
 
+/** mean-filter the gravity vector, then take acceleration downwards */
+class GravityFilter {
+    size_t N;
+    std::vector<double> gauss_taps;
+    Filt gx;
+    Filt gy;
+    Filt gz;
+public:
+    // 5 secs buffer, prime y with direction of gravity (for tests & faster init)
+    GravityFilter(double fps);
+    double filter(std::vector<double> values);
+};
+
 /**
  * Step detector from accelerometer signal.
  *
@@ -24,8 +38,7 @@ public:
 class StepDetector {
 protected:
     StepListener *listener;
-    IirFilter f_highpass;
-    Filt f_neg;
+    GravityFilter f_grav;
     SsfFilter f_ssf;
     SsfStepDetector f_ssd;
     RunningQualityFilter f_sqi;
@@ -35,9 +48,15 @@ protected:
     std::vector<double> buf_sqi;
     std::vector<double> buf_out;
 
+    Resampler res_x;
+    Resampler res_y;
+    Resampler res_z;
+    double fps;
+
 public:
-    StepDetector(StepListener *listener, bool debug = false);
-    void filter(std::vector<float> values);
+    StepDetector(double fps, StepListener *listener, bool debug = false);
+    void filter(double ts, std::vector<float> values);
+    void filter_a(double s1);
     std::vector<double> getBufSsd();
     std::vector<double> getBufSqi();
     std::vector<double> getBufOut();
@@ -46,7 +65,7 @@ public:
      * Prime the filters using the given input signal.
      * Used for debugging (non-realtime processing) to align the signal.
      */
-    void primeFilters(std::vector<double> sig);
+    double primeFilters(double fps, std::vector<double> sig);
 };
 
 #endif //PASADASUPERPROJECT_STEP_DETECTOR_H

pasada-lib/library.cpp

@@ -1,7 +0,0 @@
-#include "library.h"
-
-#include <iostream>
-
-void hello() {
-    std::cout << "Hello, World!" << std::endl;
-}

pasada-lib/pd_resamp.cpp

@@ -0,0 +1,103 @@
+//
+// Created by david on 17.05.2026.
+//
+
+#include "pd_resamp.h"
+#include "pd_signal.h"
+
+Resampler::Resampler(): N(INITIAL_SAMPLES+1), n(0), m(0), initialized(false), read_valid(false), fs(0.0) {
+    times.resize(N);
+    data.resize(N);
+    times.assign(N, 0.0);
+    data.assign(N, 0.0);
+}
+
+void Resampler::push(double ts, double val) {
+    // i: previous write position
+    auto i = static_cast<size_t>((static_cast<int>(n) - 1 + static_cast<int>(N)) % static_cast<int>(N));
+    auto im = static_cast<size_t>((static_cast<int>(m) - 1 + static_cast<int>(N)) % static_cast<int>(N));
+    if (ts < times[i]) throw std::invalid_argument("we expect ts to be time-ascending");
+    // j: current write position
+    auto j = n;
+    times[n] = ts;
+    data[n] = val;
+    n = (n+1) % N;
+    // note: we do not currently handle overrun (assume caller keeps contract)
+    if (n == INITIAL_SAMPLES && !initialized) {
+        compute_fs();
+        read_valid = true;
+        return; // returns INITIAL_SAMPLES all at once
+        // ??? need to compute 'dt' etc. for last sample! -> fall through
+    }
+    // once initialized, we skip ahead as much as possible - avoid buffering too much
+    double dt = ts - times[im];
+    double dtr = dt * 1e-9 * fs;
+    if (dtr < 0) { throw std::invalid_argument("dt is negative"); }
+    // case 1: 'ts' is less than next sample
+    if (dtr < 0.99) {
+        // drop samples (cannot be bothered with interpolation):
+        // practice shows that Android initially provides a high sampling rate, which subsequently drops later on,
+        // so we simply skip the implementation here.
+        read_valid = false;
+        return;
+    }
+    // case 2: 'ts' is exactly next sample
+    if (0.99 < dtr && dtr < 1.01) {
+        m = j; // skip directly to actual sample
+        read_valid = true;
+        return;
+    }
+    // case 3: 'ts' skips samples
+    if (dtr >= 1.01) {
+        auto ts_nm1 = times[i];
+        // x = ts[n-1] + np.linspace(1.0, dtr, int(np.round(dtr)), endpoint=True) / fs * 1e9
+        // xp = [ts[n-1], ts[n]]
+        // fp = [a[n-1], a[n]]
+        // y = np.interp(x, xp, fp)
+        std::vector<double> y;
+        std::vector<double> x;
+        std::vector<double> xp { times[i], ts };
+        std::vector<double> fp { data[i], val };
+        pd_signal::linspace(x, 1.0, dtr, static_cast<int>(round(dtr)), false);
+        for (auto& e : x) e = ts_nm1 + e / fs * 1e9;
+        pd_signal::interp(y, x, xp, fp);
+        // write to data[_ : n]
+        int s = static_cast<int>(x.size());
+        auto p0 = static_cast<size_t>((static_cast<int>(n) - s + static_cast<int>(N)) % static_cast<int>(N));
+        for (int p = 0; p < s; p++) {
+            data[(p0 + p) % N] = y[p];
+        }
+        m = p0; // provides round(dtr) samples output, interpolated
+        read_valid = true;
+        return;
+    }
+}
+
+bool Resampler::peek() {
+    if (!initialized) { return false; }
+    if (!read_valid) { return false; }
+    return n != m;
+}
+
+double Resampler::get() {
+    if (!initialized) { throw std::runtime_error("not initialized"); }
+    if (n == m) { throw std::runtime_error("empty buffer"); }
+    double val = data[m];
+    m = (m+1) % N;
+    return val;
+}
+
+double Resampler::get_fs() const {
+    return fs;
+}
+
+void Resampler::compute_fs() {
+    // compute 'fs' according to first INITIAL_SAMPLES
+    // we ignore 'n' as this is only ever called once per Resampler lifetime, and assume 'times' has been filled from 0 on
+    std::vector<double> delta_times;
+    pd_signal::diff(delta_times, times);
+    delta_times.resize(INITIAL_SAMPLES-1); // trim off trailing buffer slot (which is not filled)
+    double mean_dt = pd_signal::mean(delta_times);
+    fs = 1e9 / mean_dt;
+    initialized = true;
+}

pasada-lib/pd_signal.cpp

@@ -6,6 +6,7 @@
 #include <stdexcept>
 #include <algorithm>
 #include <iostream>
+#include <numeric>
 
 namespace pd_signal {
 
@@ -141,6 +142,35 @@ void mean(std::vector<double> &out, std::deque<std::vector<double> >& m) {
     mean_tpl(out, m);
 }
 
+double mean(const std::vector<double> &in) {
+    if (in.empty()) {
+        throw std::invalid_argument("mean: input vector is empty");
+    }
+    double sum = std::accumulate(in.begin(), in.end(), 0.0);
+    return sum / static_cast<double>(in.size());
+}
+
+void diff(std::vector<double>& out, const std::vector<double>& in) {
+    if (in.size() < 2) {
+        out.clear();
+        return;
+    }
+    out.resize(in.size() - 1);
+    for (std::size_t i = 1; i < in.size(); ++i) {
+        out[i - 1] = in[i] - in[i - 1];
+    }
+}
+
+std::vector<double> gauss(size_t N, double mu, double sigma) {
+    const double norm = sigma * sqrt(2.0 * kPi);
+    std::vector<double> data(N);
+    for (int i = 0; i < N; i++) {
+        const double x = i;
+        data[i] = std::exp(-0.5 * (x - mu) * (x - mu) / (sigma * sigma)) / norm;
+    }
+    return data;
+}
+
 // Convolution of two polynomials in ascending power order.
 void polymul(std::vector<cplx>& out,
              const std::vector<cplx>& p, const std::vector<cplx>& q) {

pasada-lib/ssf_filter.cpp

@@ -27,11 +27,18 @@ static std::vector<double> make_ones(size_t sw) {
     return ones;
 }
 
-SsfFilter::SsfFilter(size_t upslope_width) :
-    sw(upslope_width),
+static int get_upslope_width(double fps) {
+    // was 4 at 60 fps = 66.7 ms
+    if (fps == 4.0) throw std::invalid_argument("check SsfFilter ctor call, must pass fps now"); // nice-to remove
+    int usw = static_cast<int>(std::round(0.0667 * fps));
+    if (usw == 0) throw std::invalid_argument("upslope_width = 0 computed - low fps?");
+    return usw;
+}
+
+SsfFilter::SsfFilter(double fps) :
     // Filt(N, shift, offset, taps)
     f_delta_u(2, 0, 0, std::vector<double> {1.0, -1.0}),
-    f_window(upslope_width, 0, 0, make_ones(upslope_width))
+    f_window(get_upslope_width(fps), 0, 0, make_ones(get_upslope_width(fps)))
 {}
 double SsfFilter::filter(double val) {
     double du = f_delta_u.filter(val);
@@ -40,22 +47,46 @@ double SsfFilter::filter(double val) {
     return ssf;
 }
 
-size_t SsfStepDetector::initial_samples() { return (size_t) (3.0 * FPS); }
 
-SsfStepDetector::SsfStepDetector(size_t len_refr) :
+static size_t get_initial_samples(double fps) {
+    if (fps == 12.0) throw std::invalid_argument("check SsfStepDetector ctor call, must pass fps now"); // nice-to remove
+    int init_samp = static_cast<int>(std::round(3.0 * fps));
+    if (init_samp == 0) throw std::invalid_argument("init_samp = 0 computed - low fps?");
+    return init_samp;
+}
+
+size_t SsfStepDetector::initial_samples(double fps) { return get_initial_samples(fps); }
+
+static size_t get_len_refr(double fps) {
+    if (fps == 12.0) throw std::invalid_argument("check SsfStepDetector ctor call, must pass fps now"); // nice-to remove
+    size_t len_refr = static_cast<size_t>(std::round(fps / (MAX_BPM / 60)));
+    if (len_refr == 0) throw std::invalid_argument("len_refr = 0 computed - low fps?");
+    return len_refr;
+}
+
+static int get_len_ssf_th_smoothing(double fps) {
+    // was 6 at 60 fps = 100 ms
+    if (fps == 12.0) throw std::invalid_argument("check SsfStepDetector ctor call, must pass fps now"); // nice-to remove
+    int sts = static_cast<int>(std::round(0.100 * fps));
+    if (sts == 0) throw std::invalid_argument("len_ssf_th_smoothing = 0 computed - low fps?");
+    return sts;
+}
+SsfStepDetector::SsfStepDetector(double fps) :
     // note: also change above, in initial_samples()
-    LEN_INIT((size_t) (3.0 * FPS)), // initial window length for ssf_threshold
-    LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold
+    LEN_INIT(get_initial_samples(fps)), // initial window length for ssf_threshold
+    LEN_TH_WIN(get_initial_samples(fps)), // subsequent window length for ssf_threshold
     num_samples(0),
     ssf_threshold(std::numeric_limits<double>::infinity()),
     ssf_threshold_nm1(std::numeric_limits<double>::infinity()),
-    f_ssf_threshold_smoothing(6, 0, 0, make_ones(6)),
-    len_refr(len_refr), n_refr(0), is_refr(false),
+    f_ssf_threshold_smoothing(get_len_ssf_th_smoothing(fps), 0, 0, make_ones(get_len_ssf_th_smoothing(fps))),
+    len_refr(get_len_refr(fps)), n_refr(0), is_refr(false),
     ssf_nm1(0.0),
+    ssf_usw2(get_upslope_width(fps)/2),
     f_ssf_mean(LEN_TH_WIN, 0, 0, make_ones(LEN_TH_WIN))
 {
     assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold");
 }
+SsfStepDetector::~SsfStepDetector() {}
 double SsfStepDetector::filter(double ssf) {
     double ssf_mean = f_ssf_mean.filter(ssf) / ((double) LEN_TH_WIN);
     double rv = 0.0;
@@ -89,12 +120,10 @@ double SsfStepDetector::filter(double ssf) {
     } else if (num_samples > LEN_TH_WIN) {
         //DEBUG_PRINT(std::cerr << "adaptive threshold setting" << std::endl);
         // adaptive threshold setting
-        // +2 is half the window size
-        // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding -- also f_ssf_threshold_smoothing(), nb. should be even number
-        if (num_samples == n_refr + 2) {
+        // the ssf peak comes SsfFilter.upslope_width/2 = 3 samples (half-window + 1 sample) after the crossing
+        if (num_samples == n_refr + ssf_usw2 + 1) {
             //DEBUG_PRINT(std::cerr << "setting adaptive threshold setting" << std::endl);
             ssf_threshold_nm1 = ssf_threshold;
-            // the ssf peak comes 3 samples (half-window + 1 sample) after the crossing
             ssf_threshold = f_ssf_threshold_smoothing.filter(ssf) / ((double) f_ssf_threshold_smoothing.size()) * 0.6;
         }
     }
@@ -126,8 +155,13 @@ void RunningQuality::replaceTemplate(std::vector<double>& x) {
 void RunningQuality::dispatchLocked() { /* implement me, add Listener etc. */ }
 void RunningQuality::dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ }
 
-RunningQuality::RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
-RunningQuality::RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
+static int get_beat_len(double fps) {
+    /* 2*FPS for 30 bpm lower end */
+    return 2.0 * fps;
+}
+
+RunningQuality::RunningQuality(double fps): BEAT_LEN(get_beat_len(fps)), beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
+RunningQuality::RunningQuality(double fps, bool disableSsf): BEAT_LEN(get_beat_len(fps)), beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
 RunningQuality::~RunningQuality() {}
 
 // note: arg should be an iterator really, but can do later
@@ -193,7 +227,8 @@ bool RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &r
 }
 
 
-RunningQualityFilter::RunningQualityFilter(size_t upslope_width) : sqi(0.0) {}
+RunningQualityFilter::RunningQualityFilter(double fps) : f_sqi(fps), sqi(0.0) {}
+RunningQualityFilter::~RunningQualityFilter() {}
 
 double RunningQualityFilter::filter(double y, double ssf, double step) {
     if (step == 1.0) {

pasada-lib/step_detector.cpp

@@ -4,40 +4,72 @@
 
 #include "step_detector.h"
 
-// TODO: we are hardcoding filter coefficients for 60 Hz
-// TODO: this is tolerable for 50 Hz
+#include "pd_signal.h"
 
-// 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) :
+StepDetector::StepDetector(double fps, 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)
+    f_grav(fps),
+    f_ssf(fps),
+    f_ssd(fps),
+    f_sqi(fps),
+    debug(debug),
+    fps(0.0)
 {}
 
-#if (FPS != 60)
-#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
-#endif
+static int gravity_num_taps(double fps) {
+    return 5.0 * fps;
+}
 
-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);
+// 5 secs buffer, prime y with direction of gravity (for tests & faster init)
+GravityFilter::GravityFilter(double fps) :
+    N(gravity_num_taps(fps)),
+    gauss_taps(pd_signal::gauss(N, N/2, N/4)),
+    gx(N, 0, 0, gauss_taps),
+    gy(N, 0, 0, gauss_taps),
+    gz(N, 0, 0, gauss_taps)
+{
+    gy.prime(-9.81);
+}
+
+double GravityFilter::filter(std::vector<double> values) {
+    gx.push(values[0]);
+    gy.push(values[1]);
+    gz.push(values[2]);
+    double x = gx.peek(), y = gy.peek(), z = gz.peek();
+    double g = sqrt(x * x + y * y + z * z);
+    // e = mean(a)
+    double ex = x / g, ey = y / g, ez = z / g;
+    // e \in a
+    double vx = values[0] * ex;
+    double vy = values[1] * ey;
+    double vz = values[2] * ez;
+    return vx + vy + vz;
+}
+
+void StepDetector::filter(double ts, std::vector<float> values) {
+    // resample to smooth over Android sensor FPS variations
+    res_x.push(ts, values[0]);
+    res_y.push(ts, values[1]);
+    res_z.push(ts, values[2]);
+    // as soon as there is 'fps' information, re-init the classes requiring fps info
+    if (fps == 0.0 && res_x.peek()) {
+        fps = res_x.get_fs();
+        f_grav = GravityFilter(fps);
+        f_ssf = SsfFilter(fps);
+        f_ssd = SsfStepDetector(fps);
+        f_sqi = RunningQualityFilter(fps);
+    }
+    while (res_x.peek()) {
+        double x = res_x.get(), y = res_y.get(), z = res_z.get();
+        std::vector<double> samp { x, y, z };
+        // gravity filtering
+        double a = f_grav.filter(samp);
+        // pass on accel sample
+        filter_a(a);
+    }
+}
+
+void StepDetector::filter_a(double s2) {
     auto s3 = f_ssf.filter(s2);
     auto s4 = f_ssd.filter(s3);
     auto q5 = f_sqi.filter(s2, s3, s4);
@@ -56,15 +88,18 @@ 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();
+double StepDetector::primeFilters(double fps, std::vector<double> sig) {
+    const size_t N_INIT = SsfStepDetector::initial_samples(fps);
     // initialize: feed for priming the filters
+    double ts = 0;
     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});
+        filter(ts * 1e9, std::vector<float> {0.0f, a_i, 0.0f});
+        ts += 1.0 / fps;
     }
     // clear debug buffers
     buf_ssd.clear();
     buf_sqi.clear();
     buf_out.clear();
+    return ts;
 }