From 4bf4dcda57800e1b01d20c4eac479dc5bcb2b5c1 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Sun, 17 Sep 2023 18:43:54 +0200 Subject: [PATCH 01/24] [FFT] implemented FFT (needs to be tested) --- CMakeLists.txt | 3 + src/entrypoints/fft-benchmark.cpp | 57 +++++++++++++ src/shared/algorithms/FFT.h | 130 ++++++++++++++++++++++++++++++ 3 files changed, 190 insertions(+) create mode 100644 src/entrypoints/fft-benchmark.cpp create mode 100644 src/shared/algorithms/FFT.h diff --git a/CMakeLists.txt b/CMakeLists.txt index aaba63944..437c8fb84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,6 +57,9 @@ sbs_target(runcam-settings stm32f407vg_stm32f4discovery) add_executable(sdcard-benchmark src/entrypoints/sdcard-benchmark.cpp) sbs_target(sdcard-benchmark stm32f429zi_skyward_death_stack_x) +add_executable(fft-benchmark src/entrypoints/fft-benchmark.cpp) +sbs_target(fft-benchmark stm32f407vg_stm32f4discovery) + add_executable(sx1278fsk-ra01-serial src/entrypoints/sx1278-serial.cpp) target_compile_definitions(sx1278fsk-ra01-serial PRIVATE SX1278_IS_FSK) sbs_target(sx1278fsk-ra01-serial stm32f429zi_skyward_groundstation_v2) diff --git a/src/entrypoints/fft-benchmark.cpp b/src/entrypoints/fft-benchmark.cpp new file mode 100644 index 000000000..c7c80730b --- /dev/null +++ b/src/entrypoints/fft-benchmark.cpp @@ -0,0 +1,57 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <iostream> +#include <Eigen/Dense> +#include <miosix.h> +#include <complex.h> +#include <math.h> +#include <algorithms/FFT.h> + +using namespace Boardcore; + +int main(int argc, char const *argv[]) +{ + Eigen::Vector<float, 256> input_signal = Eigen::Vector<float, 256>::Zero(); + float f1, f2; + + f1 = 50; + f2 = 25; + + for (size_t i = 0; i < input_signal.size(); i++) + { + input_signal(i) = sin(2 * M_PI * i / 256 * f1) + sin(2 * M_PI * i / 256 * f2); + } + + Eigen::Vector<std::complex<float>, 256> fft_result = FFT<256>::fft(input_signal); + Eigen::Vector<float, 256> fft_freq = FFT<256>::fftfreq(1.0 / 256.0); + + std::cout << "FFT result:" << std::endl; + for (size_t i = 0; i < fft_result.size(); i++) + { + std::cout << fft_freq(i) << ' ' << fft_result(i).real() << std::endl; + } + + return 0; +} diff --git a/src/shared/algorithms/FFT.h b/src/shared/algorithms/FFT.h new file mode 100644 index 000000000..0eed6ced5 --- /dev/null +++ b/src/shared/algorithms/FFT.h @@ -0,0 +1,130 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <complex.h> +#include <Eigen/Dense> + +namespace Boardcore +{ + +typedef FFT<32> FFT32; +typedef FFT<64> FFT64; +typedef FFT<128> FFT128; +typedef FFT<256> FFT256; + +/** + * @brief Implementation of Fast Fourier Trasnform using the iterative version + * with bit-reversal index variant of the famous Cooley-Tukey FFT algorithm. + * + * NOTE: N must be a power of 2 +*/ +template <int N_size> +class FFT +{ +public: + using VectorHF = Eigen::Vector<float, N_size / 2>; + using VectorF = Eigen::Vector<float, N_size>; + using VectorCF = Eigen::Vector<std::complex<float>, N_size>; + + /** + * @brief Perform the FFT on the input signal + * + * @param input_signal NOTE: MUST BE POWER OF 2 + * @return Eigen::Vector<std::complex<float>, N_size> + */ + static const VectorCF fft(VectorF input_signal) + { + size_t rev_i; + int m, omega; + float omega_m; + std::complex<float> t, u; + + // Bit-reversal permutation + VectorCF phasors = VectorCF::Zero(); + for (int i = 0; i < N_size; i++) + { + rev_i = reverse_bits(i); + phasors(rev_i) = std::complex<float>(input_signal(i), 0); + } + + // Cooley-Tukey FFT algorithm + for (int s = 0, s <= n_bits; s++) + { + m = powl(2, s); + omega_m = cexpf(-2 * I / m); + for (int k = 0; k < n; k += m) + { + omega = std::complex<float>(1, 0); + for (int j = 0; j < m / 2; j++) + { + t = omega * phasors(k + j + m / 2); + u = phasors(k + j); + phasors(k + j) = u + t; + phasors(k + j + m / 2) = u - t; + omega *= omega_m; + } + } + } + + return phasors; + } + + /** + * @brief Get the frequency used in the FFT + * (only the first half, useful for real input functions) + * + * @param input_signal + * @return Eigen::Vector<std::complex<float>, N_size> + */ + static const VectorHF fftfreq(int sample_rate) + { + VectorHF bins = VectorF::Zero(); + for (int i = 0; i < N_size / 2; i++) + { + bins(i) = (float)i * (float)sample_rate / (float)N_size; + } + return bins; + } + +private: + static const short n_bits = log2f(N_size); + + /** + * @brief Reverse the bits of the inputted unsigned index. + * bits used = log2(N_size) + * e.g. 6 bits: 000110 (6) -> 011000 (24) + */ + static size_t reverse_bits(size_t x) + { + size_t rev = 0 + for (int i = 0; i < n_bits; i++) + { + if (x & (1 << i)) + rev |= 1 << (N_size - 1 - i); + } + return rev; + } +}; + +} // namespace Boardcore -- GitLab From ebc416447efdde54a974769315e36990a0426400 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 19 Sep 2023 09:12:48 +0200 Subject: [PATCH 02/24] [FFT] fixed implementation bugs --- src/shared/algorithms/FFT.h | 48 +++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 20 deletions(-) diff --git a/src/shared/algorithms/FFT.h b/src/shared/algorithms/FFT.h index 0eed6ced5..bce4b1cf1 100644 --- a/src/shared/algorithms/FFT.h +++ b/src/shared/algorithms/FFT.h @@ -23,16 +23,12 @@ #pragma once #include <complex.h> -#include <Eigen/Dense> +#include <math.h> +#include <Eigen/Core> namespace Boardcore { -typedef FFT<32> FFT32; -typedef FFT<64> FFT64; -typedef FFT<128> FFT128; -typedef FFT<256> FFT256; - /** * @brief Implementation of Fast Fourier Trasnform using the iterative version * with bit-reversal index variant of the famous Cooley-Tukey FFT algorithm. @@ -56,9 +52,9 @@ public: static const VectorCF fft(VectorF input_signal) { size_t rev_i; - int m, omega; - float omega_m; - std::complex<float> t, u; + int m; + float phi; + std::complex<float> t, u, omega, omega_m; // Bit-reversal permutation VectorCF phasors = VectorCF::Zero(); @@ -69,11 +65,12 @@ public: } // Cooley-Tukey FFT algorithm - for (int s = 0, s <= n_bits; s++) + for (int s = 1; s <= n_bits(N_size); s++) { m = powl(2, s); - omega_m = cexpf(-2 * I / m); - for (int k = 0; k < n; k += m) + phi = -2 * EIGEN_PI / m; + omega_m = std::complex<float>(cos(phi), sin(phi)); + for (int k = 0; k < N_size; k += m) { omega = std::complex<float>(1, 0); for (int j = 0; j < m / 2; j++) @@ -94,21 +91,24 @@ public: * @brief Get the frequency used in the FFT * (only the first half, useful for real input functions) * - * @param input_signal + * @param sample_rate in Hertz * @return Eigen::Vector<std::complex<float>, N_size> */ - static const VectorHF fftfreq(int sample_rate) + static const VectorHF fftfreq(float sample_rate) { - VectorHF bins = VectorF::Zero(); + VectorHF bins = VectorHF::Zero(); for (int i = 0; i < N_size / 2; i++) { - bins(i) = (float)i * (float)sample_rate / (float)N_size; + bins(i) = (float)i * sample_rate / (float)N_size; } return bins; } private: - static const short n_bits = log2f(N_size); + static short n_bits(size_t x) + { + return (short)log2(x); + } /** * @brief Reverse the bits of the inputted unsigned index. @@ -117,14 +117,22 @@ private: */ static size_t reverse_bits(size_t x) { - size_t rev = 0 - for (int i = 0; i < n_bits; i++) + size_t rev = 0; + short bits = n_bits(N_size); + for (int i = 0; i < bits; i++) { if (x & (1 << i)) - rev |= 1 << (N_size - 1 - i); + { + rev |= 1 << (bits - 1 - i); + } } return rev; } }; +typedef FFT<32> FFT32; +typedef FFT<64> FFT64; +typedef FFT<128> FFT128; +typedef FFT<256> FFT256; + } // namespace Boardcore -- GitLab From ee8d12f990cb733996aa754644c256fb6774e61b Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 19 Sep 2023 09:13:32 +0200 Subject: [PATCH 03/24] [FFT] added test (working) --- CMakeLists.txt | 3 + src/tests/algorithms/FFT/test-fft-data.h | 65 +++++++++++++++++++++ src/tests/algorithms/FFT/test-fft.cpp | 73 ++++++++++++++++++++++++ 3 files changed, 141 insertions(+) create mode 100644 src/tests/algorithms/FFT/test-fft-data.h create mode 100644 src/tests/algorithms/FFT/test-fft.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 437c8fb84..560f180a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -180,6 +180,9 @@ sbs_target(test-nas-parafoil stm32f429zi_skyward_parafoil) add_executable(test-nas src/tests/algorithms/NAS/test-nas.cpp) sbs_target(test-nas stm32f429zi_skyward_death_stack_v3) +add_executable(test-fft src/tests/algorithms/FFT/test-fft.cpp) +sbs_target(test-fft stm32f407vg_stm32f4discovery) + add_executable(test-nas-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp) sbs_target(test-nas-with-triad stm32f429zi_skyward_death_stack_x) diff --git a/src/tests/algorithms/FFT/test-fft-data.h b/src/tests/algorithms/FFT/test-fft-data.h new file mode 100644 index 000000000..3da338d51 --- /dev/null +++ b/src/tests/algorithms/FFT/test-fft-data.h @@ -0,0 +1,65 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <vector> + +namespace Boardcore +{ + +// ------------------------ INPUT ------------------------ +// result of sin(50 * 2πt) + 2sin(25 * 2πt) with t sampling time +// that sums to 1 period (64 samples) +static const std::vector<float> SIGNAL{ + 0.000000, 0.288001, -2.344254, 2.595312, -0.058260, -1.136140, + 0.739060, -1.795279, 2.414214, -0.000944, -2.035020, 1.358310, + -1.140652, 1.774263, 0.007497, -2.526806, 2.000000, -0.565236, + 0.772864, 0.111324, -2.554866, 2.469451, -0.187261, -0.391125, + 0.414214, -2.185460, 2.586819, -0.024999, -1.472474, 0.932373, + -1.578887, 2.249572, 0.000000, -2.249572, 1.578887, -0.932373, + 1.472474, 0.024999, -2.586819, 2.185460, -0.414214, 0.391125, + 0.187261, -2.469451, 2.554866, -0.111324, -0.772864, 0.565236, + -2.000000, 2.526806, -0.007497, -1.774263, 1.140652, -1.358310, + 2.035020, 0.000944, -2.414214, 1.795279, -0.739060, 1.136140, + 0.05826008, -2.59531214, 2.34425399, -0.28800129}; + +static const int SAMPLES = 64; +static const float SAMPLE_RATE = 64.0; + +// ------------------------ EXPECTED OUTPUT ------------------------ +static const std::vector<float> INTENSITY{ + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 2.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000}; + +static const std::vector<float> FREQUENCY{ + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, + 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, + 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, 32.0, 31.0, 32.0, 33.0, + 34.0, 35.0, 36.0, 37.0, 38.0, 39.0, 40.0, 41.0, 42.0, 43.0, 44.0, 45.0, + 46.0, 47.0, 48.0, 47.0, 48.0, 49.0, 50.0, 51.0, 52.0, 53.0, 54.0, 55.0, + 56.0, 57.0, 58.0, 59.0, 60.0, 61.0, 62.0, 63.0}; + +} // namespace Boardcore diff --git a/src/tests/algorithms/FFT/test-fft.cpp b/src/tests/algorithms/FFT/test-fft.cpp new file mode 100644 index 000000000..1e4768573 --- /dev/null +++ b/src/tests/algorithms/FFT/test-fft.cpp @@ -0,0 +1,73 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/FFT.h> +#include <complex.h> +#include <math.h> + +#include <Eigen/Core> + +#include "test-fft-data.h" + +using namespace Boardcore; +using namespace Eigen; + +int main() +{ + bool failed = false; + + Eigen::Vector<float, SAMPLES> signal_vector = + Eigen::Vector<float, SAMPLES>::Zero(); + for (int i = 0; i < SAMPLES; i++) + { + signal_vector(i) = SIGNAL[i]; + } + + Vector<std::complex<float>, SAMPLES> fft_result = + FFT<SAMPLES>::fft(signal_vector); + Vector<float, SAMPLES / 2> fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); + + for (int i = 0; i < fft_result.size() / 2; i++) + { + if (std::abs(2.0 / SAMPLES * std::abs(fft_result(i)) - INTENSITY[i]) > + 0.001) + { + printf("FFT result differs from the correct one [%d]: %f != %f\n", + i, fft_result(i).real(), INTENSITY[i]); + failed = true; + } + } + + for (int i = 0; i < fft_freq.size(); i++) + { + if (fft_freq(i) != FREQUENCY[i]) + { + printf("FFT freq differs from the correct one [%d]: %f != %f\n", i, + fft_freq(i), FREQUENCY[i]); + failed = true; + } + } + + failed ? printf("FAILED\n") : printf("PASSED\n"); + + return 0; +} -- GitLab From 22ffd6aa230c7a224fedb8ced7547046eb69331a Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 19 Sep 2023 09:19:55 +0200 Subject: [PATCH 04/24] [FFT] added more documentation --- src/shared/algorithms/FFT.h | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/shared/algorithms/FFT.h b/src/shared/algorithms/FFT.h index bce4b1cf1..6b6228918 100644 --- a/src/shared/algorithms/FFT.h +++ b/src/shared/algorithms/FFT.h @@ -24,6 +24,7 @@ #include <complex.h> #include <math.h> + #include <Eigen/Core> namespace Boardcore @@ -32,22 +33,22 @@ namespace Boardcore /** * @brief Implementation of Fast Fourier Trasnform using the iterative version * with bit-reversal index variant of the famous Cooley-Tukey FFT algorithm. - * + * * NOTE: N must be a power of 2 -*/ + */ template <int N_size> class FFT { public: using VectorHF = Eigen::Vector<float, N_size / 2>; - using VectorF = Eigen::Vector<float, N_size>; + using VectorF = Eigen::Vector<float, N_size>; using VectorCF = Eigen::Vector<std::complex<float>, N_size>; /** * @brief Perform the FFT on the input signal - * + * * @param input_signal NOTE: MUST BE POWER OF 2 - * @return Eigen::Vector<std::complex<float>, N_size> + * @return Eigen::Vector<std::complex<float>, N_size> */ static const VectorCF fft(VectorF input_signal) { @@ -88,11 +89,11 @@ public: } /** - * @brief Get the frequency used in the FFT + * @brief Get the frequency used in the FFT * (only the first half, useful for real input functions) - * + * * @param sample_rate in Hertz - * @return Eigen::Vector<std::complex<float>, N_size> + * @return Eigen::Vector<std::complex<float>, N_size> */ static const VectorHF fftfreq(float sample_rate) { @@ -105,16 +106,16 @@ public: } private: - static short n_bits(size_t x) - { - return (short)log2(x); - } + /** + * @brief Get the number of bits to differentiate x elements + */ + static short n_bits(size_t x) { return (short)log2(x); } /** * @brief Reverse the bits of the inputted unsigned index. * bits used = log2(N_size) * e.g. 6 bits: 000110 (6) -> 011000 (24) - */ + */ static size_t reverse_bits(size_t x) { size_t rev = 0; @@ -135,4 +136,4 @@ typedef FFT<64> FFT64; typedef FFT<128> FFT128; typedef FFT<256> FFT256; -} // namespace Boardcore +} // namespace Boardcore -- GitLab From ac8cb1b7c34e90a469e4b4b3a319198c3eb2b65b Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 19 Sep 2023 10:41:46 +0200 Subject: [PATCH 05/24] [FFT] improved fftfreq --- src/shared/algorithms/FFT.h | 10 ++++++---- src/tests/algorithms/FFT/test-fft-data.h | 12 ++++++------ src/tests/algorithms/FFT/test-fft.cpp | 2 +- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/shared/algorithms/FFT.h b/src/shared/algorithms/FFT.h index 6b6228918..73fada1d8 100644 --- a/src/shared/algorithms/FFT.h +++ b/src/shared/algorithms/FFT.h @@ -40,7 +40,6 @@ template <int N_size> class FFT { public: - using VectorHF = Eigen::Vector<float, N_size / 2>; using VectorF = Eigen::Vector<float, N_size>; using VectorCF = Eigen::Vector<std::complex<float>, N_size>; @@ -90,18 +89,21 @@ public: /** * @brief Get the frequency used in the FFT - * (only the first half, useful for real input functions) * * @param sample_rate in Hertz * @return Eigen::Vector<std::complex<float>, N_size> */ - static const VectorHF fftfreq(float sample_rate) + static const VectorF fftfreq(float sample_rate) { - VectorHF bins = VectorHF::Zero(); + VectorF bins = VectorF::Zero(); for (int i = 0; i < N_size / 2; i++) { bins(i) = (float)i * sample_rate / (float)N_size; } + for (int i = N_size / 2; i < N_size; i++) + { + bins(i) = (float)(i - N_size) * sample_rate / (float)N_size; + } return bins; } diff --git a/src/tests/algorithms/FFT/test-fft-data.h b/src/tests/algorithms/FFT/test-fft-data.h index 3da338d51..b94fe266c 100644 --- a/src/tests/algorithms/FFT/test-fft-data.h +++ b/src/tests/algorithms/FFT/test-fft-data.h @@ -55,11 +55,11 @@ static const std::vector<float> INTENSITY{ 0.000000, 0.000000, 0.000000, 0.000000}; static const std::vector<float> FREQUENCY{ - 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, - 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, - 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, 32.0, 31.0, 32.0, 33.0, - 34.0, 35.0, 36.0, 37.0, 38.0, 39.0, 40.0, 41.0, 42.0, 43.0, 44.0, 45.0, - 46.0, 47.0, 48.0, 47.0, 48.0, 49.0, 50.0, 51.0, 52.0, 53.0, 54.0, 55.0, - 56.0, 57.0, 58.0, 59.0, 60.0, 61.0, 62.0, 63.0}; + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, + 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, + 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, -32.0, -31.0, -30.0, -29.0, + -28.0, -27.0, -26.0, -25.0, -24.0, -23.0, -22.0, -21.0, -20.0, -19.0, -18.0, + -17.0, -16.0, -15.0, -14.0, -13.0, -12.0, -11.0, -10.0, -9.0, -8.0, -7.0, + -6.0, -5.0, -4.0, -3.0, -2.0, -1.0}; } // namespace Boardcore diff --git a/src/tests/algorithms/FFT/test-fft.cpp b/src/tests/algorithms/FFT/test-fft.cpp index 1e4768573..369587860 100644 --- a/src/tests/algorithms/FFT/test-fft.cpp +++ b/src/tests/algorithms/FFT/test-fft.cpp @@ -44,7 +44,7 @@ int main() Vector<std::complex<float>, SAMPLES> fft_result = FFT<SAMPLES>::fft(signal_vector); - Vector<float, SAMPLES / 2> fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); + Vector<float, SAMPLES> fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); for (int i = 0; i < fft_result.size() / 2; i++) { -- GitLab From 4047614a3725a4d507b647591018f2ae57ae7a02 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 19 Sep 2023 11:02:20 +0200 Subject: [PATCH 06/24] [FFT] test also neg frequencies --- src/tests/algorithms/FFT/test-fft-data.h | 21 ++++++++++----------- src/tests/algorithms/FFT/test-fft.cpp | 4 ++-- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/src/tests/algorithms/FFT/test-fft-data.h b/src/tests/algorithms/FFT/test-fft-data.h index b94fe266c..bde92390a 100644 --- a/src/tests/algorithms/FFT/test-fft-data.h +++ b/src/tests/algorithms/FFT/test-fft-data.h @@ -48,18 +48,17 @@ static const float SAMPLE_RATE = 64.0; // ------------------------ EXPECTED OUTPUT ------------------------ static const std::vector<float> INTENSITY{ - 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, - 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, - 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, - 0.000000, 0.000000, 0.000000, 0.000000, 2.000000, 0.000000, 0.000000, - 0.000000, 0.000000, 0.000000, 0.000000}; + 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., + 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., + 0., 0., 0.5, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.}; static const std::vector<float> FREQUENCY{ - 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, - 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, - 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, -32.0, -31.0, -30.0, -29.0, - -28.0, -27.0, -26.0, -25.0, -24.0, -23.0, -22.0, -21.0, -20.0, -19.0, -18.0, - -17.0, -16.0, -15.0, -14.0, -13.0, -12.0, -11.0, -10.0, -9.0, -8.0, -7.0, - -6.0, -5.0, -4.0, -3.0, -2.0, -1.0}; + 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, + 11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, + 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, -32.0, + -31.0, -30.0, -29.0, -28.0, -27.0, -26.0, -25.0, -24.0, -23.0, -22.0, -21.0, + -20.0, -19.0, -18.0, -17.0, -16.0, -15.0, -14.0, -13.0, -12.0, -11.0, -10.0, + -9.0, -8.0, -7.0, -6.0, -5.0, -4.0, -3.0, -2.0, -1.0}; } // namespace Boardcore diff --git a/src/tests/algorithms/FFT/test-fft.cpp b/src/tests/algorithms/FFT/test-fft.cpp index 369587860..01773e8e2 100644 --- a/src/tests/algorithms/FFT/test-fft.cpp +++ b/src/tests/algorithms/FFT/test-fft.cpp @@ -46,9 +46,9 @@ int main() FFT<SAMPLES>::fft(signal_vector); Vector<float, SAMPLES> fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); - for (int i = 0; i < fft_result.size() / 2; i++) + for (int i = 0; i < fft_result.size(); i++) { - if (std::abs(2.0 / SAMPLES * std::abs(fft_result(i)) - INTENSITY[i]) > + if (std::abs(1.0 / SAMPLES * std::abs(fft_result(i)) - INTENSITY[i]) > 0.001) { printf("FFT result differs from the correct one [%d]: %f != %f\n", -- GitLab From 7819e3a30bbea0ae30526e9b8e3b689a86a0dd90 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Sun, 24 Sep 2023 21:37:31 +0200 Subject: [PATCH 07/24] [FFT] added benchmark for FFT --- src/entrypoints/fft-benchmark.cpp | 74 ++++++++++++++++++++++--------- 1 file changed, 54 insertions(+), 20 deletions(-) diff --git a/src/entrypoints/fft-benchmark.cpp b/src/entrypoints/fft-benchmark.cpp index c7c80730b..53c5ebbce 100644 --- a/src/entrypoints/fft-benchmark.cpp +++ b/src/entrypoints/fft-benchmark.cpp @@ -22,36 +22,70 @@ #pragma once -#include <iostream> -#include <Eigen/Dense> -#include <miosix.h> -#include <complex.h> -#include <math.h> #include <algorithms/FFT.h> +#include <drivers/timer/TimestampTimer.h> +#include <utils/Stats/Stats.h> + +#include <Eigen/Dense> +#include <array> using namespace Boardcore; +using namespace std; -int main(int argc, char const *argv[]) -{ - Eigen::Vector<float, 256> input_signal = Eigen::Vector<float, 256>::Zero(); - float f1, f2; +const unsigned int TAKES = 5000; +const unsigned int BUFFER = 32; - f1 = 50; - f2 = 25; +array<float, TAKES> data; - for (size_t i = 0; i < input_signal.size(); i++) - { - input_signal(i) = sin(2 * M_PI * i / 256 * f1) + sin(2 * M_PI * i / 256 * f2); - } +/** + * @brief Prints the test results for the specified buffer size. + * + * @param bufferSize Buffer size of the benchmark. + * @param results Results form the benchmark. + */ +void printResults(size_t bufferSize, array<float, TAKES>& results); - Eigen::Vector<std::complex<float>, 256> fft_result = FFT<256>::fft(input_signal); - Eigen::Vector<float, 256> fft_freq = FFT<256>::fftfreq(1.0 / 256.0); +int main() +{ + Eigen::Vector<float, BUFFER> input_signal = + Eigen::Vector<float, BUFFER>::Zero(); - std::cout << "FFT result:" << std::endl; - for (size_t i = 0; i < fft_result.size(); i++) + for (int i = 0; i < TAKES; i++) { - std::cout << fft_freq(i) << ' ' << fft_result(i).real() << std::endl; + for (size_t i = 0; i < input_signal.size(); i++) + { + input_signal(i) = (float)rand() / RAND_MAX; + } + + int64_t duration = TimestampTimer::getTimestamp(); + FFT<BUFFER>::fft(input_signal); + duration = TimestampTimer::getTimestamp() - duration; + data[i] = duration; } + printResults(BUFFER, data); + return 0; } + +void printResults(size_t bufferSize, array<float, TAKES>& results) +{ + // Compute statistics on the benchmark results + Stats stats; + for (float result : results) + stats.add(result); + StatsResult statsResults = stats.getStats(); + + printf("\tBuffer size: %lu\n", (unsigned long)bufferSize); + printf("Times:\n"); + printf("- mean: % 6.1f us\n", statsResults.mean); + printf("- std dev: % 6.1f us\n", statsResults.stdDev); + printf("- min: % 6.1f us\n", statsResults.minValue); + printf("- max: % 6.1f us\n", statsResults.maxValue); + printf("Speeds:\n"); + printf("- mean: % 6.2f Hz\n", 1e6 / statsResults.mean); + printf("- min: % 6.2f Hz\n", 1e6 / statsResults.maxValue); + printf("- max: % 6.2f Hz\n", 1e6 / statsResults.minValue); + + printf("\n"); +} -- GitLab From 59c4bba616c1563b683fc91573b009e5d6366f20 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Mon, 25 Sep 2023 00:36:34 +0200 Subject: [PATCH 08/24] [FFT] added the IFFT and tested --- src/shared/algorithms/FFT.h | 48 ++++++++++++++++++++++++--- src/tests/algorithms/FFT/test-fft.cpp | 25 +++++++++++--- 2 files changed, 63 insertions(+), 10 deletions(-) diff --git a/src/shared/algorithms/FFT.h b/src/shared/algorithms/FFT.h index 73fada1d8..24e49593c 100644 --- a/src/shared/algorithms/FFT.h +++ b/src/shared/algorithms/FFT.h @@ -25,7 +25,7 @@ #include <complex.h> #include <math.h> -#include <Eigen/Core> +#include <Eigen/Dense> namespace Boardcore { @@ -60,15 +60,15 @@ public: VectorCF phasors = VectorCF::Zero(); for (int i = 0; i < N_size; i++) { - rev_i = reverse_bits(i); + rev_i = reverse_bits(i); phasors(rev_i) = std::complex<float>(input_signal(i), 0); } // Cooley-Tukey FFT algorithm for (int s = 1; s <= n_bits(N_size); s++) { - m = powl(2, s); - phi = -2 * EIGEN_PI / m; + m = powl(2, s); + phi = -2 * EIGEN_PI / m; omega_m = std::complex<float>(cos(phi), sin(phi)); for (int k = 0; k < N_size; k += m) { @@ -77,8 +77,10 @@ public: { t = omega * phasors(k + j + m / 2); u = phasors(k + j); - phasors(k + j) = u + t; + + phasors(k + j) = u + t; phasors(k + j + m / 2) = u - t; + omega *= omega_m; } } @@ -107,6 +109,40 @@ public: return bins; } + /** + * @brief Perform the inverse FFT on the input signal + * + * @param fft_vector + * @param sample_rate in Hertz + * @return Eigen::Vector<std::complex<float>, N_size> + */ + static const VectorF ifft(VectorCF fft_vector, float sample_rate) + { + float amplitude, alpha, phi; + VectorF ifft_vector = VectorF::Zero(); + VectorF bins = fftfreq(sample_rate); + + for (int i = 0; i < N_size; i++) + { + for (int j = 0; j < N_size; j++) + { + // A = |f| + amplitude = std::abs(fft_vector(j)); + // α = 2πf * t + alpha = 2 * EIGEN_PI * bins(j) * (float)i / sample_rate; + // ϕ = arg(f) + phi = std::arg(fft_vector(j)); + + // i = A * cos(α + ϕ) + ifft_vector(i) += amplitude * cos(alpha + phi); + } + // scale down + ifft_vector(i) /= N_size; + } + + return ifft_vector; + } + private: /** * @brief Get the number of bits to differentiate x elements @@ -133,6 +169,8 @@ private: } }; +typedef FFT<8> FFT8; +typedef FFT<16> FFT16; typedef FFT<32> FFT32; typedef FFT<64> FFT64; typedef FFT<128> FFT128; diff --git a/src/tests/algorithms/FFT/test-fft.cpp b/src/tests/algorithms/FFT/test-fft.cpp index 01773e8e2..9158d5314 100644 --- a/src/tests/algorithms/FFT/test-fft.cpp +++ b/src/tests/algorithms/FFT/test-fft.cpp @@ -31,21 +31,24 @@ using namespace Boardcore; using namespace Eigen; +using VectorF = Eigen::Vector<float, SAMPLES>; +using VectorCF = Eigen::Vector<std::complex<float>, SAMPLES>; + int main() { bool failed = false; - Eigen::Vector<float, SAMPLES> signal_vector = - Eigen::Vector<float, SAMPLES>::Zero(); + VectorF signal_vector = VectorF::Zero(); for (int i = 0; i < SAMPLES; i++) { signal_vector(i) = SIGNAL[i]; } - Vector<std::complex<float>, SAMPLES> fft_result = - FFT<SAMPLES>::fft(signal_vector); - Vector<float, SAMPLES> fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); + VectorCF fft_result = FFT<SAMPLES>::fft(signal_vector); + VectorF fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); + VectorF ifft_result = FFT<SAMPLES>::ifft(fft_result, SAMPLE_RATE); + // test for fft for (int i = 0; i < fft_result.size(); i++) { if (std::abs(1.0 / SAMPLES * std::abs(fft_result(i)) - INTENSITY[i]) > @@ -57,6 +60,7 @@ int main() } } + // test for fftfreq for (int i = 0; i < fft_freq.size(); i++) { if (fft_freq(i) != FREQUENCY[i]) @@ -67,6 +71,17 @@ int main() } } + // test for ifft + for (int i = 0; i < ifft_result.size(); i++) + { + if (std::abs(ifft_result(i) - SIGNAL[i]) > 0.001) + { + printf("IFFT result differs from the correct one [%d]: %f != %f\n", + i, ifft_result(i), SIGNAL[i]); + failed = true; + } + } + failed ? printf("FAILED\n") : printf("PASSED\n"); return 0; -- GitLab From 1b6e39de0bc63b5e76def1f8e79bb3ba9cd7e5fe Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Mon, 16 Oct 2023 17:46:35 +0200 Subject: [PATCH 09/24] [FFT] fixed typo --- src/shared/algorithms/FFT.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/shared/algorithms/FFT.h b/src/shared/algorithms/FFT.h index 24e49593c..128d2a254 100644 --- a/src/shared/algorithms/FFT.h +++ b/src/shared/algorithms/FFT.h @@ -31,7 +31,7 @@ namespace Boardcore { /** - * @brief Implementation of Fast Fourier Trasnform using the iterative version + * @brief Implementation of Fast Fourier Transform using the iterative version * with bit-reversal index variant of the famous Cooley-Tukey FFT algorithm. * * NOTE: N must be a power of 2 -- GitLab From fc725dfe6c4b09d66ac18031ccbffa7a66cf9906 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Mon, 16 Oct 2023 17:46:57 +0200 Subject: [PATCH 10/24] [LowPass] added first draft of LowPass filter --- cmake/boardcore.cmake | 1 + src/shared/algorithms/Filters/LowPass.cpp | 40 ++++++++++++++++ src/shared/algorithms/Filters/LowPass.h | 56 +++++++++++++++++++++++ 3 files changed, 97 insertions(+) create mode 100644 src/shared/algorithms/Filters/LowPass.cpp create mode 100644 src/shared/algorithms/Filters/LowPass.h diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index 49c749382..100029d00 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -41,6 +41,7 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp ${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp ${SBS_BASE}/src/shared/algorithms/NAS/StateInitializer.cpp + ${SBS_BASE}/src/shared/algorithms/Filters/LowPass.cpp # Debug ${SBS_BASE}/src/shared/utils/Debug.cpp diff --git a/src/shared/algorithms/Filters/LowPass.cpp b/src/shared/algorithms/Filters/LowPass.cpp new file mode 100644 index 000000000..72e609911 --- /dev/null +++ b/src/shared/algorithms/Filters/LowPass.cpp @@ -0,0 +1,40 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "LowPass.h" + +namespace Boardcore +{ + +// TODO: WARNING! initialized at 0 +LowPass::LowPass(float gain, float cutoff_freq, float lambda) + : gain(gain), cutoff_freq(cutoff_freq), lambda(lambda), output(0) +{ +} + +float LowPass::filter(float input) +{ + output = lambda * output + (gain / cutoff_freq) * (1 - lambda) * input; + return output; +} + +} // namespace Boardcore diff --git a/src/shared/algorithms/Filters/LowPass.h b/src/shared/algorithms/Filters/LowPass.h new file mode 100644 index 000000000..d562d8015 --- /dev/null +++ b/src/shared/algorithms/Filters/LowPass.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace Boardcore +{ + +class LowPass +{ +public: + /** + * @brief Construct a new Low Pass object + * + * @param gain The gain of the filter + * @param cutoff_freq The cutoff frequency of the filter + * @param lambda The lambda parameter of the filter + * + * @note WARNING: Initialize output at 0 at first + */ + LowPass(float gain, float cutoff_freq, float lambda); + + /** + * @brief Filter the input + * + * @param input The input to filter + */ + float filter(float input); + +private: + float cutoff_freq; + float lambda; + float gain; + float output; +}; + +} // namespace Boardcore -- GitLab From f385bd37fb2cb3b8e79df6e52a4356fa2660cd06 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Mon, 16 Oct 2023 18:09:18 +0200 Subject: [PATCH 11/24] [FFT] fixed cppcheck issue with shadow variable --- src/entrypoints/fft-benchmark.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/entrypoints/fft-benchmark.cpp b/src/entrypoints/fft-benchmark.cpp index 53c5ebbce..4358d4c37 100644 --- a/src/entrypoints/fft-benchmark.cpp +++ b/src/entrypoints/fft-benchmark.cpp @@ -52,9 +52,9 @@ int main() for (int i = 0; i < TAKES; i++) { - for (size_t i = 0; i < input_signal.size(); i++) + for (size_t j = 0; j < input_signal.size(); j++) { - input_signal(i) = (float)rand() / RAND_MAX; + input_signal(j) = (float)rand() / RAND_MAX; } int64_t duration = TimestampTimer::getTimestamp(); -- GitLab From 61ce0a058757ef99427796b25fcbdcbfea5b0918 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Mon, 16 Oct 2023 18:10:32 +0200 Subject: [PATCH 12/24] [LowPass] refactored name variables in camelCase --- src/shared/algorithms/Filters/LowPass.cpp | 6 +++--- src/shared/algorithms/Filters/LowPass.h | 9 +++------ 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/shared/algorithms/Filters/LowPass.cpp b/src/shared/algorithms/Filters/LowPass.cpp index 72e609911..ef2398514 100644 --- a/src/shared/algorithms/Filters/LowPass.cpp +++ b/src/shared/algorithms/Filters/LowPass.cpp @@ -26,14 +26,14 @@ namespace Boardcore { // TODO: WARNING! initialized at 0 -LowPass::LowPass(float gain, float cutoff_freq, float lambda) - : gain(gain), cutoff_freq(cutoff_freq), lambda(lambda), output(0) +LowPass::LowPass(float gain, float cutoffFreq, float lambda) + : gain(gain), cutoffFreq(cutoffFreq), lambda(lambda), output(0) { } float LowPass::filter(float input) { - output = lambda * output + (gain / cutoff_freq) * (1 - lambda) * input; + output = lambda * output + (gain / cutoffFreq) * (1 - lambda) * input; return output; } diff --git a/src/shared/algorithms/Filters/LowPass.h b/src/shared/algorithms/Filters/LowPass.h index d562d8015..89ba8c5ef 100644 --- a/src/shared/algorithms/Filters/LowPass.h +++ b/src/shared/algorithms/Filters/LowPass.h @@ -32,12 +32,12 @@ public: * @brief Construct a new Low Pass object * * @param gain The gain of the filter - * @param cutoff_freq The cutoff frequency of the filter + * @param cutoffFreq The cutoff frequency of the filter * @param lambda The lambda parameter of the filter * * @note WARNING: Initialize output at 0 at first */ - LowPass(float gain, float cutoff_freq, float lambda); + LowPass(float gain, float cutoffFreq, float lambda); /** * @brief Filter the input @@ -47,10 +47,7 @@ public: float filter(float input); private: - float cutoff_freq; - float lambda; - float gain; - float output; + float gain, cutoffFreq, lambda, output; }; } // namespace Boardcore -- GitLab From 1a2cf31a70560effa79ddc0c0577e1ec451cb8a3 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Sat, 21 Oct 2023 00:42:25 +0200 Subject: [PATCH 13/24] [MedFilter] implemented median filter with generics over window size --- src/shared/algorithms/Filters/MedFilter.h | 94 +++++++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 src/shared/algorithms/Filters/MedFilter.h diff --git a/src/shared/algorithms/Filters/MedFilter.h b/src/shared/algorithms/Filters/MedFilter.h new file mode 100644 index 000000000..c4dca2a99 --- /dev/null +++ b/src/shared/algorithms/Filters/MedFilter.h @@ -0,0 +1,94 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <array> + +namespace Boardcore +{ + +/** + * @brief Implementation of a Median Filter + * + * @tparam windowSize The size of the window + * @note WARNING: should be noted that the first windowSize - 1 outputs will be + * wrong because the window is not full yet (consider using a Shadow Mode) + */ +template <size_t windowSize> +class MedFilter +{ + // windowSize must be odd + static_assert(windowSize % 2 == 1, "windowSize must be odd"); + +public: + /** + * @brief Construct a new Median Filter object + * + * @note WARNING: Initialize output at 0 at first + */ + explicit MedFilter() : window({}) {} + + /** + * @brief Filter the input + * + * @param input The input to filter + * @note WARNING: should be noted that the first windowSize - 1 outputs will + * be wrong because the window is not full yet (consider using a Shadow + * Mode) + */ + float filter(float input) + { + slideWindow(input); + return median(); + } + +private: + /** + * @brief Slide the window by one position to the left and add the new input + * to the right + * + * @param input The input to add to the window + */ + void slideWindow(float input) + { + for (size_t i = 0; i < windowSize - 1; i++) + { + window[i] = window[i + 1]; + } + window[windowSize - 1] = input; + } + + /** + * @brief Calculate the median of the window + */ + float median() + { + std::array<float, windowSize> sortedWindow = window; + std::sort(sortedWindow.begin(), sortedWindow.end()); + return sortedWindow[windowSize / 2]; + } + + std::array<float, windowSize> window; +}; + +} // namespace Boardcore -- GitLab From 1fc1e2758500a4ae1034ef22355616b8f7ead09f Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Sat, 21 Oct 2023 00:44:16 +0200 Subject: [PATCH 14/24] [LowPass] added some comments --- src/shared/algorithms/Filters/LowPass.cpp | 1 + src/shared/algorithms/Filters/LowPass.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/shared/algorithms/Filters/LowPass.cpp b/src/shared/algorithms/Filters/LowPass.cpp index ef2398514..142c97f6c 100644 --- a/src/shared/algorithms/Filters/LowPass.cpp +++ b/src/shared/algorithms/Filters/LowPass.cpp @@ -26,6 +26,7 @@ namespace Boardcore { // TODO: WARNING! initialized at 0 +// WARNING: frequency set by parameters, look for these anyway LowPass::LowPass(float gain, float cutoffFreq, float lambda) : gain(gain), cutoffFreq(cutoffFreq), lambda(lambda), output(0) { diff --git a/src/shared/algorithms/Filters/LowPass.h b/src/shared/algorithms/Filters/LowPass.h index 89ba8c5ef..49f5c2b7e 100644 --- a/src/shared/algorithms/Filters/LowPass.h +++ b/src/shared/algorithms/Filters/LowPass.h @@ -36,6 +36,7 @@ public: * @param lambda The lambda parameter of the filter * * @note WARNING: Initialize output at 0 at first + * @note WARNING: frequency set by parameters, look for these anyway */ LowPass(float gain, float cutoffFreq, float lambda); -- GitLab From ed1169f7d1e19d50b0781f118a6cdc92816aff80 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Wed, 25 Oct 2023 13:54:23 +0200 Subject: [PATCH 15/24] [LowPass] added config constructor --- src/shared/algorithms/Filters/LowPass.cpp | 9 ++++++++- src/shared/algorithms/Filters/LowPass.h | 24 +++++++++++++++++++++-- 2 files changed, 30 insertions(+), 3 deletions(-) diff --git a/src/shared/algorithms/Filters/LowPass.cpp b/src/shared/algorithms/Filters/LowPass.cpp index 142c97f6c..cdb97376f 100644 --- a/src/shared/algorithms/Filters/LowPass.cpp +++ b/src/shared/algorithms/Filters/LowPass.cpp @@ -26,12 +26,19 @@ namespace Boardcore { // TODO: WARNING! initialized at 0 -// WARNING: frequency set by parameters, look for these anyway +// WARNING: frequency set by parameters, look for these in any case LowPass::LowPass(float gain, float cutoffFreq, float lambda) : gain(gain), cutoffFreq(cutoffFreq), lambda(lambda), output(0) { } +// TODO: WARNING! initialized at 0 +// WARNING: frequency set by parameters, look for these in any case +LowPass::LowPass(const LowPassConfig& config) + : LowPass(config.gain, config.cutoffFreq, config.lambda) +{ +} + float LowPass::filter(float input) { output = lambda * output + (gain / cutoffFreq) * (1 - lambda) * input; diff --git a/src/shared/algorithms/Filters/LowPass.h b/src/shared/algorithms/Filters/LowPass.h index 49f5c2b7e..ebd1ab7cd 100644 --- a/src/shared/algorithms/Filters/LowPass.h +++ b/src/shared/algorithms/Filters/LowPass.h @@ -25,21 +25,41 @@ namespace Boardcore { +/** + * @brief Online Low Pass filter with frequency-aware parameters + */ class LowPass { public: + struct LowPassConfig + { + float gain; + float cutoffFreq; + float lambda; + }; + /** - * @brief Construct a new Low Pass object + * @brief Construct an online Low Pass by providing each parameter * * @param gain The gain of the filter * @param cutoffFreq The cutoff frequency of the filter * @param lambda The lambda parameter of the filter * * @note WARNING: Initialize output at 0 at first - * @note WARNING: frequency set by parameters, look for these anyway + * @note WARNING: frequency set by parameters, look for these in any case */ LowPass(float gain, float cutoffFreq, float lambda); + /** + * @brief Construct an online Low Pass from a configuration + * + * @param config The configuration of the filter + * + * @note WARNING: Initialize output at 0 at first + * @note WARNING: frequency set by parameters, look for these in any case + */ + explicit LowPass(const LowPassConfig& config); + /** * @brief Filter the input * -- GitLab From e50dd51cfcf25d57c678cd0f682d9f62af1358ac Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Wed, 25 Oct 2023 16:41:13 +0200 Subject: [PATCH 16/24] [LowPass] minor: warnings --- src/shared/algorithms/Filters/LowPass.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/shared/algorithms/Filters/LowPass.h b/src/shared/algorithms/Filters/LowPass.h index ebd1ab7cd..bd6e18ba5 100644 --- a/src/shared/algorithms/Filters/LowPass.h +++ b/src/shared/algorithms/Filters/LowPass.h @@ -45,8 +45,8 @@ public: * @param cutoffFreq The cutoff frequency of the filter * @param lambda The lambda parameter of the filter * - * @note WARNING: Initialize output at 0 at first - * @note WARNING: frequency set by parameters, look for these in any case + * @warning Initialize output at 0 at first + * @warning frequency set by parameters, look for these in any case */ LowPass(float gain, float cutoffFreq, float lambda); @@ -55,8 +55,8 @@ public: * * @param config The configuration of the filter * - * @note WARNING: Initialize output at 0 at first - * @note WARNING: frequency set by parameters, look for these in any case + * @warning Initialize output at 0 at first + * @warning frequency set by parameters, look for these in any case */ explicit LowPass(const LowPassConfig& config); -- GitLab From 582439cf82330c48ef2a3a00eefbe060f6ca2af7 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Wed, 25 Oct 2023 13:56:10 +0200 Subject: [PATCH 17/24] [SlidingWindow] implemented a sliding window class for utility --- src/shared/utils/SlidingWindow.h | 70 ++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 src/shared/utils/SlidingWindow.h diff --git a/src/shared/utils/SlidingWindow.h b/src/shared/utils/SlidingWindow.h new file mode 100644 index 000000000..720acf3b9 --- /dev/null +++ b/src/shared/utils/SlidingWindow.h @@ -0,0 +1,70 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <array> + +namespace Boardcore +{ + +template <typename T, size_t D> +class SlidingWindow +{ +public: + explicit SlidingWindow() : window({}), filled(0) {} + + void push(T value) + { + shiftWindow(1); + filled < D ? filled++ : filled; + setLast(value); + } + + bool isFull() { return filled == D; } + + T last() { return window[D - 1]; } + + std::array<T, D>& all() { return &window; } + +private: + inline void setLast(T value) { window[D - 1] = value; } + + /** + * @brief Shift the window by `n` positions to the left leaving the last + * values unchanged (hard assumption, be sure to know what you're doing) + * + * @param input The input to add to the window + */ + void shiftWindow(size_t n) + { + for (int i = 0; i < D - n; i++) + { + window[i] = window[i + n]; + } + } + + std::array<T, D> window; + size_t filled; +}; + +} // namespace Boardcore -- GitLab From 4579a45050ca981b3523006887f75028bf85c296 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Wed, 25 Oct 2023 13:54:51 +0200 Subject: [PATCH 18/24] [MedFilter] now uses SlidingWindow class --- src/shared/algorithms/Filters/MedFilter.h | 41 ++++++++--------------- 1 file changed, 14 insertions(+), 27 deletions(-) diff --git a/src/shared/algorithms/Filters/MedFilter.h b/src/shared/algorithms/Filters/MedFilter.h index c4dca2a99..4b9d16b53 100644 --- a/src/shared/algorithms/Filters/MedFilter.h +++ b/src/shared/algorithms/Filters/MedFilter.h @@ -22,6 +22,8 @@ #pragma once +#include <utils/SlidingWindow.h> + #include <array> namespace Boardcore @@ -30,15 +32,15 @@ namespace Boardcore /** * @brief Implementation of a Median Filter * - * @tparam windowSize The size of the window - * @note WARNING: should be noted that the first windowSize - 1 outputs will be + * @tparam WIN_SIZE The size of the window + * @note WARNING: should be noted that the first WIN_SIZE - 1 outputs will be * wrong because the window is not full yet (consider using a Shadow Mode) */ -template <size_t windowSize> +template <size_t WIN_SIZE> class MedFilter { - // windowSize must be odd - static_assert(windowSize % 2 == 1, "windowSize must be odd"); + // WIN_SIZE must be odd + static_assert(WIN_SIZE % 2 == 1, "WIN_SIZE must be odd"); public: /** @@ -46,49 +48,34 @@ public: * * @note WARNING: Initialize output at 0 at first */ - explicit MedFilter() : window({}) {} + explicit MedFilter() : window() {} /** * @brief Filter the input * * @param input The input to filter - * @note WARNING: should be noted that the first windowSize - 1 outputs will + * @note WARNING: should be noted that the first WIN_SIZE - 1 outputs will * be wrong because the window is not full yet (consider using a Shadow * Mode) */ float filter(float input) { - slideWindow(input); + window.push(input); return median(); } private: - /** - * @brief Slide the window by one position to the left and add the new input - * to the right - * - * @param input The input to add to the window - */ - void slideWindow(float input) - { - for (size_t i = 0; i < windowSize - 1; i++) - { - window[i] = window[i + 1]; - } - window[windowSize - 1] = input; - } - /** * @brief Calculate the median of the window */ float median() { - std::array<float, windowSize> sortedWindow = window; - std::sort(sortedWindow.begin(), sortedWindow.end()); - return sortedWindow[windowSize / 2]; + std::array<float, WIN_SIZE> sorted_window = window.all(); + std::sort(sorted_window.begin(), sorted_window.end()); + return sorted_window[WIN_SIZE / 2]; } - std::array<float, windowSize> window; + SlidingWindow<float, WIN_SIZE> window; }; } // namespace Boardcore -- GitLab From ebc9e703a3b5d6ee08ee5c3f08b8f19674939741 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Wed, 25 Oct 2023 16:41:37 +0200 Subject: [PATCH 19/24] [SlidingWindow] minor (added simple method + init little change) --- src/shared/utils/SlidingWindow.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/shared/utils/SlidingWindow.h b/src/shared/utils/SlidingWindow.h index 720acf3b9..37c0cbc58 100644 --- a/src/shared/utils/SlidingWindow.h +++ b/src/shared/utils/SlidingWindow.h @@ -31,7 +31,7 @@ template <typename T, size_t D> class SlidingWindow { public: - explicit SlidingWindow() : window({}), filled(0) {} + explicit SlidingWindow() : window({0}), filled(0) {} void push(T value) { @@ -42,6 +42,11 @@ public: bool isFull() { return filled == D; } + /** + * @brief Get the actual number of elements in the window + */ + size_t filled() { return filled; } + T last() { return window[D - 1]; } std::array<T, D>& all() { return &window; } -- GitLab From 59903fedd276b4911fa00a804a65b459a1592969 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Wed, 25 Oct 2023 16:42:19 +0200 Subject: [PATCH 20/24] [MedFilter] split MedFilter in two (exact and ajdustable) based on requirements --- .../algorithms/Filters/AdjustableMedFilter.h | 74 +++++++++++++++++++ .../Filters/{MedFilter.h => ExactMedFilter.h} | 17 +++-- 2 files changed, 83 insertions(+), 8 deletions(-) create mode 100644 src/shared/algorithms/Filters/AdjustableMedFilter.h rename src/shared/algorithms/Filters/{MedFilter.h => ExactMedFilter.h} (80%) diff --git a/src/shared/algorithms/Filters/AdjustableMedFilter.h b/src/shared/algorithms/Filters/AdjustableMedFilter.h new file mode 100644 index 000000000..a068864c3 --- /dev/null +++ b/src/shared/algorithms/Filters/AdjustableMedFilter.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <utils/SlidingWindow.h> + +#include <array> + +namespace Boardcore +{ + +/** + * @brief Median Filter with adjustable window size in order to use the widest + * window size available if enough values have been pushed through + * + * @tparam WIN_SIZE The maximum size of the window + */ +template <size_t WIN_SIZE> +class AdjustableMedFilter +{ + // WIN_SIZE must be odd + static_assert(WIN_SIZE % 2 == 1, "WIN_SIZE must be odd"); + +public: + explicit AdjustableMedFilter() : window() {} + + float filter(float input) + { + window.push(input); + return median(); + } + +private: + /** + * @warning assumption: UB if the window completely empty + */ + float median() + { + std::array<float, WIN_SIZE> sorted_window = window.all(); + + size_t fil = window.filled(); + // reduce to the nearest odd number + size_t best_win_size = fil % 2 && fil > 0 ? fil - 1 : fil; + + std::sort(sorted_window.begin() + WIN_SIZE - best_win_size, + sorted_window.end()); + + return sorted_window[best_win_size / 2 + WIN_SIZE - best_win_size]; + } + + SlidingWindow<float, WIN_SIZE> window; +}; + +} // namespace Boardcore diff --git a/src/shared/algorithms/Filters/MedFilter.h b/src/shared/algorithms/Filters/ExactMedFilter.h similarity index 80% rename from src/shared/algorithms/Filters/MedFilter.h rename to src/shared/algorithms/Filters/ExactMedFilter.h index 4b9d16b53..63bfa091a 100644 --- a/src/shared/algorithms/Filters/MedFilter.h +++ b/src/shared/algorithms/Filters/ExactMedFilter.h @@ -30,31 +30,32 @@ namespace Boardcore { /** - * @brief Implementation of a Median Filter + * @brief Exact Median Filter based on a fixed window size, mind that while it + * is not full the filtered output should not be taken into consideration * - * @tparam WIN_SIZE The size of the window - * @note WARNING: should be noted that the first WIN_SIZE - 1 outputs will be + * @tparam WIN_SIZE The exact size of the window + * @warning: should be noted that the first WIN_SIZE - 1 outputs will be * wrong because the window is not full yet (consider using a Shadow Mode) */ template <size_t WIN_SIZE> -class MedFilter +class ExactMedFilter { // WIN_SIZE must be odd static_assert(WIN_SIZE % 2 == 1, "WIN_SIZE must be odd"); public: /** - * @brief Construct a new Median Filter object + * @brief Construct a new Exact Median Filter object * - * @note WARNING: Initialize output at 0 at first + * @warning Initialize output at 0 at first */ - explicit MedFilter() : window() {} + explicit ExactMedFilter() : window() {} /** * @brief Filter the input * * @param input The input to filter - * @note WARNING: should be noted that the first WIN_SIZE - 1 outputs will + * @warning should be noted that the first WIN_SIZE - 1 outputs will * be wrong because the window is not full yet (consider using a Shadow * Mode) */ -- GitLab From 02ce28d0863b310773993d28fdef5a2a5673ff5e Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 21 Nov 2023 18:53:14 +0100 Subject: [PATCH 21/24] [SlidingWindow] renamed some variables and fixed unnecessary references --- src/shared/utils/SlidingWindow.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/shared/utils/SlidingWindow.h b/src/shared/utils/SlidingWindow.h index 37c0cbc58..2494f623f 100644 --- a/src/shared/utils/SlidingWindow.h +++ b/src/shared/utils/SlidingWindow.h @@ -31,25 +31,25 @@ template <typename T, size_t D> class SlidingWindow { public: - explicit SlidingWindow() : window({0}), filled(0) {} + explicit SlidingWindow() : window({0}), valuesFilled(0) {} void push(T value) { shiftWindow(1); - filled < D ? filled++ : filled; + valuesFilled < D ? valuesFilled++ : valuesFilled; setLast(value); } - bool isFull() { return filled == D; } + bool isFull() { return valuesFilled == D; } /** * @brief Get the actual number of elements in the window */ - size_t filled() { return filled; } + size_t filled() { return valuesFilled; } T last() { return window[D - 1]; } - std::array<T, D>& all() { return &window; } + std::array<T, D>& all() { return window; } private: inline void setLast(T value) { window[D - 1] = value; } @@ -62,14 +62,14 @@ private: */ void shiftWindow(size_t n) { - for (int i = 0; i < D - n; i++) + for (size_t i = 0; i < D - n; i++) { window[i] = window[i + n]; } } std::array<T, D> window; - size_t filled; + size_t valuesFilled; }; } // namespace Boardcore -- GitLab From 0c3f478906e34e23465d0421d9c26fd5942b5055 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 21 Nov 2023 18:52:42 +0100 Subject: [PATCH 22/24] [MedFilter] fixed include missing --- src/shared/algorithms/Filters/AdjustableMedFilter.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/shared/algorithms/Filters/AdjustableMedFilter.h b/src/shared/algorithms/Filters/AdjustableMedFilter.h index a068864c3..6b806c6ce 100644 --- a/src/shared/algorithms/Filters/AdjustableMedFilter.h +++ b/src/shared/algorithms/Filters/AdjustableMedFilter.h @@ -24,6 +24,7 @@ #include <utils/SlidingWindow.h> +#include <algorithm> #include <array> namespace Boardcore -- GitLab From c58c3e53e70408a6fd8a47c77888011ad728e146 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 31 Oct 2023 14:24:48 +0100 Subject: [PATCH 23/24] [FFT] fixed warnings to fft-benchmark --- src/entrypoints/fft-benchmark.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/entrypoints/fft-benchmark.cpp b/src/entrypoints/fft-benchmark.cpp index 4358d4c37..81bc9550a 100644 --- a/src/entrypoints/fft-benchmark.cpp +++ b/src/entrypoints/fft-benchmark.cpp @@ -20,8 +20,6 @@ * THE SOFTWARE. */ -#pragma once - #include <algorithms/FFT.h> #include <drivers/timer/TimestampTimer.h> #include <utils/Stats/Stats.h> @@ -50,9 +48,9 @@ int main() Eigen::Vector<float, BUFFER> input_signal = Eigen::Vector<float, BUFFER>::Zero(); - for (int i = 0; i < TAKES; i++) + for (unsigned int i = 0; i < TAKES; i++) { - for (size_t j = 0; j < input_signal.size(); j++) + for (size_t j = 0; j < (size_t)input_signal.size(); j++) { input_signal(j) = (float)rand() / RAND_MAX; } -- GitLab From 9a42d3a22f8d297d7d8fceefa4804cff67d93834 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Tue, 14 Nov 2023 19:17:27 +0100 Subject: [PATCH 24/24] [VSCode] added words to spellcheck --- .vscode/settings.json | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.vscode/settings.json b/.vscode/settings.json index fa393981b..209769535 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -168,6 +168,7 @@ "Fatt", "Fatttr", "fedetft's", + "fftfreq", "fiprintf", "FMCEN", "FMPIE", @@ -202,6 +203,7 @@ "HSCMRNN", "HWMAPPING", "IDLEIE", + "ifft", "Impli", "Implii", "INAK", @@ -221,6 +223,7 @@ "LIFCR", "LISR", "logdecoder", + "Lolli", "LSBFIRST", "Luca", "Mandelli", @@ -255,6 +258,7 @@ "OUTY", "OUTZ", "peripehral", + "phasors", "Piazzolla", "PINC", "Pitot", @@ -310,6 +314,7 @@ "tparam", "TSCPP", "TSVREFE", + "Tukey", "Tweakable", "TXCRCR", "txfp", -- GitLab