Added Funnel Two-Layer MRAC controller (#11)

Author: mattia-gramuglia

.gitignore

@@ -12,5 +12,3 @@
 
 # Ignore .vscode folder
 .vscode/
-
-

CMakeLists.txt

@@ -51,12 +51,15 @@ find_package(lifecycle_msgs REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 
 include_directories(${Boost_INCLUDE_DIRS})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/eigen)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/config)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/funnel_two_layer_mrac)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/mrac)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/pid)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/control/two_layer_mrac)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/logging)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/mocap)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/flightstack/pixhawk_interface)
@@ -69,10 +72,15 @@ set(SOURCES
   src/config/config.cpp
   src/control/control.cpp
   src/control/control_callback.cpp
+  src/control/outer_loop_safety_mechanism.cpp
+  src/control/funnel_two_layer_mrac/funnel_two_layer_mrac.cpp
+  src/control/funnel_two_layer_mrac/logging_funnel_two_layer_mrac.cpp
   src/control/mrac/logging_mrac.cpp
   src/control/mrac/mrac.cpp
   src/control/pid/logging_pid.cpp
   src/control/pid/pid.cpp
+  src/control/two_layer_mrac/two_layer_mrac.cpp
+  src/control/two_layer_mrac/logging_two_layer_mrac.cpp
   src/logging/logging_mocap.cpp
   src/main.cpp
   src/mocap/io_context.cpp
@@ -109,6 +117,23 @@ install(TARGETS
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_low_pass_filter
+    test/utils/low_pass_filter_test.cpp
+  )
+
+  if(TARGET test_low_pass_filter)
+    target_include_directories(test_low_pass_filter PUBLIC
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
+    # Optional: link extra libraries if needed
+    # target_link_libraries(test_low_pass_filter some_library)
+
+    # Correct way to label the test for CTest
+    set_tests_properties(test_low_pass_filter PROPERTIES LABELS "flightstack_core")
+  endif()
 endif()
 
 ament_package()

include/flightstack/config/config.hpp

@@ -39,6 +39,7 @@
 
 #include <fstream>
 #include <nlohmann/json.hpp>
+#include <Eigen/Dense>
 
 // Forward declaration of MultiThreadedNode class
 class MultiThreadedNode;
@@ -50,9 +51,11 @@ class MultiThreadedNode;
 // Define named constants for controller types
 #define __PID__ 1
 #define __MRAC__ 2
+#define __TWO_LAYER_MRAC__ 3
+#define __FUNNEL_TWO_LAYER_MRAC__ 4
 
 // SELECT here the CONTROLLER you want to run -----------------------------------------------------------------------
-#define SELECTED_CONTROLLER __MRAC__
+#define SELECTED_CONTROLLER __FUNNEL_TWO_LAYER_MRAC__
 // ------------------------------------------------------------------------------------------------------------------
 
 // Define ControlType based on SELECTED_CONTROLLER using type aliasing
@@ -68,10 +71,41 @@ using ControlType = PID;
 #include "logging_mrac.hpp"
 using ControlType = MRAC;
 
+#elif SELECTED_CONTROLLER == __TWO_LAYER_MRAC__
+
+#include "two_layer_mrac.hpp"
+#include "logging_two_layer_mrac.hpp"
+using ControlType = TwoLayerMRAC;
+
+#elif SELECTED_CONTROLLER == __FUNNEL_TWO_LAYER_MRAC__
+
+#include "funnel_two_layer_mrac.hpp"
+#include "logging_funnel_two_layer_mrac.hpp"
+using ControlType = FunnelTwoLayerMRAC;
+
 #else
 #error "ERROR: Unsupported controller type selected."
 #endif
 
+namespace config_param {
+
+// Flag for publishing data to the motors
+inline constexpr bool PUBLISH_ACTUATOR_MOTORS_FLAG = true;
+
+// Flag to enable/disable the MRAC projection operator
+inline constexpr bool USE_PROJECTION_OPERATOR = true;
+
+// Flag to enable/disable the debug logger
+inline constexpr bool USE_DEBUG_LOGGER = false;
+
+// Flag to enable/disable the safety mechanism between outer and inner loop
+inline constexpr bool USE_OUTER_LOOP_SAFETY_MECHANISM = true;
+
+// Flag to enable/disable motor failure
+inline constexpr bool ENABLE_MOTOR_FAILURE = false;
+
+} // namespace config_param
+
 
 /*********************************************************************************************************************
   Configurations Parameters set from config.json 
@@ -85,6 +119,16 @@ struct ConfigurationParameters
   // Time in seconds for which the UAV will perform hovering after having completed the user-defined trajectory
   double hover_after_trajectory_time_seconds;
 
+  // Time in seconds at which motor failure happens (start)
+  double motor_failure_start_time_seconds;
+
+  // Time in seconds for the motor failure duration
+  double motor_failure_duration_time_seconds;
+
+  // Array containing the motor efficiencies to simulate motor failure. Order: T1, T2, ..., T8.
+  // A "zero" turns off the motor completely, a "one" means the motor works as usual.
+  Eigen::Matrix<double, 8, 1> motor_failure_efficiencies;
+
   static ConfigurationParameters readConfigurationParametersFile(const std::string& configFileName);
 
 };
@@ -95,9 +139,6 @@ struct ConfigurationParameters
 */
 struct GlobalParameters
 {
-  // Flag for publishing data to the motors
-  bool PUBLISH_ACTUATOR_MOTORS_FLAG;
-
   // Time in seconds at which we arm the motors
   double ARM_START_TIME_SECONDS; 
 
@@ -117,6 +158,12 @@ struct GlobalParameters
   // This must always be performed before takeoff
   double OFFSET_ODOMETRY_TIME_SECONDS;
 
+  // Time in seconds at which motor failure happens (start)
+  double MOTOR_FAILURE_START_TIME_SECONDS;
+
+  // Time in seconds at which motor failure is resolved (end)
+  double MOTOR_FAILURE_END_TIME_SECONDS;
+
   // Default constructor
   GlobalParameters() = default;