mathlib: merge LowPassFilter2p, LowPassFilter2pArray, and LowPassFilter2pVector3f

- fix reset for direct form 2
 - push NAN checks out to filter users

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<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;
 

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
-	};
-
-	_airspeed_pub.publish(diff_pressure);
+	if (PX4_ISFINITE(diff_press_pa_raw)) {
+		differential_pressure_s 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;
 

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<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

src/drivers/differential_pressure/sdp3x/SDP3X.cpp

@@ -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);
 

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<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
 
 	bool init_sdp3x();
 

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);
+	}
 }

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<float> _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
 
 	void air_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);
 

src/lib/controllib/BlockLowPass2.hpp

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

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)

src/lib/mathlib/math/Functions.hpp

@@ -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 */

src/lib/mathlib/math/filter/LowPassFilter2p.cpp

@@ -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

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 <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;
-
-		if (!PX4_ISFINITE(delay_element_0)) {
-			// don't allow bad values to propagate via the filter
-			delay_element_0 = sample;
-		}
+		T delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
 
-		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{};
 
-protected:
+		if (fabsf(1 + _a1 + _a2) > FLT_EPSILON) {
+			_delay_element_1 = _delay_element_2 = input / (1 + _a1 + _a2);
 
-	float _cutoff_freq{0.f};
-	float _sample_freq{0.f};
+			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
+
+	// 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

src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp

@@ -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

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp

@@ -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

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp

@@ -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

src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp

@@ -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;
 };

src/lib/mathlib/math/filter/NotchFilter.hpp

@@ -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);
 
 	/**

src/modules/angular_velocity_controller/AngularVelocityController.hpp

@@ -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>

src/modules/mc_rate_control/RateControl/RateControl.hpp

@@ -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>

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)) {
-
-		// 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);
-
-		_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);
-
-			return;
+		const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z};
+
+		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;
+
+			// 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;
+			}
 		}
 	}
 }

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -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,

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();
+				UpdateDynamicNotchEscRpm();
+				UpdateDynamicNotchFFT();
 
-			Vector3f angular_velocity;
-			Vector3f angular_acceleration;
+				Vector3f angular_velocity;
+				Vector3f angular_acceleration;
 
-			float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
+				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]};
+				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);
-			}
+					// 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);
+				// Publish
+				CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration);
+			}
 		}
 	}
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -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};