fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-05 18:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

integrator: add a put method for known intervals

Browse Source 
This adds a second put method to the integrator class. This allows to
integrate with known intervals between samples, rather than based on
timestamps. This makes integrating the samples coming out of the MPU9250
FIFO buffer easier.
This commit is contained in:
Julian Oes
2016-05-04 22:28:19 +02:00
committed by Lorenz Meier
parent e24bef1f70
commit f528c63030
2 changed files with 36 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/device/integrator.cpp
+20
View File
@@ -111,6 +111,26 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ
}
}
bool
Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral,
uint64_t &integral_dt)
{
if (_last_integration_time == 0) {
/* this is the first item in the integrator */
uint64_t now = hrt_absolute_time();
_last_integration_time = now;
_last_reset_time = now;
_last_val = val;
return false;
}
// Create the timestamp artifically.
uint64_t timestamp = _last_integration_time + interval_us;
return put(timestamp, val, integral, integral_dt);
}
math::Vector<3>
Integrator::get(bool reset, uint64_t &integral_dt)
{
src/drivers/device/integrator.h
+16 -1
View File
@@ -60,7 +60,22 @@ public:
* @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);
bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
/**
* Put an item into the integral but provide an interval instead of a timestamp.
*
* @param interval_us Interval in us since last integration.
* @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). Note that this
* values might not be accurate vs. hrt_absolute_time because it is just the sum of the
* supplied intervals.
* @return true if putting the item triggered an integral reset and the integral should be
* published.
*/
bool put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral,
uint64_t &integral_dt);
/**
* Get the current integral and reset the integrator if needed.