diff --git a/src/xrt/auxiliary/math/m_filter_fifo.c b/src/xrt/auxiliary/math/m_filter_fifo.c index 72821b1d6..63cbf4a97 100644 --- a/src/xrt/auxiliary/math/m_filter_fifo.c +++ b/src/xrt/auxiliary/math/m_filter_fifo.c @@ -88,6 +88,12 @@ m_ff_vec3_f32_free(struct m_ff_vec3_f32 **ff_ptr) *ff_ptr = NULL; } +size_t +m_ff_vec3_f32_get_num(struct m_ff_vec3_f32 *ff) +{ + return ff->num; +} + void m_ff_vec3_f32_push(struct m_ff_vec3_f32 *ff, const struct xrt_vec3 *sample, @@ -103,15 +109,21 @@ m_ff_vec3_f32_push(struct m_ff_vec3_f32 *ff, ff->timestamps_ns[i] = timestamp_ns; } -void +bool m_ff_vec3_f32_get(struct m_ff_vec3_f32 *ff, size_t num, struct xrt_vec3 *out_sample, uint64_t *out_timestamp_ns) { + if (num >= ff->num) { + return false; + } + size_t pos = (ff->latest + num) % ff->num; *out_sample = ff->samples[pos]; *out_timestamp_ns = ff->timestamps_ns[pos]; + + return true; } size_t @@ -242,6 +254,12 @@ m_ff_f64_free(struct m_ff_f64 **ff_ptr) *ff_ptr = NULL; } +size_t +m_ff_f64_get_num(struct m_ff_f64 *ff) +{ + return ff->num; +} + void m_ff_f64_push(struct m_ff_f64 *ff, const double *sample, uint64_t timestamp_ns) { @@ -255,15 +273,21 @@ m_ff_f64_push(struct m_ff_f64 *ff, const double *sample, uint64_t timestamp_ns) ff->timestamps_ns[i] = timestamp_ns; } -void +bool m_ff_f64_get(struct m_ff_f64 *ff, size_t num, double *out_sample, uint64_t *out_timestamp_ns) { + if (num >= ff->num) { + return false; + } + size_t pos = (ff->latest + num) % ff->num; *out_sample = ff->samples[pos]; *out_timestamp_ns = ff->timestamps_ns[pos]; + + return true; } size_t diff --git a/src/xrt/auxiliary/math/m_filter_fifo.h b/src/xrt/auxiliary/math/m_filter_fifo.h index 16b880b90..ffbfb774a 100644 --- a/src/xrt/auxiliary/math/m_filter_fifo.h +++ b/src/xrt/auxiliary/math/m_filter_fifo.h @@ -32,6 +32,12 @@ m_ff_vec3_f32_alloc(struct m_ff_vec3_f32 **ff_out, size_t num); void m_ff_vec3_f32_free(struct m_ff_vec3_f32 **ff_ptr); +/*! + * Return the number of samples that can fill the fifo. + */ +size_t +m_ff_vec3_f32_get_num(struct m_ff_vec3_f32 *ff); + /*! * Pushes a sample at the given timepoint, pushing samples out of order yields * unspecified behaviour, so samples must be pushed in time order. @@ -45,7 +51,7 @@ m_ff_vec3_f32_push(struct m_ff_vec3_f32 *ff, * Return the sample at the index, zero means the last sample push, one second * last and so on. */ -void +bool m_ff_vec3_f32_get(struct m_ff_vec3_f32 *ff, size_t num, struct xrt_vec3 *out_sample, @@ -82,6 +88,12 @@ m_ff_f64_alloc(struct m_ff_f64 **ff_out, size_t num); void m_ff_f64_free(struct m_ff_f64 **ff_ptr); +/*! + * Return the number of samples that can fill the fifo. + */ +size_t +m_ff_f64_get_num(struct m_ff_f64 *ff); + /*! * Pushes a sample at the given timepoint, pushing samples out of order yields * unspecified behaviour, so samples must be pushed in time order. @@ -93,7 +105,7 @@ m_ff_f64_push(struct m_ff_f64 *ff, const double *sample, uint64_t timestamp_ns); * Return the sample at the index, zero means the last sample push, one second * last and so on. */ -void +bool m_ff_f64_get(struct m_ff_f64 *ff, size_t num, double *out_sample,