integrator: improvements and cleanup

The integrator had an untested read mode which did not apply the coning
correction. Instead of keeping two integrals (auto/read) it is now one
and the reset mechanism can be selected by setting the
auto_reset_interval to 0 to disable it or some positive number else.

Also, the integrator could potentially explode if a (single) timestamp
was wrong, so before the last integrated one. This is now caught with a
dt of 0 instead of inf/nan.

src/drivers/device/integrator.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,19 +37,18 @@
  * A resettable integrator
  *
  * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include "integrator.h"
 
 Integrator::Integrator(uint64_t auto_reset_interval, bool coning_compensation) :
 	_auto_reset_interval(auto_reset_interval),
-	_last_integration(0),
-	_last_auto(0),
-	_integral_auto(0.0f, 0.0f, 0.0f),
-	_integral_read(0.0f, 0.0f, 0.0f),
+	_last_integration_time(0),
+	_last_reset_time(0),
+	_integral(0.0f, 0.0f, 0.0f),
 	_last_val(0.0f, 0.0f, 0.0f),
 	_last_delta(0.0f, 0.0f, 0.0f),
-	_auto_callback(nullptr),
 	_coning_comp_on(coning_compensation)
 {
 
@@ -63,19 +62,25 @@ Integrator::~Integrator()
 bool
 Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt)
 {
-	bool auto_reset = false;
-
-	if (_last_integration == 0) {
+	if (_last_integration_time == 0) {
 		/* this is the first item in the integrator */
-		_last_integration = timestamp;
-		_last_auto = timestamp;
+		_last_integration_time = timestamp;
+		_last_reset_time = timestamp;
 		_last_val = val;
+
 		return false;
 	}
 
-	// Integrate
-	double dt = (double)(timestamp - _last_integration) / 1000000.0;
-	math::Vector<3> i = (val + _last_val) * dt * 0.5f;
+	double dt = 0.0;
+
+	// Integrate:
+	// Leave dt at 0 if the integration time does not make sense.
+	// Without this check the integral is likely to explode.
+	if (timestamp >= _last_integration_time) {
+		dt = (double)(timestamp - _last_integration_time) / 1000000.0;
+	}
+
+	math::Vector<3> delta = (val + _last_val) * dt * 0.5f;
 
 	// Apply coning compensation if required
 	if (_coning_comp_on) {
@@ -84,45 +89,46 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ
 		// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
 		// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
 
-		i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f;
+		delta += ((_integral + _last_delta * (1.0f / 6.0f)) % delta) * 0.5f;
 	}
 
-	_integral_auto += i;
-	_integral_read += i;
+	_integral += delta;
 
-	_last_integration = timestamp;
+	_last_integration_time = timestamp;
 	_last_val = val;
-	_last_delta = i;
-
-	if ((timestamp - _last_auto) > _auto_reset_interval) {
-		if (_auto_callback) {
-			/* call the callback */
-			_auto_callback(timestamp, _integral_auto);
-		}
-
-		integral = _integral_auto;
-		integral_dt = (timestamp - _last_auto);
-
-		auto_reset = true;
-		_last_auto = timestamp;
-		_integral_auto(0) = 0.0f;
-		_integral_auto(1) = 0.0f;
-		_integral_auto(2) = 0.0f;
-	}
+	_last_delta = delta;
+
+	// Only do auto reset if auto reset interval is not 0.
+	if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) > _auto_reset_interval) {
 
-	return auto_reset;
+		integral = _integral;
+		_reset(integral_dt);
+
+		return true;
+	} else {
+		return false;
+	}
 }
 
 math::Vector<3>
-Integrator::read(bool auto_reset)
+Integrator::get(bool reset, uint64_t &integral_dt)
 {
-	math::Vector<3> val = _integral_read;
+	math::Vector<3> val = _integral;
 
-	if (auto_reset) {
-		_integral_read(0) = 0.0f;
-		_integral_read(1) = 0.0f;
-		_integral_read(2) = 0.0f;
+	if (reset) {
+		_reset(integral_dt);
 	}
 
 	return val;
 }
+
+void
+Integrator::_reset(uint64_t &integral_dt)
+{
+	_integral(0) = 0.0f;
+	_integral(1) = 0.0f;
+	_integral(2) = 0.0f;
+
+	integral_dt = (_last_integration_time - _last_reset_time);
+	_last_reset_time = _last_integration_time;
+}

src/drivers/device/integrator.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +37,7 @@
  * A resettable integrator
  *
  * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #pragma once
@@ -52,46 +53,41 @@ public:
 	/**
 	 * Put an item into the integral.
 	 *
-	 * @param timestamp	Timestamp of the current value
-	 * @param val		Item to put
+	 * @param timestamp	Timestamp of the current value.
+	 * @param val		Item to put.
 	 * @param integral	Current integral in case the integrator did reset, else the value will not be modified
-	 * @return		true if putting the item triggered an integral reset
-	 *			and the integral should be published
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
+	 * @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);
 
 	/**
-	 * Get the current integral value
+	 * Get the current integral and reset the integrator if needed.
 	 *
-	 * @return		the integral since the last auto-reset
-	 */
-	math::Vector<3>		get() { return _integral_auto; }
-
-	/**
-	 * Read from the integral
-	 *
-	 * @param auto_reset	Reset the integral to zero on read
+	 * @param reset	    	Reset the integral to zero.
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
 	 * @return		the integral since the last read-reset
 	 */
-	math::Vector<3>		read(bool auto_reset);
-
-	/**
-	 * Get current integral start time
-	 */
-	uint64_t		current_integral_start() { return _last_auto; }
+	math::Vector<3>		get(bool reset, uint64_t &integral_dt);
 
 private:
-	uint64_t _auto_reset_interval;		/**< the interval after which the content will be published and the integrator reset */
-	uint64_t _last_integration;			/**< timestamp of the last integration step */
-	uint64_t _last_auto;				/**< last auto-announcement of integral value */
-	math::Vector<3> _integral_auto;			/**< the integrated value which auto-resets after _auto_reset_interval */
-	math::Vector<3> _integral_read;			/**< the integrated value since the last read */
+	uint64_t _auto_reset_interval;			/**< the interval after which the content will be published
+							     and the integrator reset, 0 if no auto-reset */
+	uint64_t _last_integration_time;		/**< timestamp of the last integration step */
+	uint64_t _last_reset_time;			/**< last auto-announcement of integral value */
+	math::Vector<3> _integral;			/**< the integrated value */
 	math::Vector<3> _last_val;			/**< previously integrated last value */
 	math::Vector<3> _last_delta;			/**< last local delta */
-	void (*_auto_callback)(uint64_t, math::Vector<3>);	/**< the function callback for auto-reset */
 	bool _coning_comp_on;				/**< coning compensation */
 
 	/* we don't want this class to be copied */
 	Integrator(const Integrator &);
 	Integrator operator=(const Integrator &);
+
+	/* Do a reset.
+	 *
+	 * @param integral_dt	Get the dt in us of the current integration.
+	 */
+	void _reset(uint64_t &integral_dt);
 };