diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp index f5b982f682..f4c221d2b0 100644 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp @@ -92,7 +92,7 @@ protected: int measure() override; int collect() override; - math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ}; + math::LowPassFilter2p _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; diff --git a/src/drivers/differential_pressure/ms5525/MS5525.cpp b/src/drivers/differential_pressure/ms5525/MS5525.cpp index aa5e3f6ea5..c5debb50af 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.cpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.cpp @@ -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; diff --git a/src/drivers/differential_pressure/ms5525/MS5525.hpp b/src/drivers/differential_pressure/ms5525/MS5525.hpp index de05204123..4b4c8d4905 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.hpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.hpp @@ -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 _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 diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index 4d7a89d9de..2655308d7c 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -156,16 +156,18 @@ SDP3X::collect() float diff_press_pa_raw = static_cast(P) / static_cast(_scale); float temperature_c = temp / static_cast(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); diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index 755d42429c..cc7685f33a 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -100,7 +100,7 @@ private: int configure(); int read_scale(); - math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ}; + math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ}; bool init_sdp3x(); diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp index 8ce01b2715..32f6135cee 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.cpp +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -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); + } } diff --git a/src/drivers/uavcan/sensors/differential_pressure.hpp b/src/drivers/uavcan/sensors/differential_pressure.hpp index 5364940e17..a862b2dd34 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.hpp +++ b/src/drivers/uavcan/sensors/differential_pressure.hpp @@ -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 _filter{10.f, 1.1f}; /// Adapted from MS5525 driver void air_sub_cb(const uavcan::ReceivedDataStructure &msg); diff --git a/src/lib/controllib/BlockLowPass2.hpp b/src/lib/controllib/BlockLowPass2.hpp index 98132ecc76..364fe295b0 100644 --- a/src/lib/controllib/BlockLowPass2.hpp +++ b/src/lib/controllib/BlockLowPass2.hpp @@ -80,7 +80,7 @@ protected: float _state; control::BlockParamFloat _fCut; float _fs; - math::LowPassFilter2p _lp; + math::LowPassFilter2p _lp; }; } // namespace control diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 27f3783d24..900d7feac7 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -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) diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 346555687b..dafa172701 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -41,6 +41,7 @@ #include "Limits.hpp" +#include #include namespace math @@ -223,4 +224,14 @@ constexpr int16_t negate(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 */ diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp deleted file mode 100644 index d9a69bb8c1..0000000000 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ /dev/null @@ -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 - -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 diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index a2bc7c4f7e..d77da0dac3 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -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 /// Adapted for PX4 by Andrew Tridgell #pragma once -#include +#include +#include +#include namespace math { -class __EXPORT LowPassFilter2p + +template +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 diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp deleted file mode 100644 index 28846fcd0a..0000000000 --- a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp +++ /dev/null @@ -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 - -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 diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp deleted file mode 100644 index 44927628ce..0000000000 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp +++ /dev/null @@ -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 - -#include - -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 diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp deleted file mode 100644 index ec8b0aa218..0000000000 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp +++ /dev/null @@ -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 - -#pragma once - -#include - -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 diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp index 0f772b6283..c08869ca6f 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp @@ -40,7 +40,7 @@ #include #include -#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 _lpf{800.f, 30.f}; const float _epsilon_near = 0.08f; }; diff --git a/src/lib/mathlib/math/filter/NotchFilter.hpp b/src/lib/mathlib/math/filter/NotchFilter.hpp index fbe75df54f..0049022a72 100644 --- a/src/lib/mathlib/math/filter/NotchFilter.hpp +++ b/src/lib/mathlib/math/filter/NotchFilter.hpp @@ -42,7 +42,7 @@ #pragma once -#include +#include #include #include #include @@ -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 class NotchFilter { @@ -67,7 +57,6 @@ public: NotchFilter() = default; ~NotchFilter() = default; - void setParameters(float sample_freq, float notch_freq, float bandwidth); /** diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/modules/angular_velocity_controller/AngularVelocityController.hpp index a31ae859be..c80529753e 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.hpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.hpp @@ -35,7 +35,6 @@ #include -#include #include #include #include diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index 6f0783a349..a39dabb194 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -40,7 +40,6 @@ #pragma once #include -#include #include #include diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index b064627feb..b050b696df 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -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; + } } } } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index b47ec476c7..3b3a5863dc 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 _lp_filter{kInitialRateHz, 30.f}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_accel_cutoff, diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 6e110f19b2..790f2a70bf 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -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); } } } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 23d13ca524..023ad5877c 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include #include @@ -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 _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; math::NotchFilter _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 _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; uint32_t _selected_sensor_device_id{0};