diff --git a/msg/sensor_gyro_fft.msg b/msg/sensor_gyro_fft.msg index 4463a000d9..31d1906d15 100644 --- a/msg/sensor_gyro_fft.msg +++ b/msg/sensor_gyro_fft.msg @@ -7,6 +7,8 @@ float32 dt # delta time between samples (microseconds) float32 resolution_hz -float32[2] peak_frequency_x # x axis peak frequencies -float32[2] peak_frequency_y # y axis peak frequencies -float32[2] peak_frequency_z # z axis peak frequencies +float32[3] peak_frequency # peak frequency per axis + +float32[4] peak_frequencies_x # x axis peak frequencies +float32[4] peak_frequencies_y # y axis peak frequencies +float32[4] peak_frequencies_z # z axis peak frequencies diff --git a/src/examples/gyro_fft/GyroFFT.cpp b/src/examples/gyro_fft/GyroFFT.cpp index 5b36d62f9d..17d8ef4043 100644 --- a/src/examples/gyro_fft/GyroFFT.cpp +++ b/src/examples/gyro_fft/GyroFFT.cpp @@ -137,8 +137,8 @@ static constexpr float tau(float x) float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index) { // find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/) - int16_t real[3] {fft[peak_index - 2], fft[peak_index], fft[peak_index + 2]}; - int16_t imag[3] {fft[peak_index - 2 + 1], fft[peak_index + 1], fft[peak_index + 2 + 1]}; + int16_t real[3] { fft[peak_index - 2], fft[peak_index], fft[peak_index + 2] }; + int16_t imag[3] { fft[peak_index - 2 + 1], fft[peak_index + 1], fft[peak_index + 2 + 1] }; const int k = 1; @@ -255,7 +255,10 @@ void GyroFFT::Run() static constexpr uint16_t MIN_SNR = 10; // TODO: - static constexpr int MAX_NUM_PEAKS = 2; + uint32_t max_peak_magnitude = 0; + uint8_t max_peak_index = 0; + + static constexpr int MAX_NUM_PEAKS = 4; uint32_t peaks_magnitude[MAX_NUM_PEAKS] {}; uint8_t peak_index[MAX_NUM_PEAKS] {}; @@ -275,6 +278,12 @@ void GyroFFT::Run() const uint32_t fft_magnitude_squared = real * real + complex * complex; if (fft_magnitude_squared > MIN_SNR) { + + if (fft_magnitude_squared > max_peak_magnitude) { + max_peak_magnitude = fft_magnitude_squared; + max_peak_index = bucket_index; + } + for (int i = 0; i < MAX_NUM_PEAKS; i++) { if (fft_magnitude_squared > peaks_magnitude[i]) { peaks_magnitude[i] = fft_magnitude_squared; @@ -287,20 +296,25 @@ void GyroFFT::Run() } } + if (max_peak_index > 0) { + _sensor_gyro_fft.peak_frequency[axis] = _median_filter[axis].apply(EstimatePeakFrequency(_fft_outupt_buffer, + max_peak_index)); + } + if (publish) { float *peak_frequencies; switch (axis) { case 0: - peak_frequencies = _sensor_gyro_fft.peak_frequency_x; + peak_frequencies = _sensor_gyro_fft.peak_frequencies_x; break; case 1: - peak_frequencies = _sensor_gyro_fft.peak_frequency_y; + peak_frequencies = _sensor_gyro_fft.peak_frequencies_y; break; case 2: - peak_frequencies = _sensor_gyro_fft.peak_frequency_z; + peak_frequencies = _sensor_gyro_fft.peak_frequencies_z; break; } diff --git a/src/examples/gyro_fft/GyroFFT.hpp b/src/examples/gyro_fft/GyroFFT.hpp index 1f98d0a068..7a2905022b 100644 --- a/src/examples/gyro_fft/GyroFFT.hpp +++ b/src/examples/gyro_fft/GyroFFT.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include #include #include @@ -111,6 +112,8 @@ private: unsigned _gyro_last_generation{0}; + math::MedianFilter _median_filter[3] {}; + sensor_gyro_fft_s _sensor_gyro_fft{}; DEFINE_PARAMETERS( diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index a2ab5ab397..f753a4bfba 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -38,5 +38,6 @@ px4_add_library(mathlib math/filter/LowPassFilter2pVector3f.cpp ) +px4_add_unit_gtest(SRC math/filter/MedianFilterTest.cpp) px4_add_unit_gtest(SRC math/filter/NotchFilterTest.cpp) px4_add_unit_gtest(SRC math/FunctionsTest.cpp) diff --git a/src/lib/mathlib/math/filter/MedianFilter.hpp b/src/lib/mathlib/math/filter/MedianFilter.hpp new file mode 100644 index 0000000000..3975661ffd --- /dev/null +++ b/src/lib/mathlib/math/filter/MedianFilter.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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 MedianFilter.hpp + * + * @brief Implementation of a median filter with C qsort. + * + */ + +#pragma once + +#include +#include +#include + +namespace math +{ + +template +class MedianFilter +{ +public: + static_assert(WINDOW >= 3, "MedianFilter window size must be >= 3"); + static_assert(WINDOW % 2, "MedianFilter window size must be odd"); // odd + + MedianFilter() = default; + + void insert(const T &sample) + { + _head = (_head + 1) % WINDOW; + _buffer[_head] = sample; + } + + T median() + { + T sorted[WINDOW]; + memcpy(sorted, _buffer, sizeof(_buffer)); + qsort(&sorted, WINDOW, sizeof(T), cmp); + + return sorted[WINDOW / 2]; + } + + T apply(const T &sample) + { + insert(sample); + return median(); + } + +private: + + static int cmp(const void *a, const void *b) + { + return (*(T *)a >= *(T *)b) ? 1 : -1; + } + + T _buffer[WINDOW] {}; + uint8_t _head{0}; +}; + +} // namespace math diff --git a/src/lib/mathlib/math/filter/MedianFilterTest.cpp b/src/lib/mathlib/math/filter/MedianFilterTest.cpp new file mode 100644 index 0000000000..29edfa2669 --- /dev/null +++ b/src/lib/mathlib/math/filter/MedianFilterTest.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +/** + * Test code for the Median filter + * Run this test only using make tests TESTFILTER=MedianFilter + */ + +#include +#include +#include + +#include "MedianFilter.hpp" + +using namespace math; +using matrix::Vector3f; + +class MedianFilterTest : public ::testing::Test +{ +public: + + +}; + +TEST_F(MedianFilterTest, test3f_simple) +{ + MedianFilter median_filter3; + + for (int i = 0; i < 3; i++) { + median_filter3.insert(i); + } + + EXPECT_EQ(median_filter3.median(), 1); // 0, 1, 2 +} + +TEST_F(MedianFilterTest, test3f_100) +{ + MedianFilter median_filter3; + + for (int i = 0; i < 100; i++) { + EXPECT_EQ(median_filter3.apply(i), max(0, i - 1)); + } +} + +TEST_F(MedianFilterTest, test5u_simple) +{ + MedianFilter median_filter5; + + for (int i = 0; i < 5; i++) { + median_filter5.insert(i); + } + + EXPECT_EQ(median_filter5.median(), 2); // 0, 1, 2, 4, 5 +} + +TEST_F(MedianFilterTest, test5i_100) +{ + MedianFilter median_filter5; + + for (int i = 0; i < 100; i++) { + EXPECT_EQ(median_filter5.apply(i), max(0, i - 2)); + } +}