fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 16:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

integrator: improvements and cleanup

Browse Source 
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.
This commit is contained in:
Julian Oes
2016-04-23 08:41:54 +02:00
committed by Lorenz Meier
parent f91be1e281
commit 302bf879de
2 changed files with 67 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/device/integrator.cpp
+45 -39
View File
@@ -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;
_last_delta = delta;
if ((timestamp - _last_auto) > _auto_reset_interval) {
if (_auto_callback) {
/* call the callback */
_auto_callback(timestamp, _integral_auto);
}
// Only do auto reset if auto reset interval is not 0.
if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) > _auto_reset_interval) {
integral = _integral_auto;
integral_dt = (timestamp - _last_auto);
integral = _integral;
_reset(integral_dt);
auto_reset = true;
_last_auto = timestamp;
_integral_auto(0) = 0.0f;
_integral_auto(1) = 0.0f;
_integral_auto(2) = 0.0f;
return true;
} else {
return false;
}
return auto_reset;
}
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;
}