dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_imu: Integrator simplify and make header only

This commit is contained in:
Daniel Agar 2021-03-08 15:01:18 -05:00
parent cf5e6e4133
commit 0b9e4a5902
5 changed files with 128 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/sensors/vehicle_imu/CMakeLists.txt
View File

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

97
src/modules/sensors/vehicle_imu/Integrator.cpp
View File

@ -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;
}

148
src/modules/sensors/vehicle_imu/Integrator.hpp
View File

@ -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);
/**
* 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)
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 */
};

10
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -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

6
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -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};