Quadrotor mixer, compile speed, configuration, json function, and backward compatability additions

CMakeLists.txt

@@ -40,6 +40,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 # Set the path to the Boost root directory
+set(Boost_NO_BOOST_CMAKE ON)
 set(BOOST_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/include/boost")
 
 # find dependencies
@@ -67,6 +68,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/json/include)
 
 set(SOURCES
   src/config/config.cpp
+  src/control/vehicleParams.cpp
   src/control/control.cpp
   src/control/control_callback.cpp
   src/control/mrac/logging_mrac.cpp
@@ -89,15 +91,14 @@ set(SOURCES
   src/utils/json_parser.cpp
 )
 
-
 # Add executable
 add_executable(flightstack ${SOURCES})
-
+target_compile_definitions(flightstack PUBLIC  BOOST_ALL_DYN_LINK)
 # Add header files to the target
 target_include_directories(flightstack PUBLIC)
 
 # Link against Boost libraries
-target_link_libraries(flightstack ${Boost_LIBRARIES})
+target_link_libraries(flightstack ${Boost_LIBRARIES} stdc++fs)
 
 ament_target_dependencies(flightstack rclcpp px4_msgs lifecycle_msgs rclcpp_lifecycle)
 

include/flightstack/config/config.hpp

@@ -43,6 +43,22 @@
 // Forward declaration of MultiThreadedNode class
 class MultiThreadedNode;
 
+/*********************************************************************************************************************
+  COMPILE SWITCHES
+**********************************************************************************************************************
+*/
+#define ENABLE_MOCAP false
+
+/*********************************************************************************************************************
+  Architecture selection
+**********************************************************************************************************************
+*/
+// Define named constants for vehicle architecture
+#define ARCH_X8 0
+#define ARCH_QUAD 1
+// SELECT here the ARCHITECTURE you want to run -----------------------------------------------------------------------
+#define VEH_ARCH ARCH_QUAD
+
 /*********************************************************************************************************************
   CONTROLLER selection
 **********************************************************************************************************************
@@ -52,8 +68,12 @@ class MultiThreadedNode;
 #define __MRAC__ 2
 
 // SELECT here the CONTROLLER you want to run -----------------------------------------------------------------------
-#define SELECTED_CONTROLLER __MRAC__
+#define SELECTED_CONTROLLER __PID__
 // ------------------------------------------------------------------------------------------------------------------
+#define FUSION_MODE_GPS 0
+#define FUSION_MODE_MOCAP 1
+
+#define EKF2_FUSION_MODE FUSION_MODE_GPS
 
 // Define ControlType based on SELECTED_CONTROLLER using type aliasing
 #if SELECTED_CONTROLLER == __PID__ 

include/flightstack/control/control.hpp

@@ -42,14 +42,12 @@
 #include <vector>
 #include <functional> 
 #include <chrono>
-
-#include <Eigen/Dense>
 #include <boost/numeric/odeint.hpp>
+#include <Eigen/Dense>
 
-#include "vehicle_info.hpp"
-#include "piecewise_polynomial_trajectory.hpp"
+#include "vehicleParams.hpp"
 
-using namespace boost::numeric::odeint;
+using boost::numeric::odeint::runge_kutta4;
 
 // The type of container used to hold the state vector
 typedef std::vector<double> state_type;
@@ -60,6 +58,7 @@ class MultiThreadedNode;
 class Control 
 {
 public:
+  static inline const std::string paramsFile = "./src/flightstack/params/config/vehicle_info.json";
 
   /*
     This struct contains the variables that are common among all controllers as they are the foundations of the base
@@ -124,11 +123,9 @@ class Control
   // Constructor
   Control(MultiThreadedNode& node);
 
-  void readJSONdifferentiatorFile();
-
   // Getter functions
   MultiThreadedNode& getNode() const;
-  const VehicleInfo& getVehicleInfo() const;
+  const ParamsVehicle& getParamsVehicle() const;
   const ControlInternalMembers& getControlInternalMembers() const;
   const std::chrono::duration<double, std::micro>& getAlgorithmExecutionTimeMicroseconds() const;
 
@@ -146,9 +143,9 @@ class Control
 
   Eigen::Matrix3d rotationMatrix321GlobalToLocal(const double& roll, const double& pitch, const double& yaw);
 
-  void computeNormalizedThrustQuadcopterMode(ControlInternalMembers& cim, VehicleInfo& vehicle_info);
+  void computeNormalizedThrustQuadcopterMode(ControlInternalMembers& cim, ParamsVehicle& vehicle_info);
 
-  void computeNormalizedThrust(ControlInternalMembers& cim, VehicleInfo& vehicle_info);
+  void computeNormalizedThrust(ControlInternalMembers& cim, ParamsVehicle& vehicle_info);
 
   void compute_U1_RollDes_PitchDes(ControlInternalMembers& cim);
 
@@ -203,8 +200,8 @@ class Control
 
 protected:
 
-  VehicleInfo vehicle_info;
-
+  ParamsVehicle vehicle_info;
+  
   ControlInternalMembers cim;
 
   ControlReferences cr;

include/flightstack/control/mrac/logging_mrac.hpp

@@ -40,8 +40,8 @@
 #include <atomic>
 #include <cstddef>
 #include <chrono>
-#include <filesystem>
 #include <fstream>
+#include <experimental/filesystem>
 #include <iomanip>
 #include <ostream>
 #include <sstream>
@@ -76,6 +76,7 @@ namespace src = boost::log::sources;
 namespace expr = boost::log::expressions;
 namespace attrs = boost::log::attributes;
 namespace keywords = boost::log::keywords;
+namespace fs = std::experimental::filesystem;
 
 // Forward declaration of MultiThreadedNode class
 class MultiThreadedNode;

include/flightstack/control/mrac/mrac.hpp

@@ -33,7 +33,6 @@
  * 
  * GitHub:    https://github.com/andrealaffly/ACSL-flightstack.git
  **********************************************************************************************************************/
-
 #ifndef MRAC_HPP
 #define MRAC_HPP
 
@@ -47,6 +46,9 @@
 #include "mrac_gains.hpp"
 #include "continuous_lyapunov_equation.hpp"
 #include "projection_operator.hpp"
+#include "json_parser.hpp"
+
+using namespace nlohmann;
 
 // Forward declaration of LogData_MRAC class
 class LogData_MRAC;
@@ -148,39 +150,39 @@ class MRAC : public Control
   std::shared_ptr<LogData_MRAC> getLogData() const;
   static const std::string& getControllerName();
 
-  void readJSONfile(const std::string& fileName);
+  void loadGains(json j);
 
-  void initializeControllerParameters(VehicleInfo& vehicle_info, GainsMRAC& gains_);
+  void initializeControllerParameters(ParamsVehicle& vehicle_info, GainsMRAC& gains_);
 
   void assignSystemToDxdt(state_type /* &x */, state_type &dxdt, const double /* t */);
 
   void computeFilterDifferentiatorVariables(ControlInternalMembers& cim, 
-                                            VehicleInfo& vehicle_info, 
+                                            ParamsVehicle& vehicle_info, 
                                             StateController& state_);
 
   void computeOuterLoop(ControlInternalMembers& cim,
-                        VehicleInfo& vehicle_info,
+                        ParamsVehicle& vehicle_info,
                         StateController& state_, 
                         ControlReferences& cr,
                         GainsMRAC& gains_,
                         ControllerSpecificInternalMembers& csim_);
 
   void computeOuterLoopDEBUGGING(ControlInternalMembers& cim,
-                                  VehicleInfo& vehicle_info,
+                                  ParamsVehicle& vehicle_info,
                                   StateController& state_, 
                                   ControlReferences& cr,
                                   GainsMRAC& gains_,
                                   ControllerSpecificInternalMembers& csim_);
 
   void computeInnerLoop(ControlInternalMembers& cim,
-                        VehicleInfo& vehicle_info,
+                        ParamsVehicle& vehicle_info,
                         StateController& state_, 
                         ControlReferences& cr,
                         GainsMRAC& gains_,
                         ControllerSpecificInternalMembers& csim_);
 
   void computeInnerLoopDEBUGGING(ControlInternalMembers& cim,
-                                  VehicleInfo& vehicle_info,
+                                  ParamsVehicle& vehicle_info,
                                   StateController& state_, 
                                   ControlReferences& cr,
                                   GainsMRAC& gains_,

include/flightstack/control/mrac/mrac_gains.hpp

@@ -33,7 +33,6 @@
  * 
  * GitHub:    https://github.com/andrealaffly/ACSL-flightstack.git
  **********************************************************************************************************************/
-
 #ifndef MRAC_GAINS_HPP
 #define MRAC_GAINS_HPP
 

include/flightstack/control/mrac/projection_operator.hpp

@@ -35,7 +35,6 @@
  * 
  * GitHub:    https://github.com/andrealaffly/ACSL-flightstack.git
  **********************************************************************************************************************/
-
 #ifndef PROJECTION_OPERATOR_HPP
 #define PROJECTION_OPERATOR_HPP
 

include/flightstack/control/pid/logging_pid.hpp

@@ -37,45 +37,9 @@
 #ifndef LOGGING_PID_HPP
 #define LOGGING_PID_HPP
 
-#include <atomic>
-#include <cstddef>
-#include <chrono>
-#include <filesystem>
-#include <fstream>
-#include <iomanip>
-#include <ostream>
-#include <sstream>
-#include <string>
 
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <boost/date_time/posix_time/posix_time_types.hpp>
-#include <boost/log/attributes.hpp>
-#include <boost/log/attributes/scoped_attribute.hpp>
-#include <boost/log/core.hpp>
-#include <boost/log/expressions.hpp>
-#include <boost/log/sinks/sync_frontend.hpp>
-#include <boost/log/sinks/text_ostream_backend.hpp>
-#include <boost/log/sources/basic_logger.hpp>
 #include <boost/log/sources/logger.hpp>
-#include <boost/log/sources/record_ostream.hpp>
-#include <boost/log/sources/severity_channel_logger.hpp>
-#include <boost/log/sources/severity_logger.hpp>
-#include <boost/log/support/date_time.hpp>
-#include <boost/log/trivial.hpp>
-#include <boost/log/utility/setup/common_attributes.hpp>
-#include <boost/log/utility/setup/file.hpp>
-#include <boost/log/utility/value_ref.hpp>
-#include <boost/phoenix/bind.hpp>
-#include <boost/smart_ptr/make_shared_object.hpp>
-#include <boost/smart_ptr/shared_ptr.hpp>
-#include <Eigen/Dense>
-
-namespace logging = boost::log;
-namespace sinks = boost::log::sinks;
-namespace src = boost::log::sources;
-namespace expr = boost::log::expressions;
-namespace attrs = boost::log::attributes;
-namespace keywords = boost::log::keywords;
+using boost::log::sources::logger;
 
 // Forward declaration of MultiThreadedNode class
 class MultiThreadedNode;
@@ -88,7 +52,7 @@ class LogData_PID
 public:
 
   // Define logger for LogData
-  static src::logger logger_logdata;
+  static logger logger_logdata;
 
 	// Constructor
   LogData_PID(MultiThreadedNode& node, PID& controller);

include/flightstack/control/pid/pid.hpp

@@ -33,7 +33,6 @@
  * 
  * GitHub:    https://github.com/andrealaffly/ACSL-flightstack.git
  **********************************************************************************************************************/
-
 #ifndef PID_HPP
 #define PID_HPP
 
@@ -116,18 +115,18 @@ class PID : public Control
   void assignSystemToDxdt(state_type /* &x */, state_type &dxdt, const double /* t */);
 
   void computeFilterDifferentiatorVariables(ControlInternalMembers& cim, 
-                                            VehicleInfo& vehicle_info, 
+                                            ParamsVehicle& vehicle_info, 
                                             StateController& state_);
 
   void computeOuterLoop(ControlInternalMembers& cim,
-                        VehicleInfo& vehicle_info,
+                        ParamsVehicle& vehicle_info,
                         StateController& state_, 
                         ControlReferences& cr,
                         GainsPID& gains_,
                         ControllerSpecificInternalMembers& csim_);
 
   void computeInnerLoop(ControlInternalMembers& cim,
-                        VehicleInfo& vehicle_info,
+                        ParamsVehicle& vehicle_info,
                         StateController& state_, 
                         ControlReferences& cr,
                         GainsPID& gains_,

include/flightstack/control/pid/pid_gains.hpp

@@ -33,12 +33,9 @@
  * 
  * GitHub:    https://github.com/andrealaffly/ACSL-flightstack.git
  **********************************************************************************************************************/
-
 #ifndef PID_GAINS_HPP
 #define PID_GAINS_HPP
 
-#include <cmath>
-#include <algorithm>
 
 #include <Eigen/Dense>