From b5430c22c433987e0e73575e44efee11bb12ef0a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Jul 2021 16:44:55 -0400 Subject: [PATCH] AlphaFilter: move from ecl -> mathlib --- src/lib/battery/battery.h | 2 +- src/lib/ecl/CMakeLists.txt | 5 +++++ src/lib/ecl/EKF/estimator_interface.h | 2 +- src/lib/ecl/test/CMakeLists.txt | 1 - src/lib/mathlib/CMakeLists.txt | 7 ++++--- .../AlphaFilter => mathlib/math/filter}/AlphaFilter.hpp | 0 .../math/test/AlphaFilterTest.cpp} | 8 +++++++- .../math/{filter => test}/LowPassFilter2pVector3fTest.cpp | 2 +- .../mathlib/math/{filter => test}/MedianFilterTest.cpp | 2 +- src/lib/mathlib/math/{filter => test}/NotchFilterTest.cpp | 2 +- src/lib/tecs/TECS.hpp | 2 +- .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp | 2 +- .../tasks/Utility/StickAccelerationXY.hpp | 2 +- src/modules/mc_att_control/mc_att_control.hpp | 2 +- 14 files changed, 25 insertions(+), 14 deletions(-) rename src/lib/{ecl/AlphaFilter => mathlib/math/filter}/AlphaFilter.hpp (100%) rename src/lib/{ecl/test/test_AlphaFilter.cpp => mathlib/math/test/AlphaFilterTest.cpp} (99%) rename src/lib/mathlib/math/{filter => test}/LowPassFilter2pVector3fTest.cpp (98%) rename src/lib/mathlib/math/{filter => test}/MedianFilterTest.cpp (98%) rename src/lib/mathlib/math/{filter => test}/NotchFilterTest.cpp (99%) diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 42c553b52c..ca047f767c 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt index 603d0ca6f9..ea2e8a9673 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/ecl/CMakeLists.txt @@ -95,6 +95,11 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) add_custom_target(prebuild_targets) + # temporary + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/../mathlib/math/filter/AlphaFilter.hpp ${CMAKE_CURRENT_BINARY_DIR}/mathlib/math/filter/AlphaFilter.hpp COPYONLY) + + include_directories(${CMAKE_CURRENT_BINARY_DIR}) + if(MSVC) add_compile_options( /W4 diff --git a/src/lib/ecl/EKF/estimator_interface.h b/src/lib/ecl/EKF/estimator_interface.h index 2eacbb2dd4..470c2ca9a9 100644 --- a/src/lib/ecl/EKF/estimator_interface.h +++ b/src/lib/ecl/EKF/estimator_interface.h @@ -44,7 +44,6 @@ #include #include "common.h" #include "RingBuffer.h" -#include #include "imu_down_sampler.hpp" #include "sensor_range_finder.hpp" #include "utils.hpp" @@ -52,6 +51,7 @@ #include #include #include +#include using namespace estimator; diff --git a/src/lib/ecl/test/CMakeLists.txt b/src/lib/ecl/test/CMakeLists.txt index 40e3cdbe3b..adbdcb4d1f 100644 --- a/src/lib/ecl/test/CMakeLists.txt +++ b/src/lib/ecl/test/CMakeLists.txt @@ -42,7 +42,6 @@ set(SRCS test_EKF_ringbuffer.cpp test_EKF_measurementSampling.cpp test_EKF_imuSampling.cpp - test_AlphaFilter.cpp test_EKF_fusionLogic.cpp test_EKF_initialization.cpp test_EKF_gps_yaw.cpp diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 900d7feac7..2e9e8d2eb0 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -38,7 +38,8 @@ px4_add_library(mathlib math/filter/NotchFilter.hpp ) -px4_add_unit_gtest(SRC math/filter/LowPassFilter2pVector3fTest.cpp LINKLIBS mathlib) -px4_add_unit_gtest(SRC math/filter/MedianFilterTest.cpp) -px4_add_unit_gtest(SRC math/filter/NotchFilterTest.cpp) +px4_add_unit_gtest(SRC math/test/LowPassFilter2pVector3fTest.cpp LINKLIBS mathlib) +px4_add_unit_gtest(SRC math/test/AlphaFilterTest.cpp) +px4_add_unit_gtest(SRC math/test/MedianFilterTest.cpp) +px4_add_unit_gtest(SRC math/test/NotchFilterTest.cpp) px4_add_unit_gtest(SRC math/FunctionsTest.cpp) diff --git a/src/lib/ecl/AlphaFilter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp similarity index 100% rename from src/lib/ecl/AlphaFilter/AlphaFilter.hpp rename to src/lib/mathlib/math/filter/AlphaFilter.hpp diff --git a/src/lib/ecl/test/test_AlphaFilter.cpp b/src/lib/mathlib/math/test/AlphaFilterTest.cpp similarity index 99% rename from src/lib/ecl/test/test_AlphaFilter.cpp rename to src/lib/mathlib/math/test/AlphaFilterTest.cpp index 06c23299d3..1e0cbc1889 100644 --- a/src/lib/ecl/test/test_AlphaFilter.cpp +++ b/src/lib/mathlib/math/test/AlphaFilterTest.cpp @@ -41,7 +41,7 @@ #include #include -#include "AlphaFilter/AlphaFilter.hpp" +#include using matrix::Vector3f; @@ -63,9 +63,11 @@ TEST(AlphaFilterTest, runZero) { AlphaFilter filter_float{}; const float input = 0.f; + for (int i = 0; i < 10; i++) { filter_float.update(input); } + ASSERT_EQ(filter_float.getState(), input); } @@ -147,6 +149,7 @@ TEST(AlphaFilterTest, convergenceVector3f) // THEN the state of the filter should have converged to the input (1% error allowed) Vector3f output = filter_v3.getState(); + for (int i = 0; i < 3; i++) { ASSERT_NEAR(output(i), input(i), fabsf(0.01f * input(i))); } @@ -168,6 +171,7 @@ TEST(AlphaFilterTest, convergenceVector3fAlpha) // THEN the state of the filter should have reached 65% (2% error allowed) Vector3f output = filter_v3.getState(); + for (int i = 0; i < 3; i++) { ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i))); } @@ -184,12 +188,14 @@ TEST(AlphaFilterTest, convergenceVector3fTauDt) // WHEN we run the filter (1 * time constant) const float n = tau / dt; + for (int i = 0; i < n; i++) { filter_v3.update(input); } // THEN the state of the filter should have reached 65% (2% error allowed) Vector3f output = filter_v3.getState(); + for (int i = 0; i < 3; i++) { ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i))); } diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp b/src/lib/mathlib/math/test/LowPassFilter2pVector3fTest.cpp similarity index 98% rename from src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp rename to src/lib/mathlib/math/test/LowPassFilter2pVector3fTest.cpp index c08869ca6f..c7ee0de444 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp +++ b/src/lib/mathlib/math/test/LowPassFilter2pVector3fTest.cpp @@ -40,7 +40,7 @@ #include #include -#include "LowPassFilter2p.hpp" +#include using matrix::Vector3f; diff --git a/src/lib/mathlib/math/filter/MedianFilterTest.cpp b/src/lib/mathlib/math/test/MedianFilterTest.cpp similarity index 98% rename from src/lib/mathlib/math/filter/MedianFilterTest.cpp rename to src/lib/mathlib/math/test/MedianFilterTest.cpp index 29edfa2669..f74cc7ef76 100644 --- a/src/lib/mathlib/math/filter/MedianFilterTest.cpp +++ b/src/lib/mathlib/math/test/MedianFilterTest.cpp @@ -40,7 +40,7 @@ #include #include -#include "MedianFilter.hpp" +#include using namespace math; using matrix::Vector3f; diff --git a/src/lib/mathlib/math/filter/NotchFilterTest.cpp b/src/lib/mathlib/math/test/NotchFilterTest.cpp similarity index 99% rename from src/lib/mathlib/math/filter/NotchFilterTest.cpp rename to src/lib/mathlib/math/test/NotchFilterTest.cpp index f528a5995c..b9243aa95a 100644 --- a/src/lib/mathlib/math/filter/NotchFilterTest.cpp +++ b/src/lib/mathlib/math/test/NotchFilterTest.cpp @@ -39,7 +39,7 @@ #include #include -#include "NotchFilter.hpp" +#include using namespace math; using matrix::Vector3f; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index c2f2629936..4c45138ffc 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -41,7 +41,7 @@ #include #include -#include +#include class TECS { diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index c4b14200a0..d2868aae0e 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -41,7 +41,7 @@ #include "FlightTask.hpp" #include "Sticks.hpp" -#include +#include #include class FlightTaskManualAltitude : public FlightTask diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 06ae583ae4..347dbaf049 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -40,7 +40,7 @@ #pragma once #include -#include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index c8cc197aaa..c5da9ac6e7 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include