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

PX4 sensor driver helpers

Browse Source
This commit is contained in:
Daniel Agar
2019-01-18 10:39:15 -05:00
parent a2e9d9ffce
commit 91dcfb7ab2
12 changed files with 826 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/device/integrator.cpp
+8 -9
View File
@@ -44,14 +44,15 @@
#include <drivers/drv_hrt.h>
Integrator::Integrator(uint64_t auto_reset_interval, bool coning_compensation) :
_auto_reset_interval(auto_reset_interval),
Integrator::Integrator(uint32_t auto_reset_interval, bool coning_compensation) :
_coning_comp_on(coning_compensation)
{
set_autoreset_interval(auto_reset_interval);
}
bool
Integrator::put(uint64_t timestamp, matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
Integrator::put(const uint64_t &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral,
uint32_t &integral_dt)
{
if (_last_integration_time == 0) {
/* this is the first item in the integrator */
@@ -72,7 +73,7 @@ Integrator::put(uint64_t timestamp, matrix::Vector3f &val, matrix::Vector3f &int
}
// Use trapezoidal integration to calculate the delta integral
matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f;
const matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f;
_last_val = val;
// Calculate coning corrections if required
@@ -128,7 +129,7 @@ Integrator::put_with_interval(unsigned interval_us, matrix::Vector3f &val, matri
}
// Create the timestamp artifically.
uint64_t timestamp = _last_integration_time + interval_us;
const uint64_t timestamp = _last_integration_time + interval_us;
return put(timestamp, val, integral, integral_dt);
}
@@ -149,12 +150,10 @@ matrix::Vector3f
Integrator::get_and_filtered(bool reset, uint32_t &integral_dt, matrix::Vector3f &filtered_val)
{
// Do the usual get with reset first but don't return yet.
matrix::Vector3f ret_integral = get(reset, integral_dt);
const matrix::Vector3f 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;
filtered_val = ret_integral * 1000000 / integral_dt;
return ret_integral;
}
src/lib/drivers/device/integrator.h
+5 -5
View File
@@ -48,7 +48,7 @@
class Integrator
{
public:
Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
Integrator(uint32_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
~Integrator() = default;
// no copy, assignment, move, move assignment
@@ -67,7 +67,7 @@ public:
* @return true if putting the item triggered an integral reset and the integral should be
* published.
*/
bool put(uint64_t timestamp, matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt);
bool put(const uint64_t &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt);
/**
* Put an item into the integral but provide an interval instead of a timestamp.
@@ -108,12 +108,12 @@ public:
/**
* Set auto reset interval during runtime. This won't reset the integrator.
*
* @param auto_reset_interval New reset time interval for the integrator.
* @param auto_reset_interval New reset time interval for the integrator (+- 10%).
*/
void set_autoreset_interval(uint64_t auto_reset_interval) { _auto_reset_interval = auto_reset_interval; }
void set_autoreset_interval(uint32_t auto_reset_interval) { _auto_reset_interval = 0.90f * auto_reset_interval; }
private:
uint64_t _auto_reset_interval{0}; /**< the interval after which the content will be published
uint32_t _auto_reset_interval{0}; /**< the interval after which the content will be published
and the integrator reset, 0 if no auto-reset */
uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */