diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 2fc8b30243..771d2d0851 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -32,7 +32,6 @@ ############################################################################ px4_add_library(vehicle_imu - Integrator.cpp Integrator.hpp VehicleIMU.cpp VehicleIMU.hpp diff --git a/src/modules/sensors/vehicle_imu/Integrator.cpp b/src/modules/sensors/vehicle_imu/Integrator.cpp deleted file mode 100644 index cf79744e42..0000000000 --- a/src/modules/sensors/vehicle_imu/Integrator.cpp +++ /dev/null @@ -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 - -using matrix::Vector3f; - -bool Integrator::put(const hrt_abstime ×tamp, 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(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; -} diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/vehicle_imu/Integrator.hpp index 02c8d2fd05..ede1559158 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/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 ×tamp, const matrix::Vector3f &val); - - /** - * 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 ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt) + inline void put(const matrix::Vector3f &val, const float 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: - uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */ - uint64_t _last_reset_time{0}; /**< last auto-announcement of integral value */ + reset(); - matrix::Vector3f _alpha{0.f, 0.f, 0.f}; /**< integrated value before coning corrections are applied */ - 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 */ + return true; + } - 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 */ + }; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 43b1e7cd3e..06357fc492 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/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 diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 323e1cad82..5eeb12ddbe 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/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 _gyro_integrator{true}; // 200 Hz default, coning compensation enabled + Integrator _accel_integrator{}; + IntegratorConing _gyro_integrator{}; hrt_abstime _last_timestamp_sample_accel{0}; hrt_abstime _last_timestamp_sample_gyro{0};