diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index 3437cbebff..60f3af5bb4 100644 --- a/src/drivers/device/integrator.cpp +++ b/src/drivers/device/integrator.cpp @@ -111,6 +111,26 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ } } +bool +Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral, + uint64_t &integral_dt) +{ + if (_last_integration_time == 0) { + /* this is the first item in the integrator */ + uint64_t now = hrt_absolute_time(); + _last_integration_time = now; + _last_reset_time = now; + _last_val = val; + + return false; + } + + // Create the timestamp artifically. + uint64_t timestamp = _last_integration_time + interval_us; + + return put(timestamp, val, integral, integral_dt); +} + math::Vector<3> Integrator::get(bool reset, uint64_t &integral_dt) { diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h index 4d5cb47ebc..11aca58e80 100644 --- a/src/drivers/device/integrator.h +++ b/src/drivers/device/integrator.h @@ -60,7 +60,22 @@ public: * @return true if putting the item triggered an integral reset and the integral should be * published. */ - bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt); + bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt); + + /** + * Put an item into the integral but provide an interval instead of a timestamp. + * + * @param interval_us Interval in us since last integration. + * @param val Item to put. + * @param integral Current integral in case the integrator did reset, else the value will not be modified + * @param integral_dt Get the dt in us of the current integration (only if reset). Note that this + * values might not be accurate vs. hrt_absolute_time because it is just the sum of the + * supplied intervals. + * @return true if putting the item triggered an integral reset and the integral should be + * published. + */ + bool put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral, + uint64_t &integral_dt); /** * Get the current integral and reset the integrator if needed.