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

sensors/vehicle_angular_velocity: use full raw FIFO data (sensor_gyro_fifo) if available

Browse Source
This commit is contained in:
Daniel Agar
2021-02-25 10:06:17 -05:00
committed by GitHub
parent 8f625e5744
commit 6482120d9a
10 changed files with 323 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
-21
View File
@@ -33,8 +33,6 @@
#include "LowPassFilter2p.hpp"
#include <px4_platform_common/defines.h>
#include <math.h>
namespace math
@@ -72,25 +70,6 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}
float LowPassFilter2p::apply(float sample)
{
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (!PX4_ISFINITE(delay_element_0)) {
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
// return the value. Should be no need to check limits
return output;
}
float LowPassFilter2p::reset(float sample)
{
const float dval = sample / (_b0 + _b1 + _b2);
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+19 -1
View File
@@ -38,6 +38,8 @@
#pragma once
#include <px4_platform_common/defines.h>
namespace math
{
class __EXPORT LowPassFilter2p
@@ -58,7 +60,23 @@ public:
*
* @return retrieve the filtered result
*/
float apply(float sample);
inline float apply(float sample)
{
// Direct Form II implementation
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (!PX4_ISFINITE(delay_element_0)) {
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
return output;
}
// Return the cutoff frequency
float get_cutoff_freq() const { return _cutoff_freq; }
src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp
+9 -21
View File
@@ -51,35 +51,23 @@ public:
{
}
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
inline float apply(const int16_t samples[], uint8_t num_samples)
// Filter array of samples in place using the Direct form II.
inline void apply(float samples[], uint8_t num_samples)
{
float output = 0.0f;
for (int n = 0; n < num_samples; n++) {
// do the filtering
float delay_element_0 = samples[n] - _delay_element_1 * _a1 - _delay_element_2 * _a2;
// Direct Form II implementation
float delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2};
if (n == num_samples - 1) {
output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
// don't allow bad values to propagate via the filter
if (!PX4_ISFINITE(delay_element_0)) {
delay_element_0 = samples[n];
}
samples[n] = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
}
// don't allow bad values to propagate via the filter
if (!PX4_ISFINITE(output)) {
reset(samples[num_samples - 1]);
output = samples[num_samples - 1];
}
// return the value. Should be no need to check limits
return output;
}
};
src/lib/mathlib/math/filter/NotchFilterArray.hpp
+3 -7
View File
@@ -64,11 +64,7 @@ public:
NotchFilterArray() = default;
~NotchFilterArray() = default;
/**
* Add new raw values to the filter using the Direct form II.
*
* @return retrieve the filtered result
*/
// Filter array of samples in place using the Direct form II.
inline void apply(T samples[], uint8_t num_samples)
{
for (int n = 0; n < num_samples; n++) {
@@ -96,8 +92,8 @@ public:
{
for (int n = 0; n < num_samples; n++) {
// Direct Form II implementation
const T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 -
_a2 * _delay_element_output_2;
T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 -
_a2 * _delay_element_output_2;
// don't allow bad values to propagate via the filter
if (!isFinite(output)) {