feat: StepDetector update FPS, more FPS fixes

pasada-lib/step_detector.cpp

@@ -12,7 +12,8 @@ StepDetector::StepDetector(double fps, StepListener *listener, bool debug) :
     f_ssf(fps),
     f_ssd(fps),
     f_sqi(fps),
-    debug(debug)
+    debug(debug),
+    fps(0.0)
 {}
 
 static int gravity_num_taps(double fps) {
@@ -50,6 +51,14 @@ void StepDetector::filter(double ts, std::vector<float> values) {
     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 };
@@ -85,7 +94,7 @@ double StepDetector::primeFilters(double fps, std::vector<double> sig) {
     double ts = 0;
     for (size_t i = 0; i < N_INIT; i++) {
         const auto a_i = static_cast<float>(sig[i]);
-        filter(ts, 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