From f528c630300648de1032ffe2b342252e0ec1d1d3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 3 May 2016 17:40:07 +0200
Subject: [PATCH] integrator: add a put method for known intervals

This adds a second put method to the integrator class. This allows to
integrate with known intervals between samples, rather than based on
timestamps. This makes integrating the samples coming out of the MPU9250
FIFO buffer easier.
---
 src/drivers/device/integrator.cpp | 20 ++++++++++++++++++++
 src/drivers/device/integrator.h   | 17 ++++++++++++++++-
 2 files changed, 36 insertions(+), 1 deletion(-)

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.