mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-08 10:50:19 +08:00
integrator: add function to return filtered data
Instead of only being able to get the integral and its integration time, it can also be handy to get the integral divided/differentiated by the the integration time. This data is then just filtered by the integrator.
This commit is contained in:
@@ -143,6 +143,20 @@ Integrator::get(bool reset, uint64_t &integral_dt)
|
||||
return val;
|
||||
}
|
||||
|
||||
math::Vector<3>
|
||||
Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val)
|
||||
{
|
||||
// Do the usual get with reset first but don't return yet.
|
||||
math::Vector<3> ret_integral = get(reset, integral_dt);
|
||||
|
||||
// Because we need both the integral and the integral_dt.
|
||||
filtered_val(0) = ret_integral(0) * 1000000 / integral_dt;
|
||||
filtered_val(1) = ret_integral(1) * 1000000 / integral_dt;
|
||||
filtered_val(2) = ret_integral(2) * 1000000 / integral_dt;
|
||||
|
||||
return ret_integral;
|
||||
}
|
||||
|
||||
void
|
||||
Integrator::_reset(uint64_t &integral_dt)
|
||||
{
|
||||
|
||||
@@ -86,6 +86,17 @@ public:
|
||||
*/
|
||||
math::Vector<3> get(bool reset, uint64_t &integral_dt);
|
||||
|
||||
/**
|
||||
* Get the current integral and reset the integrator if needed. Additionally give the
|
||||
* integral over the samples differentiated by the integration time (mean filtered values).
|
||||
*
|
||||
* @param reset Reset the integral to zero.
|
||||
* @param integral_dt Get the dt in us of the current integration (only if reset).
|
||||
* @param filtered_val The integral differentiated by the integration time.
|
||||
* @return the integral since the last read-reset
|
||||
*/
|
||||
math::Vector<3> get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val);
|
||||
|
||||
private:
|
||||
uint64_t _auto_reset_interval; /**< the interval after which the content will be published
|
||||
and the integrator reset, 0 if no auto-reset */
|
||||
|
||||
Reference in New Issue
Block a user