dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mathlib: merge LowPassFilter2p, LowPassFilter2pArray, and LowPassFilter2pVector3f 

- fix reset for direct form 2
 - push NAN checks out to filter users
This commit is contained in:
Daniel Agar 2021-05-25 13:45:24 -04:00
parent 2a792ca201
commit 5493d96d17
23 changed files with 210 additions and 483 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
View File

@ -92,7 +92,7 @@ protected:
int measure() override;
int collect() override;
math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
math::LowPassFilter2p<float> _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
};
/*
@ -196,16 +196,18 @@ MEASAirspeed::collect()
and bottom port is used as the static port
*/
differential_pressure_s report{};
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s report{};
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
_airspeed_pub.publish(report);
}
ret = OK;

20
src/drivers/differential_pressure/ms5525/MS5525.cpp
View File

@ -250,16 +250,18 @@ MS5525::collect()
const float temperature_c = TEMP * 0.01f;
differential_pressure_s diff_pressure = {
.timestamp = hrt_absolute_time(),
.error_count = perf_event_count(_comms_errors),
.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset,
.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset,
.temperature = temperature_c,
.device_id = _device_id.devid
};
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s diff_pressure{};
_airspeed_pub.publish(diff_pressure);
diff_pressure.error_count = perf_event_count(_comms_errors);
diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
diff_pressure.temperature = temperature_c;
diff_pressure.device_id = _device_id.devid;
diff_pressure.timestamp = hrt_absolute_time();
_airspeed_pub.publish(diff_pressure);
}
ret = OK;

2
src/drivers/differential_pressure/ms5525/MS5525.hpp
View File

@ -71,7 +71,7 @@ private:
int collect() override;
// temperature is read once every 10 cycles
math::LowPassFilter2p _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ};
math::LowPassFilter2p<float> _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ};
static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command
static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command

18
src/drivers/differential_pressure/sdp3x/SDP3X.cpp
View File

@ -156,16 +156,18 @@ SDP3X::collect()
float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
differential_pressure_s report{};
if (PX4_ISFINITE(diff_press_pa_raw)) {
differential_pressure_s report{};
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature_c;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature_c;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
_airspeed_pub.publish(report);
_airspeed_pub.publish(report);
}
perf_end(_sample_perf);

2
src/drivers/differential_pressure/sdp3x/SDP3X.hpp
View File

@ -100,7 +100,7 @@ private:
int configure();
int read_scale();
math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
math::LowPassFilter2p<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
bool init_sdp3x();

19
src/drivers/uavcan/sensors/differential_pressure.cpp
View File

@ -75,14 +75,15 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
float diff_press_pa = msg.differential_pressure;
float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
differential_pressure_s report = {
.timestamp = hrt_absolute_time(),
.error_count = 0,
.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset,
.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset, /// TODO: Create filter
.temperature = temperature_c,
.device_id = _device_id.devid
};
if (PX4_ISFINITE(diff_press_pa)) {
differential_pressure_s report{};
publish(msg.getSrcNodeID().get(), &report);
report.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset;
report.temperature = temperature_c;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
publish(msg.getSrcNodeID().get(), &report);
}
}

2
src/drivers/uavcan/sensors/differential_pressure.hpp
View File

@ -59,7 +59,7 @@ public:
private:
float _diff_pres_offset{0.f};
math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
math::LowPassFilter2p<float> _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
void air_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);

2
src/lib/controllib/BlockLowPass2.hpp
View File

@ -80,7 +80,7 @@ protected:
float _state;
control::BlockParamFloat _fCut;
float _fs;
math::LowPassFilter2p _lp;
math::LowPassFilter2p<float> _lp;
};
} // namespace control

5
src/lib/mathlib/CMakeLists.txt
View File

@ -33,8 +33,9 @@
px4_add_library(mathlib
math/test/test.cpp
math/filter/LowPassFilter2p.cpp
math/filter/LowPassFilter2pVector3f.cpp
math/filter/LowPassFilter2p.hpp
math/filter/MedianFilter.hpp
math/filter/NotchFilter.hpp
)
px4_add_unit_gtest(SRC math/filter/LowPassFilter2pVector3fTest.cpp LINKLIBS mathlib)

11
src/lib/mathlib/math/Functions.hpp
View File

@ -41,6 +41,7 @@
#include "Limits.hpp"
#include <px4_platform_common/defines.h>
#include <matrix/matrix/math.hpp>
namespace math
@ -223,4 +224,14 @@ constexpr int16_t negate<int16_t>(int16_t value)
return (value == INT16_MIN) ? INT16_MAX : -value;
}
inline bool isFinite(const float &value)
{
return PX4_ISFINITE(value);
}
inline bool isFinite(const matrix::Vector3f &value)
{
return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));
}
} /* namespace math */

90
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -1,90 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "LowPassFilter2p.hpp"
#include <math.h>
namespace math
{
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
_sample_freq = sample_freq;
// reset delay elements on filter change
_delay_element_1 = 0.0f;
_delay_element_2 = 0.0f;
if (_cutoff_freq <= 0.0f) {
// no filtering
_b0 = 1.0f;
_b1 = 0.0f;
_b2 = 0.0f;
_a1 = 0.0f;
_a2 = 0.0f;
return;
}
const float fr = sample_freq / _cutoff_freq;
const float ohm = tanf(M_PI_F / fr);
const float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
_b0 = ohm * ohm / c;
_b1 = 2.0f * _b0;
_b2 = _b0;
_a1 = 2.0f * (ohm * ohm - 1.0f) / c;
_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}
float LowPassFilter2p::reset(float sample)
{
const float dval = sample / (_b0 + _b1 + _b2);
if (PX4_ISFINITE(dval)) {
_delay_element_1 = dval;
_delay_element_2 = dval;
} else {
_delay_element_1 = sample;
_delay_element_2 = sample;
}
return apply(sample);
}
} // namespace math

112
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012-2021 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
@ -31,21 +31,24 @@
*
****************************************************************************/
/// @file LowPassFilter.h
/// @file LowPassFilter2p.hpp
/// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com>
/// Adapted for PX4 by Andrew Tridgell
#pragma once
#include <px4_platform_common/defines.h>
#include <mathlib/math/Functions.hpp>
#include <float.h>
#include <matrix/math.hpp>
namespace math
{
class __EXPORT LowPassFilter2p
template<typename T>
class LowPassFilter2p
{
public:
LowPassFilter2p(float sample_freq, float cutoff_freq)
{
// set initial parameters
@ -53,24 +56,49 @@ public:
}
// Change filter parameters
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
void set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2)
|| !isFinite(sample_freq) || !isFinite(cutoff_freq)) {
disable();
return;
}
// reset delay elements on filter change
_delay_element_1 = {};
_delay_element_2 = {};
_cutoff_freq = math::constrain(cutoff_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
_sample_freq = sample_freq;
const float fr = sample_freq / _cutoff_freq;
const float ohm = tanf(M_PI_F / fr);
const float c = 1.f + 2.f * cosf(M_PI_F / 4.f) * ohm + ohm * ohm;
_b0 = ohm * ohm / c;
_b1 = 2.f * _b0;
_b2 = _b0;
_a1 = 2.f * (ohm * ohm - 1.f) / c;
_a2 = (1.f - 2.f * cosf(M_PI_F / 4.f) * ohm + ohm * ohm) / c;
if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) {
disable();
}
}
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
inline float apply(float sample)
inline T apply(const T &sample)
{
// Direct Form II implementation
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
T 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;
const T 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;
@ -78,19 +106,63 @@ public:
return output;
}
// Filter array of samples in place using the Direct form II.
inline void applyArray(T samples[], int num_samples)
{
for (int n = 0; n < num_samples; n++) {
samples[n] = apply(samples[n]);
}
}
// Return the cutoff frequency
float get_cutoff_freq() const { return _cutoff_freq; }
// Return the sample frequency
float get_sample_freq() const { return _sample_freq; }
float getMagnitudeResponse(float frequency) const;
// Reset the filter state to this value
float reset(float sample);
T reset(const T &sample)
{
const T input = isFinite(sample) ? sample : T{};
if (fabsf(1 + _a1 + _a2) > FLT_EPSILON) {
_delay_element_1 = _delay_element_2 = input / (1 + _a1 + _a2);
if (!isFinite(_delay_element_1) || !isFinite(_delay_element_2)) {
_delay_element_1 = _delay_element_2 = input;
}
} else {
_delay_element_1 = _delay_element_2 = input;
}
return apply(input);
}
void disable()
{
// no filtering
_sample_freq = 0.f;
_cutoff_freq = 0.f;
_delay_element_1 = {};
_delay_element_2 = {};
_b0 = 1.f;
_b1 = 0.f;
_b2 = 0.f;
_a1 = 0.f;
_a2 = 0.f;
}
protected:
T _delay_element_1{}; // buffered sample -1
T _delay_element_2{}; // buffered sample -2
float _cutoff_freq{0.f};
float _sample_freq{0.f};
// All the coefficients are normalized by a0, so a0 becomes 1 here
float _a1{0.f};
float _a2{0.f};
@ -98,8 +170,8 @@ protected:
float _b1{0.f};
float _b2{0.f};
float _delay_element_1{0.f}; // buffered sample -1
float _delay_element_2{0.f}; // buffered sample -2
float _cutoff_freq{0.f};
float _sample_freq{0.f};
};
} // namespace math

75
src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp
View File

@ -1,75 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/// @file LowPassFilter2pArray.hpp
/// @brief A class to implement a second order low pass filter
#pragma once
#include "LowPassFilter2p.hpp"
#include <px4_platform_common/defines.h>
namespace math
{
class LowPassFilter2pArray : public LowPassFilter2p
{
public:
LowPassFilter2pArray(float sample_freq, float cutoff_freq) : LowPassFilter2p(sample_freq, cutoff_freq)
{
}
// Filter array of samples in place using the Direct form II.
inline void apply(float samples[], uint8_t num_samples)
{
for (int n = 0; n < num_samples; n++) {
// Direct Form II implementation
float delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2};
// 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;
}
}
};
} // namespace math

92
src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp
View File

@ -1,92 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2018 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "LowPassFilter2pVector3f.hpp"
#include <px4_platform_common/defines.h>
#include <math.h>
namespace math
{
void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
_sample_freq = sample_freq;
// reset delay elements on filter change
_delay_element_1.zero();
_delay_element_2.zero();
if (_cutoff_freq <= 0.0f) {
// no filtering
_b0 = 1.0f;
_b1 = 0.0f;
_b2 = 0.0f;
_a1 = 0.0f;
_a2 = 0.0f;
return;
}
const float fr = sample_freq / _cutoff_freq;
const float ohm = tanf(M_PI_F / fr);
const float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
_b0 = ohm * ohm / c;
_b1 = 2.0f * _b0;
_b2 = _b0;
_a1 = 2.0f * (ohm * ohm - 1.0f) / c;
_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}
matrix::Vector3f LowPassFilter2pVector3f::reset(const matrix::Vector3f &sample)
{
const matrix::Vector3f dval = sample / (_b0 + _b1 + _b2);
if (PX4_ISFINITE(dval(0)) && PX4_ISFINITE(dval(1)) && PX4_ISFINITE(dval(2))) {
_delay_element_1 = dval;
_delay_element_2 = dval;
} else {
_delay_element_1 = sample;
_delay_element_2 = sample;
}
return apply(sample);
}
} // namespace math

99
src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp
View File

@ -1,99 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2018 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/// @file LowPassFilter2pVector3f.hpp
/// @brief A class to implement a second order low pass filter on a Vector3f
/// Based on LowPassFilter2p.hpp by Leonard Hall <LeonardTHall@gmail.com>
#pragma once
#include <matrix/math.hpp>
namespace math
{
class LowPassFilter2pVector3f
{
public:
LowPassFilter2pVector3f(float sample_freq, float cutoff_freq)
{
// set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq);
}
// Change filter parameters
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
inline matrix::Vector3f apply(const matrix::Vector3f &sample)
{
// do the filtering
const matrix::Vector3f delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
const matrix::Vector3f 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; }
// Return the sample frequency
float get_sample_freq() const { return _sample_freq; }
// Reset the filter state to this value
matrix::Vector3f reset(const matrix::Vector3f &sample);
private:
float _cutoff_freq{0.0f};
float _sample_freq{0.0f};
float _a1{0.0f};
float _a2{0.0f};
float _b0{0.0f};
float _b1{0.0f};
float _b2{0.0f};
matrix::Vector3f _delay_element_1{0.0f, 0.0f, 0.0f}; // buffered sample -1
matrix::Vector3f _delay_element_2{0.0f, 0.0f, 0.0f}; // buffered sample -2
};
} // namespace math

4
src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp
View File

@ -40,7 +40,7 @@
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/defines.h>
#include "LowPassFilter2pVector3f.hpp"
#include "LowPassFilter2p.hpp"
using matrix::Vector3f;
@ -49,7 +49,7 @@ class LowPassFilter2pVector3fTest : public ::testing::Test
public:
void runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg, const Vector3f &gain_db);
math::LowPassFilter2pVector3f _lpf{800.f, 30.f};
math::LowPassFilter2p<Vector3f> _lpf{800.f, 30.f};
const float _epsilon_near = 0.08f;
};

13
src/lib/mathlib/math/filter/NotchFilter.hpp
View File

@ -42,7 +42,7 @@
#pragma once
#include <px4_platform_common/defines.h>
#include <mathlib/math/Functions.hpp>
#include <cmath>
#include <float.h>
#include <matrix/math.hpp>
@ -50,16 +50,6 @@
namespace math
{
inline bool isFinite(const float &value)
{
return PX4_ISFINITE(value);
}
inline bool isFinite(const matrix::Vector3f &value)
{
return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));
}
template<typename T>
class NotchFilter
{
@ -67,7 +57,6 @@ public:
NotchFilter() = default;
~NotchFilter() = default;
void setParameters(float sample_freq, float notch_freq, float bandwidth);
/**

1
src/modules/angular_velocity_controller/AngularVelocityController.hpp
View File

@ -35,7 +35,6 @@
#include <AngularVelocityControl.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>

1
src/modules/mc_rate_control/RateControl/RateControl.hpp
View File

@ -40,7 +40,6 @@
#pragma once
#include <matrix/matrix/math.hpp>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
#include <uORB/topics/rate_ctrl_status.h>

35
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -227,26 +227,29 @@ void VehicleAcceleration::Run()
sensor_accel_s sensor_data;
while (_sensor_sub.update(&sensor_data)) {
const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z};
// Apply calibration and filter
// - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
// - biquad low-pass filter
const Vector3f accel_corrected = _calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias;
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
if (math::isFinite(accel_raw)) {
// Apply calibration and filter
// - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
// - biquad low-pass filter
const Vector3f accel_corrected = _calibration.Correct(accel_raw) - _bias;
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
_acceleration_prev = accel_corrected;
_acceleration_prev = accel_corrected;
// publish once all new samples are processed
if (!_sensor_sub.updated()) {
// Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel_filtered.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration);
// publish once all new samples are processed
if (!_sensor_sub.updated()) {
// Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel_filtered.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration);
return;
return;
}
}
}
}

4
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View File

@ -36,7 +36,7 @@
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@ -96,7 +96,7 @@ private:
static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
float _filter_sample_rate{NAN};
math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.f};
math::LowPassFilter2p<matrix::Vector3f> _lp_filter{kInitialRateHz, 30.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,

61
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -617,7 +617,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
}
// Apply general low-pass filter (IMU_GYRO_CUTOFF)
_lp_filter_velocity[axis].apply(data, N);
_lp_filter_velocity[axis].applyArray(data, N);
// return last filtered sample
return data[N - 1];
@ -709,40 +709,43 @@ void VehicleAngularVelocity::Run()
sensor_gyro_s sensor_data;
while (_sensor_sub.update(&sensor_data)) {
if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {
_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
}
if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) {
const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), 0.00002f, 0.02f);
_timestamp_sample_last = sensor_data.timestamp_sample;
if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {
_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
}
if (_reset_filters) {
ResetFilters();
const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), 0.00002f, 0.02f);
_timestamp_sample_last = sensor_data.timestamp_sample;
if (_reset_filters) {
continue; // not safe to run until filters configured
ResetFilters();
if (_reset_filters) {
continue; // not safe to run until filters configured
}
}
UpdateDynamicNotchEscRpm();
UpdateDynamicNotchFFT();
Vector3f angular_velocity;
Vector3f angular_acceleration;
float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
for (int axis = 0; axis < 3; axis++) {
// copy sensor sample to float array for filtering
float data[1] {raw_data_array[axis]};
// save last filtered sample
angular_velocity(axis) = FilterAngularVelocity(axis, data);
angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data);
}
// Publish
CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration);
}
UpdateDynamicNotchEscRpm();
UpdateDynamicNotchFFT();
Vector3f angular_velocity;
Vector3f angular_acceleration;
float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
for (int axis = 0; axis < 3; axis++) {
// copy sensor sample to float array for filtering
float data[1] {raw_data_array[axis]};
// save last filtered sample
angular_velocity(axis) = FilterAngularVelocity(axis, data);
angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data);
}
// Publish
CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration);
}
}
}

5
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -38,7 +38,6 @@
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/NotchFilter.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
@ -134,7 +133,7 @@ private:
static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
// angular velocity filters
math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::LowPassFilter2p<float> _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::NotchFilter<float> _notch_filter_velocity[3] {};
#if !defined(CONSTRAINED_FLASH)
@ -168,7 +167,7 @@ private:
#endif // !CONSTRAINED_FLASH
// angular acceleration filter
math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::LowPassFilter2p<float> _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
uint32_t _selected_sensor_device_id{0};