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