sensors/vehicle_imu: Integrator simplify and make header only

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -32,7 +32,6 @@
 ############################################################################
 
 px4_add_library(vehicle_imu
-	Integrator.cpp
 	Integrator.hpp
 	VehicleIMU.cpp
 	VehicleIMU.hpp

src/modules/sensors/vehicle_imu/Integrator.cpp

@@ -1,97 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015-2020 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "Integrator.hpp"
-
-#include <drivers/drv_hrt.h>
-
-using matrix::Vector3f;
-
-bool Integrator::put(const hrt_abstime &timestamp, const Vector3f &val)
-{
-	if ((_last_integration_time == 0) || (timestamp <= _last_integration_time)) {
-		/* this is the first item in the integrator */
-		_last_integration_time = timestamp;
-		_last_reset_time = timestamp;
-		_last_val = val;
-
-		return false;
-	}
-
-	// Use trapezoidal integration to calculate the delta integral
-	const float dt = static_cast<float>(timestamp - _last_integration_time) * 1e-6f;
-	const matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f;
-	_last_val = val;
-	_last_integration_time = timestamp;
-	_integrated_samples++;
-
-	// Calculate coning corrections if required
-	if (_coning_comp_on) {
-		// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
-		// following:
-		// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
-		// Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
-		// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
-		_beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f;
-		_last_delta_alpha = delta_alpha;
-		_last_alpha = _alpha;
-	}
-
-	// accumulate delta integrals
-	_alpha += delta_alpha;
-
-	return true;
-}
-
-bool Integrator::reset(Vector3f &integral, uint32_t &integral_dt)
-{
-	if (integral_ready()) {
-		integral = Vector3f{_alpha};
-		_alpha.zero();
-
-		integral_dt = (_last_integration_time - _last_reset_time);
-		_last_reset_time = _last_integration_time;
-		_integrated_samples = 0;
-
-		// apply coning corrections if required
-		if (_coning_comp_on) {
-			integral += _beta;
-			_beta.zero();
-			_last_alpha.zero();
-		}
-
-		return true;
-	}
-
-	return false;
-}

src/modules/sensors/vehicle_imu/Integrator.hpp

@@ -48,7 +48,7 @@
 class Integrator
 {
 public:
-	Integrator(bool coning_compensation = false) : _coning_comp_on(coning_compensation) {}
+	Integrator() = default;
 	~Integrator() = default;
 
 	/**
@@ -58,29 +58,23 @@ public:
 	 * @param val		Item to put.
 	 * @return		true if data was accepted and integrated.
 	 */
-	bool put(const uint64_t &timestamp, const matrix::Vector3f &val);
+	inline void put(const matrix::Vector3f &val, const float dt)
-
-	/**
-	 * Put an item into the integral.
-	 *
-	 * @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
-	 * @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(const uint64_t &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
 	{
-		return put(timestamp, val) && reset(integral, integral_dt);
+		if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) {
+			_alpha += integrate(val, dt);
+
+		} else {
+			reset();
+			_last_val = val;
+		}
 	}
 
 	/**
 	 * Set reset interval during runtime. This won't reset the integrator.
 	 *
-	 * @param reset_interval	    	New reset time interval for the integrator.
+	 * @param reset_interval	New reset time interval for the integrator in microseconds.
 	 */
-	void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval; }
+	void set_reset_interval(uint32_t reset_interval_us) { _reset_interval_min = reset_interval_us * 1e-6f; }
 
 	/**
 	 * Set required samples for reset. This won't reset the integrator.
@@ -95,29 +89,123 @@ public:
 	 *
 	 * @return		true if integrator has sufficient data (minimum interval & samples satisfied) to reset.
 	 */
-	bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_last_integration_time >= (_last_reset_time + _reset_interval_min)); }
+	inline bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); }
+
+	void reset()
+	{
+		_alpha.zero();
+		_integral_dt = 0;
+		_integrated_samples = 0;
+	}
 
 	/* Reset integrator and return current integral & integration time
 	 *
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 * @return		true if integral valid
 	 */
-	bool reset(matrix::Vector3f &integral, uint32_t &integral_dt);
+	bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
+	{
+		if (integral_ready()) {
+			integral = _alpha;
+			integral_dt = roundf(_integral_dt * 1e6f); // seconds to microseconds
 
-private:
+			reset();
-	uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */
-	uint64_t _last_reset_time{0};       /**< last auto-announcement of integral value */
 
-	matrix::Vector3f _alpha{0.f, 0.f, 0.f};            /**< integrated value before coning corrections are applied */
+			return true;
-	matrix::Vector3f _last_alpha{0.f, 0.f, 0.f};       /**< previous value of _alpha */
+		}
-	matrix::Vector3f _beta{0.f, 0.f, 0.f};             /**< accumulated coning corrections */
-	matrix::Vector3f _last_val{0.f, 0.f, 0.f};         /**< previous input */
-	matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */
 
-	uint32_t _reset_interval_min{1}; /**< the interval after which the content will be published and the integrator reset */
+		return false;
+	}
 
-	uint8_t _integrated_samples{0};
+protected:
+
+	inline matrix::Vector3f integrate(const matrix::Vector3f &val, const float dt)
+	{
+		// Use trapezoidal integration to calculate the delta integral
+		_integrated_samples++;
+		_integral_dt += dt;
+		const matrix::Vector3f delta_alpha{(val + _last_val) *dt * 0.5f};
+		_last_val = val;
+
+		return delta_alpha;
+	}
+
+	matrix::Vector3f _alpha{0.f, 0.f, 0.f};    /**< integrated value before coning corrections are applied */
+	matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */
+	float _integral_dt{0};
+
+	float _reset_interval_min{0.005f}; /**< the interval after which the content will be published and the integrator reset */
 	uint8_t _reset_samples_min{1};
 
-	const bool _coning_comp_on{false};                       /**< true to turn on coning corrections */
+	uint8_t _integrated_samples{0};
+};
+
+class IntegratorConing : public Integrator
+{
+public:
+	IntegratorConing() = default;
+	~IntegratorConing() = default;
+
+	/**
+	 * Put an item into the integral.
+	 *
+	 * @param timestamp	Timestamp of the current value.
+	 * @param val		Item to put.
+	 * @return		true if data was accepted and integrated.
+	 */
+	inline void put(const matrix::Vector3f &val, const float dt)
+	{
+		if (dt > 0.f && dt <= (_reset_interval_min * 2.f)) {
+			// Use trapezoidal integration to calculate the delta integral
+			const matrix::Vector3f delta_alpha{integrate(val, dt)};
+
+			// Calculate coning corrections
+			// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
+			// following:
+			// Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms
+			// Sourced: https://arc.aiaa.org/doi/pdf/10.2514/2.4228
+			// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
+			_beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f;
+			_last_delta_alpha = delta_alpha;
+			_last_alpha = _alpha;
+
+			// accumulate delta integrals
+			_alpha += delta_alpha;
+
+		} else {
+			reset();
+			_last_val = val;
+		}
+	}
+
+	void reset()
+	{
+		Integrator::reset();
+		_beta.zero();
+		_last_alpha.zero();
+	}
+
+	/* Reset integrator and return current integral & integration time
+	 *
+	 * @param integral_dt	Get the dt in us of the current integration.
+	 * @return		true if integral valid
+	 */
+	bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
+	{
+		if (Integrator::reset(integral, integral_dt)) {
+			// apply coning corrections
+			integral += _beta;
+			_beta.zero();
+			_last_alpha.zero();
+			return true;
+		}
+
+		return false;
+	}
+
+private:
+	matrix::Vector3f _beta{0.f, 0.f, 0.f};             /**< accumulated coning corrections */
+	matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */
+	matrix::Vector3f _last_alpha{0.f, 0.f, 0.f};       /**< previous value of _alpha */
+
 };

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -249,9 +249,11 @@ void VehicleIMU::Run()
 		_gyro_temperature += gyro.temperature;
 		_gyro_sum_count++;
 
-		_gyro_integrator.put(gyro.timestamp_sample, gyro_raw);
+		const float dt = (gyro.timestamp_sample - _last_timestamp_sample_gyro) * 1e-6f;
 		_last_timestamp_sample_gyro = gyro.timestamp_sample;
 
+		_gyro_integrator.put(gyro_raw, dt);
+
 		// break if interval is configured and we haven't fallen behind
 		if (_intervals_configured && _gyro_integrator.integral_ready()
 		    && (hrt_elapsed_time(&gyro.timestamp) < _imu_integration_interval_us) && !sensor_data_gap) {
@@ -296,9 +298,11 @@ void VehicleIMU::Run()
 		_accel_temperature += accel.temperature;
 		_accel_sum_count++;
 
-		_accel_integrator.put(accel.timestamp_sample, accel_raw);
+		const float dt = (accel.timestamp_sample - _last_timestamp_sample_accel) * 1e-6f;
 		_last_timestamp_sample_accel = accel.timestamp_sample;
 
+		_accel_integrator.put(accel_raw, dt);
+
 		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
 
 			// rotate sensor clip counts into vehicle body frame

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -100,8 +100,8 @@ private:
 	calibration::Accelerometer _accel_calibration{};
 	calibration::Gyroscope _gyro_calibration{};
 
-	Integrator _accel_integrator{}; // 200 Hz default
+	Integrator       _accel_integrator{};
-	Integrator _gyro_integrator{true};   // 200 Hz default, coning compensation enabled
+	IntegratorConing _gyro_integrator{};
 
 	hrt_abstime _last_timestamp_sample_accel{0};
 	hrt_abstime _last_timestamp_sample_gyro{0};