From 080a136e502a8d846634e7e2293c190a6546aa2e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 May 2016 21:37:53 +0200 Subject: [PATCH] integrator: add function to return filtered data Instead of only being able to get the integral and its integration time, it can also be handy to get the integral divided/differentiated by the the integration time. This data is then just filtered by the integrator. --- src/drivers/device/integrator.cpp | 14 ++++++++++++++ src/drivers/device/integrator.h | 11 +++++++++++ 2 files changed, 25 insertions(+) diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index 60f3af5bb4..072e3096a2 100644 --- a/src/drivers/device/integrator.cpp +++ b/src/drivers/device/integrator.cpp @@ -143,6 +143,20 @@ Integrator::get(bool reset, uint64_t &integral_dt) return val; } +math::Vector<3> +Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val) +{ + // Do the usual get with reset first but don't return yet. + math::Vector<3> ret_integral = get(reset, integral_dt); + + // Because we need both the integral and the integral_dt. + filtered_val(0) = ret_integral(0) * 1000000 / integral_dt; + filtered_val(1) = ret_integral(1) * 1000000 / integral_dt; + filtered_val(2) = ret_integral(2) * 1000000 / integral_dt; + + return ret_integral; +} + void Integrator::_reset(uint64_t &integral_dt) { diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h index 11aca58e80..48a232066f 100644 --- a/src/drivers/device/integrator.h +++ b/src/drivers/device/integrator.h @@ -86,6 +86,17 @@ public: */ math::Vector<3> get(bool reset, uint64_t &integral_dt); + /** + * Get the current integral and reset the integrator if needed. Additionally give the + * integral over the samples differentiated by the integration time (mean filtered values). + * + * @param reset Reset the integral to zero. + * @param integral_dt Get the dt in us of the current integration (only if reset). + * @param filtered_val The integral differentiated by the integration time. + * @return the integral since the last read-reset + */ + math::Vector<3> get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val); + private: uint64_t _auto_reset_interval; /**< the interval after which the content will be published and the integrator reset, 0 if no auto-reset */