diff --git a/CMakeLists.txt b/CMakeLists.txt index 73c03d7..cb9fcec 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/include/flightstack/config/config.hpp b/include/flightstack/config/config.hpp index 2b21827..00b7fcc 100644 --- a/include/flightstack/config/config.hpp +++ b/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__ diff --git a/include/flightstack/control/control.hpp b/include/flightstack/control/control.hpp index 31adcc2..6bce16c 100644 --- a/include/flightstack/control/control.hpp +++ b/include/flightstack/control/control.hpp @@ -42,14 +42,12 @@ #include #include #include - -#include #include +#include -#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 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& 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; diff --git a/include/flightstack/control/mrac/logging_mrac.hpp b/include/flightstack/control/mrac/logging_mrac.hpp index 2972310..ba3204b 100644 --- a/include/flightstack/control/mrac/logging_mrac.hpp +++ b/include/flightstack/control/mrac/logging_mrac.hpp @@ -40,8 +40,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -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; diff --git a/include/flightstack/control/mrac/mrac.hpp b/include/flightstack/control/mrac/mrac.hpp index fa8ecd0..8219898 100644 --- a/include/flightstack/control/mrac/mrac.hpp +++ b/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 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_, diff --git a/include/flightstack/control/mrac/mrac_gains.hpp b/include/flightstack/control/mrac/mrac_gains.hpp index 8b9f879..3ec8e85 100644 --- a/include/flightstack/control/mrac/mrac_gains.hpp +++ b/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 diff --git a/include/flightstack/control/mrac/projection_operator.hpp b/include/flightstack/control/mrac/projection_operator.hpp index c014d35..05cc81f 100644 --- a/include/flightstack/control/mrac/projection_operator.hpp +++ b/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 diff --git a/include/flightstack/control/pid/logging_pid.hpp b/include/flightstack/control/pid/logging_pid.hpp index d8c266a..5708561 100644 --- a/include/flightstack/control/pid/logging_pid.hpp +++ b/include/flightstack/control/pid/logging_pid.hpp @@ -37,45 +37,9 @@ #ifndef LOGGING_PID_HPP #define LOGGING_PID_HPP -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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); diff --git a/include/flightstack/control/pid/pid.hpp b/include/flightstack/control/pid/pid.hpp index 9fadc75..1b2453b 100644 --- a/include/flightstack/control/pid/pid.hpp +++ b/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_, diff --git a/include/flightstack/control/pid/pid_gains.hpp b/include/flightstack/control/pid/pid_gains.hpp index 8d05a65..417a118 100644 --- a/include/flightstack/control/pid/pid_gains.hpp +++ b/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 -#include #include diff --git a/include/flightstack/control/vehicleParams.hpp b/include/flightstack/control/vehicleParams.hpp new file mode 100644 index 0000000..c3d61c4 --- /dev/null +++ b/include/flightstack/control/vehicleParams.hpp @@ -0,0 +1,113 @@ +/*********************************************************************************************************************** + * Copyright (c) 2024 Mattia Gramuglia, Giri M. Kumar, Andrea L'Afflitto. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + **********************************************************************************************************************/ + +/*********************************************************************************************************************** + * File: pcac_gains.hpp + * Author: Mattia Gramuglia + * Date: April 23, 2024 + * For info: Andrea L'Afflitto + * a.lafflitto@vt.edu + * + * Description: Tuning gains of the PCAC controller. + * + * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git + **********************************************************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +using Eigen::MatrixXd; +using std::string; +using nlohmann::json; + + /* + PCAC coding style guide + 1. Variables are camelCase + 2. Variables must not include digits + 3. Matrices are PascalCase and may use numbers + 4. Instances of gains structure are all UPPERCASE + 5. + */ +inline constexpr double GRAVITATIONAL_ACCELERATION = 9.80665; // [m/s^2] + +// Struct containing the PCAC parameters coming from the .json file +class ParamsVehicle +{ + public: + double motorSaturationUpper; + double motorSaturationLower; + double minValuePublishMotors; + double mass; + + Eigen::Matrix3d inertia_matrix; + double air_density; + double surface_area; + Eigen::Matrix3d drag_coefficient_matrix; + + double distance_motors_centerline_x_dir; + double distance_motors_centerline_y_dir; + double propeller_drag_coefficient; + + // Filter/differentiator values to compute roll_desired_dot + Eigen::Matrix2d A_filter_roll_des; + Eigen::Vector2d B_filter_roll_des; + Eigen::RowVector2d C_filter_roll_des; + double D_filter_roll_des; + + // Filter/differentiator values to compute pitch_desired_dot + Eigen::Matrix2d A_filter_pitch_des; + Eigen::Vector2d B_filter_pitch_des; + Eigen::RowVector2d C_filter_pitch_des; + double D_filter_pitch_des; + + // Filter/differentiator values to compute roll_desired_dot_dot + Eigen::Matrix2d A_filter_roll_dot_des; + Eigen::Vector2d B_filter_roll_dot_des; + Eigen::RowVector2d C_filter_roll_dot_des; + double D_filter_roll_dot_des; + + // Filter/differentiator values to compute pitch_desired_dot_dot + Eigen::Matrix2d A_filter_pitch_dot_des; + Eigen::Vector2d B_filter_pitch_dot_des; + Eigen::RowVector2d C_filter_pitch_dot_des; + double D_filter_pitch_dot_des; + + // Declare matrix mixer_matrix using a lambda function + Eigen::Matrix mixer_matrix; + + // Declare matrix mixer_matrix_quadcopter using a lambda function + Eigen::Matrix4d mixer_matrix_quadcopter; + + // Polynomial coefficients vector to evaluate the Commanded Thrust [-] based on the Thrust in Newton + // S500 with Holybro props + Eigen::VectorXd thrust_polynomial_coefficients_quadcopter; + + ParamsVehicle(); + ParamsVehicle(json j); +}; diff --git a/include/flightstack/control/vehicle_info.hpp b/include/flightstack/control/vehicle_info.hpp deleted file mode 100644 index 9abf9ee..0000000 --- a/include/flightstack/control/vehicle_info.hpp +++ /dev/null @@ -1,248 +0,0 @@ -/*********************************************************************************************************************** - * Copyright (c) 2024 Mattia Gramuglia, Giri M. Kumar, Andrea L'Afflitto. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the - * following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following - * disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the - * following disclaimer in the documentation and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote - * products derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - **********************************************************************************************************************/ - -/*********************************************************************************************************************** - * File: vehicle_info.hpp - * Author: Mattia Gramuglia - * Date: April 22, 2024 - * For info: Andrea L'Afflitto - * a.lafflitto@vt.edu - * - * Description: Vehicle properties such as mass, inertia matrix, motor thrust curves, etc. - * - * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git - **********************************************************************************************************************/ - -#ifndef VEHICLE_INFO_HPP -#define VEHICLE_INFO_HPP - -#include - -#include - -inline constexpr double GRAVITATIONAL_ACCELERATION = 9.80665; // [m/s^2] - -inline constexpr double UPPER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON = 9.5; - -inline constexpr double LOWER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON = 0.3; - -inline constexpr double MINIMUM_VALUE_PUBLISH_MOTORS = 0.05; - - -struct VehicleInfo -{ - - const double mass = 1.920; // [kg] vehicle mass - - const Eigen::Matrix3d inertia_matrix = (Eigen::Matrix3d() << - 2.271990e-02, -6.557000e-06, -1.003498e-03, - -6.557000e-06, 2.202047e-02, 5.658400e-06, - -1.003498e-03, 5.658400e-06, 1.614693e-02 - ).finished(); // [kg*m^2] inertia matrix of the vehicle system (drone frame + box + propellers) expressed in - // Pixhawk coordinate system (FRD - x-Front, y-Right, z-Down), computed at the vehicle center of mass - - const double air_density = 1.225; // [kg/m^3] air density - - const double surface_area = 0.07; // [m^2] surface area of the drone to account fro the drag - - const double drag_coefficient = 1.28; // [-] drag coefficient (equal to that of a plate) - const Eigen::Matrix3d drag_coefficient_matrix = (Eigen::Matrix3d() << - drag_coefficient, 0, 0, - 0, drag_coefficient, 0, - 0, 0, 0 - ).finished(); - - const double distance_motors_cenetrline_x_dir = 0.0881269; // [m] distance between the centerline of the drone and the - // motors along x direction in local FRD frame - const double distance_motors_cenetrline_y_dir = 0.1083450; // [m] distance between the centerline of the drone and the - // motors along y direction in local FRD frame - const double propeller_drag_coefficient = 0.01; // [m] propellers drag coefficient - - // const Eigen::Matrix2d A_filter_roll_des = (Eigen::Matrix2d() << - // -40.824, -1049.8, - // 1, 0 - // ).finished(); // A matrix of the roll_des filter - - // const Eigen::Vector2d B_filter_roll_des = Eigen::Vector2d(1, 0); // B matrix of the roll_des filter - - // const Eigen::RowVector2d C_filter_roll_des = Eigen::RowVector2d(1049.8, 0); // C matrix of the roll_des filter - - // const double D_filter_roll_des = 0.0; // D matrix of the roll_des filter - - // const Eigen::Matrix2d A_filter_pitch_des = (Eigen::Matrix2d() << - // -40.824, -1049.8, - // 1, 0 - // ).finished(); // A matrix of the pitch_des filter - - // const Eigen::Vector2d B_filter_pitch_des = Eigen::Vector2d(1, 0); // B matrix of the pitch_des filter - - // const Eigen::RowVector2d C_filter_pitch_des = Eigen::RowVector2d(1049.8, 0); // C matrix of the pitch_des filter - - // const double D_filter_pitch_des = 0.0; // D matrix of the pitch_des filter - - // Filter/differentiator values to compute roll_desired_dot - Eigen::Matrix2d A_filter_roll_des; - Eigen::Vector2d B_filter_roll_des; - Eigen::RowVector2d C_filter_roll_des; - double D_filter_roll_des; - - // Filter/differentiator values to compute pitch_desired_dot - Eigen::Matrix2d A_filter_pitch_des; - Eigen::Vector2d B_filter_pitch_des; - Eigen::RowVector2d C_filter_pitch_des; - double D_filter_pitch_des; - - // Filter/differentiator values to compute roll_desired_dot_dot - Eigen::Matrix2d A_filter_roll_dot_des; - Eigen::Vector2d B_filter_roll_dot_des; - Eigen::RowVector2d C_filter_roll_dot_des; - double D_filter_roll_dot_des; - - // Filter/differentiator values to compute pitch_desired_dot_dot - Eigen::Matrix2d A_filter_pitch_dot_des; - Eigen::Vector2d B_filter_pitch_dot_des; - Eigen::RowVector2d C_filter_pitch_dot_des; - double D_filter_pitch_dot_des; - - // Declare const matrix mixer_matrix using a lambda function - const Eigen::Matrix mixer_matrix = [this]() { - Eigen::Matrix mat; // 8 rows, 4 columns - - // [1/8, -1/(8*l_y), 1/(8*l_x), 1/(8*c_t)] - // [1/8, 1/(8*l_y), 1/(8*l_x), -1/(8*c_t)] - // [1/8, 1/(8*l_y), -1/(8*l_x), 1/(8*c_t)] - // [1/8, -1/(8*l_y), -1/(8*l_x), -1/(8*c_t)] - // [1/8, 1/(8*l_y), 1/(8*l_x), 1/(8*c_t)] - // [1/8, -1/(8*l_y), 1/(8*l_x), -1/(8*c_t)] - // [1/8, -1/(8*l_y), -1/(8*l_x), 1/(8*c_t)] - // [1/8, 1/(8*l_y), -1/(8*l_x), -1/(8*c_t)] - - // Assign values element-by-element - mat(0, 0) = 1.0 / 8.0; - mat(0, 1) = -1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(0, 2) = 1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(0, 3) = 1.0 / (8.0 * this->propeller_drag_coefficient); - - mat(1, 0) = 1.0 / 8.0; - mat(1, 1) = 1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(1, 2) = 1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(1, 3) = -1.0 / (8.0 * this->propeller_drag_coefficient); - - mat(2, 0) = 1.0 / 8.0; - mat(2, 1) = 1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(2, 2) = -1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(2, 3) = 1.0 / (8.0 * this->propeller_drag_coefficient); - - mat(3, 0) = 1.0 / 8.0; - mat(3, 1) = -1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(3, 2) = -1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(3, 3) = -1.0 / (8.0 * this->propeller_drag_coefficient); - - mat(4, 0) = 1.0 / 8.0; - mat(4, 1) = 1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(4, 2) = 1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(4, 3) = 1.0 / (8.0 * this->propeller_drag_coefficient); - - mat(5, 0) = 1.0 / 8.0; - mat(5, 1) = -1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(5, 2) = 1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(5, 3) = -1.0 / (8.0 * this->propeller_drag_coefficient); - - mat(6, 0) = 1.0 / 8.0; - mat(6, 1) = -1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(6, 2) = -1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(6, 3) = 1.0 / (8.0 * this->propeller_drag_coefficient); - - mat(7, 0) = 1.0 / 8.0; - mat(7, 1) = 1.0 / (8.0 * this->distance_motors_cenetrline_y_dir); - mat(7, 2) = -1.0 / (8.0 * this->distance_motors_cenetrline_x_dir); - mat(7, 3) = -1.0 / (8.0 * this->propeller_drag_coefficient); - - return mat; - }(); - - - // Declare const matrix mixer_matrix_quadcopter using a lambda function - const Eigen::Matrix4d mixer_matrix_quadcopter = [this]() { - Eigen::Matrix4d mat; // 4 rows, 4 columns - - // [1/4, -1/(4*l_y), 1/(4*l_x), 1/(4*c_t)] - // [1/4, 1/(4*l_y), 1/(4*l_x), -1/(4*c_t)] - // [1/4, 1/(4*l_y), -1/(4*l_x), 1/(4*c_t)] - // [1/4, -1/(4*l_y), -1/(4*l_x), -1/(4*c_t)] - - // Assign values element-by-element - mat(0, 0) = 1.0 / 4.0; - mat(0, 1) = -1.0 / (4.0 * this->distance_motors_cenetrline_y_dir); - mat(0, 2) = 1.0 / (4.0 * this->distance_motors_cenetrline_x_dir); - mat(0, 3) = 1.0 / (4.0 * this->propeller_drag_coefficient); - - mat(1, 0) = 1.0 / 4.0; - mat(1, 1) = 1.0 / (4.0 * this->distance_motors_cenetrline_y_dir); - mat(1, 2) = 1.0 / (4.0 * this->distance_motors_cenetrline_x_dir); - mat(1, 3) = -1.0 / (4.0 * this->propeller_drag_coefficient); - - mat(2, 0) = 1.0 / 4.0; - mat(2, 1) = 1.0 / (4.0 * this->distance_motors_cenetrline_y_dir); - mat(2, 2) = -1.0 / (4.0 * this->distance_motors_cenetrline_x_dir); - mat(2, 3) = 1.0 / (4.0 * this->propeller_drag_coefficient); - - mat(3, 0) = 1.0 / 4.0; - mat(3, 1) = -1.0 / (4.0 * this->distance_motors_cenetrline_y_dir); - mat(3, 2) = -1.0 / (4.0 * this->distance_motors_cenetrline_x_dir); - mat(3, 3) = -1.0 / (4.0 * this->propeller_drag_coefficient); - - return mat; - }(); - - // // Polynomial coefficients vector to evaluate the Commanded Thrust [-] based on the Thrust in Newton - // // OLD ESCs - // const Eigen::VectorXd thrust_polynomial_coefficients_quadcopter = (Eigen::VectorXd(8) << - // 0.0000105272829209078, - // -0.000353007963591775, - // 0.00474469218478217, - // -0.0326989257250683, - // 0.123233111657682, - // -0.256734365616374, - // 0.376206271320848, - // -0.0492707360005048 - // ).finished(); - - // Polynomial coefficients vector to evaluate the Commanded Thrust [-] based on the Thrust in Newton - // NEW ESCs (TMotor) - const Eigen::VectorXd thrust_polynomial_coefficients_quadcopter = (Eigen::VectorXd(8) << - 0.00000318912344541255, - -0.000107583270223678, - 0.00147671457913486, - -0.0107666934546496, - 0.0459838527842087, - -0.121504752465409, - 0.285725583084306, - -0.0118110779377008 - ).finished(); - -}; - - -#endif // VEHICLE_INFO_HPP diff --git a/include/flightstack/logging/logging_mocap.hpp b/include/flightstack/logging/logging_mocap.hpp index 29650eb..48d6515 100644 --- a/include/flightstack/logging/logging_mocap.hpp +++ b/include/flightstack/logging/logging_mocap.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include #include @@ -70,6 +70,7 @@ #include #include +namespace fs = std::experimental::filesystem; namespace logging = boost::log; namespace sinks = boost::log::sinks; namespace src = boost::log::sources; diff --git a/include/flightstack/main.hpp b/include/flightstack/main.hpp index 8ce25cb..1fc2b02 100644 --- a/include/flightstack/main.hpp +++ b/include/flightstack/main.hpp @@ -37,31 +37,4 @@ #ifndef MAIN_HPP #define MAIN_HPP -#include "multi_threaded_node.hpp" - -/*********************************************************************************************************************** - DATA FUSION using GPS or MOCAP -***********************************************************************************************************************/ -// Enum class for the possible modes to fuse Pixhawk IMU data -enum class EKF2FusionMode { - GPS, - MOCAP // VICON -}; - -/* - * Perform data fusion using GPS - */ -// inline constexpr EKF2FusionMode CURRENT_EKF2_FUSION_MODE = EKF2FusionMode::GPS; - -/* - * Perform data fusion using MOCAP - */ -inline constexpr EKF2FusionMode CURRENT_EKF2_FUSION_MODE = EKF2FusionMode::MOCAP; - -/*********************************************************************************************************************** - -***********************************************************************************************************************/ - - - #endif // MAIN_HPP \ No newline at end of file diff --git a/include/flightstack/mocap/io_context.hpp b/include/flightstack/mocap/io_context.hpp index 4299b3f..fc884fd 100644 --- a/include/flightstack/mocap/io_context.hpp +++ b/include/flightstack/mocap/io_context.hpp @@ -61,6 +61,7 @@ #include #include #include +#include #include diff --git a/include/flightstack/mocap/mocap.hpp b/include/flightstack/mocap/mocap.hpp index cb5ea5d..384f61f 100644 --- a/include/flightstack/mocap/mocap.hpp +++ b/include/flightstack/mocap/mocap.hpp @@ -63,7 +63,6 @@ #include #include #include -#include #include #include @@ -73,15 +72,12 @@ #include "udp_driver.hpp" #include "logging_mocap.hpp" +#include "json_parser.hpp" using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; +using namespace nlohmann; -// -> IP of the Odroid M1s -inline constexpr const char* GROUND_STATION_IP = "127.0.0.1"; // For testing -inline constexpr const char* ODROID_M1S_IP = "192.168.12.1"; -// -> PORT of the Odroid M1s -inline constexpr uint16_t ODROID_M1S_PORT = 52000; /// ------------- USER DEFINED ------------- /// @@ -121,6 +117,13 @@ namespace _udp_driver_ double yawspeed; }; + struct network_params + { + std::string GROUND_STATION_IP; + std::string HOST_IP; + int HOST_PORT; + }; + /// \brief UdpReceiverNode class which can receive UDP datagrams class UdpReceiverNode final : public lc::LifecycleNode @@ -173,15 +176,12 @@ class UdpReceiverNode final /// \brief Get the parameters for the ip and port to ping void get_params(); + // Load network parameters from json file + void setNetwork(json j); + /// Pointer to the asio context owned by this node for async communication std::unique_ptr m_owned_ctx{}; - /// String for the ip of the odrioid - std::string m_ip{}; - - /// String for the port of the odroid - uint16_t m_port{}; - /// Pointer for the udp driver which wraps the udp socket std::unique_ptr m_udp_driver; @@ -200,6 +200,9 @@ class UdpReceiverNode final /// Instantiate mocap_states struct struct mocap_states mc; + /// Instantiate network_params struct + struct network_params np; + /// \brief Debugger function to output the mocap data. void debugMocapData2screen(); diff --git a/include/flightstack/multi_threaded_node.hpp b/include/flightstack/multi_threaded_node.hpp index ab48754..91cb4d4 100644 --- a/include/flightstack/multi_threaded_node.hpp +++ b/include/flightstack/multi_threaded_node.hpp @@ -58,10 +58,16 @@ #include "config.hpp" #include "json_parser.hpp" #include "vehicle_state.hpp" + #include "user_defined_trajectory.hpp" #include "piecewise_polynomial_trajectory.hpp" + #include "control.hpp" + +#if EKF2_FUSION_MODE == FUSION_MODE_MOCAP #include "mocap.hpp" +#endif //ENABLE_MOCAP == true + #include "print_helpers.hpp" @@ -132,7 +138,7 @@ class MultiThreadedNode : public rclcpp::Node 2 = flight terminated */ int offboard_flag_; - + // Create a pointer to the PiecewisePolynomialTrajectory instance std::shared_ptr user_defined_trajectory_; @@ -156,4 +162,4 @@ class MultiThreadedNode : public rclcpp::Node std::string string_thread_id(); -#endif // MULTI_THREADED_NODE_HPP +#endif //MULTI_THREADED_NODE_HPP diff --git a/include/flightstack/user_defined_trajectory/piecewise_polynomial_trajectory.hpp b/include/flightstack/user_defined_trajectory/piecewise_polynomial_trajectory.hpp index 42216e7..0e64ba1 100644 --- a/include/flightstack/user_defined_trajectory/piecewise_polynomial_trajectory.hpp +++ b/include/flightstack/user_defined_trajectory/piecewise_polynomial_trajectory.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include diff --git a/include/flightstack/utils/json_parser.hpp b/include/flightstack/utils/json_parser.hpp index d2422c3..ea5d40c 100644 --- a/include/flightstack/utils/json_parser.hpp +++ b/include/flightstack/utils/json_parser.hpp @@ -42,11 +42,13 @@ #include #include +using namespace nlohmann; + +json readJsonFile(const std::string& jsonFile); struct MotorsCommands { void readJSONConfig(const std::string& jsonFile); - float motor_1; float motor_2; float motor_3; @@ -67,26 +69,34 @@ template inline Eigen::Matrix extractMatrixFromJSON(const nlohmann::json& jsonMatrix) { // Extract the scaling coefficient - Scalar scaling_coef = jsonMatrix["scaling_coef"]; + Scalar scaling_coef; + if (jsonMatrix.contains("scaling_coef")) { + scaling_coef = jsonMatrix["scaling_coef"]; + } + else { + scaling_coef = 1.0; + } // Initialize the Eigen matrix Eigen::Matrix matrix; // Populate the Eigen matrix with the values from the JSON, scaled by the scaling coefficient - for (int i = 0; i < Rows; ++i) { - for (int j = 0; j < Cols; ++j) { - matrix(i, j) = jsonMatrix["matrix"][i][j]; + + if (jsonMatrix.contains("matrix")) { + for (int i = 0; i < Rows; ++i) { + for (int j = 0; j < Cols; ++j) { + matrix(i, j) = jsonMatrix["matrix"][i][j]; + } } + matrix *= scaling_coef; + } + else { + throw std::runtime_error("Matrix input missing in JSON"); } - - matrix *= scaling_coef; return matrix; } - - - #endif // JSON_PARSER_HPP diff --git a/params/config/config.json b/params/config/config.json index f4ca960..d6be347 100644 --- a/params/config/config.json +++ b/params/config/config.json @@ -1,4 +1,9 @@ { - "user_defined_trajectory_file": "bean_trajectory0p6.json", - "hover_after_trajectory_time_seconds": 5.0 + "user_defined_trajectory_file": "takeoff_1meter.json", + "hover_after_trajectory_time_seconds": 5.0, + "network": { + "ground_station_ip": "127.0.0.1", + "host_ip": "0.0.0.0", + "host_port": 52000 + } } diff --git a/params/config/vehicle_info.json b/params/config/vehicle_info.json new file mode 100644 index 0000000..a56f6c4 --- /dev/null +++ b/params/config/vehicle_info.json @@ -0,0 +1,121 @@ +{ + "motorSaturationUpper":20.0, + "motorSaturationLower":0.3, + "minValuePublishMotors":0.05, + "mass":1.92, + "inertia_matrix":{ + + "scaling_coef": 2.202047e-02, + "matrix":[[1, 0, 0], + [0, 1, 0], + [0, 0, 0]] + }, + "air_density":1.225, + "surface_area":0.07, + "drag_coefficient_matrix":{ + + "scaling_coef": 1.28, + "matrix":[[1, 0, 0], + [0, 1, 0], + [0, 0, 0]] + }, + "distance_motors_centerline_x_dir":0.18, + "distance_motors_centerline_y_dir":0.18, + "propeller_drag_coefficient":0.01, + "A_filter_roll_des": { + "scaling_coef": 1.0, + "matrix": [ + [-72, -1600], + [1, 0] + ] + }, + "B_filter_roll_des": { + "scaling_coef": 1.0, + "matrix": [ + [1], + [0] + ] + }, + "C_filter_roll_des": { + "scaling_coef": 1.0, + "matrix": [ + [1600, 0] + ] + }, + "D_filter_roll_des": 0.0, + + "A_filter_pitch_des": { + "scaling_coef": 1.0, + "matrix": [ + [-72, -1600], + [1, 0] + ] + }, + "B_filter_pitch_des": { + "scaling_coef": 1.0, + "matrix": [ + [1], + [0] + ] + }, + "C_filter_pitch_des": { + "scaling_coef": 1.0, + "matrix": [ + [1600, 0] + ] + }, + "D_filter_pitch_des": 0.0, + + "A_filter_roll_dot_des": { + "scaling_coef": 1.0, + "matrix": [ + [-50, -2500], + [1, 0] + ] + }, + "B_filter_roll_dot_des": { + "scaling_coef": 1.0, + "matrix": [ + [1], + [0] + ] + }, + "C_filter_roll_dot_des": { + "scaling_coef": 0.15, + "matrix": [ + [2500, 0] + ] + }, + "D_filter_roll_dot_des": 0.0, + + "A_filter_pitch_dot_des": { + "scaling_coef": 1.0, + "matrix": [ + [-50, -2500], + [1, 0] + ] + }, + "B_filter_pitch_dot_des": { + "scaling_coef": 1.0, + "matrix": [ + [1], + [0] + ] + }, + "C_filter_pitch_dot_des": { + "scaling_coef": 0.15, + "matrix": [ + [2500, 0] + ] + }, + "D_filter_pitch_dot_des": 0.0, + "thrust_polynomial_coefficients_quadcopter":{ + "matrix":[ + [-4.351268484662274e-05, + 0.001669101414746, + 0.022577139547753, + 0.181545764715026, + 0.001722447659307] + ] + } +} diff --git a/params/user_defined_trajectory/do_nothing.json b/params/user_defined_trajectory/do_nothing.json new file mode 100644 index 0000000..c354d76 --- /dev/null +++ b/params/user_defined_trajectory/do_nothing.json @@ -0,0 +1,52 @@ +{ + "waypoints": [ + [ + 0, + 0 + ], + [ + 0, + 0 + ], + [ + 0, + 0 + ] + ], + "waypoint_times": [ + 0, + 4 + ], + "piecewise_polynomial_coefficients": [ + [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ] + ] +} diff --git a/params/user_defined_trajectory/takeoff_1meter.json b/params/user_defined_trajectory/takeoff_1meter.json index 65b09bd..e733797 100644 --- a/params/user_defined_trajectory/takeoff_1meter.json +++ b/params/user_defined_trajectory/takeoff_1meter.json @@ -15,7 +15,8 @@ ], "waypoint_times": [ 0, - 4 + 4, + 12 ], "piecewise_polynomial_coefficients": [ [ diff --git a/src/config/config.cpp b/src/config/config.cpp index e47eb88..73266ad 100644 --- a/src/config/config.cpp +++ b/src/config/config.cpp @@ -59,11 +59,11 @@ ConfigurationParameters ConfigurationParameters::readConfigurationParametersFile ConfigurationParameters config; - if (config_json.contains("user_defined_trajectory_file")) { +if (config_json.find("user_defined_trajectory_file") != config_json.end()) { config.user_defined_trajectory_file = config_json["user_defined_trajectory_file"].get(); } - if (config_json.contains("hover_after_trajectory_time_seconds")) { +if (config_json.find("hover_after_trajectory_time_seconds") != config_json.end()) { config.hover_after_trajectory_time_seconds = config_json["hover_after_trajectory_time_seconds"].get(); } diff --git a/src/control/control.cpp b/src/control/control.cpp index 8127fd2..4e0ac41 100644 --- a/src/control/control.cpp +++ b/src/control/control.cpp @@ -35,8 +35,11 @@ * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" #include "multi_threaded_node.hpp" #include "control.hpp" +#include "piecewise_polynomial_trajectory.hpp" +#include "json_parser.hpp" // Constructor @@ -88,45 +91,8 @@ Control::Control(MultiThreadedNode& node) : cr(node), node_(node) { - this->readJSONdifferentiatorFile(); -} - -/* - Function to read the differentiator gains coming from the .json file and assign it to the vehicle info - in vehicle_info.hpp -*/ -void Control::readJSONdifferentiatorFile() -{ - // Define the JSON file where the differentiator gains are located - const std::string jsonFile = "./src/flightstack/params/control/differentiator_gains.json"; - - std::ifstream file(jsonFile); - if (!file.is_open()) { - throw std::runtime_error("Could not open file: " + jsonFile); - } - - nlohmann::json j; - file >> j; - - this->vehicle_info.A_filter_roll_des = extractMatrixFromJSON(j["A_filter_roll_des"]); - this->vehicle_info.B_filter_roll_des = extractMatrixFromJSON(j["B_filter_roll_des"]); - this->vehicle_info.C_filter_roll_des = extractMatrixFromJSON(j["C_filter_roll_des"]); - this->vehicle_info.D_filter_roll_des = j["D_filter_roll_des"]; - - this->vehicle_info.A_filter_pitch_des = extractMatrixFromJSON(j["A_filter_pitch_des"]); - this->vehicle_info.B_filter_pitch_des = extractMatrixFromJSON(j["B_filter_pitch_des"]); - this->vehicle_info.C_filter_pitch_des = extractMatrixFromJSON(j["C_filter_pitch_des"]); - this->vehicle_info.D_filter_pitch_des = j["D_filter_pitch_des"]; - - this->vehicle_info.A_filter_roll_dot_des = extractMatrixFromJSON(j["A_filter_roll_dot_des"]); - this->vehicle_info.B_filter_roll_dot_des = extractMatrixFromJSON(j["B_filter_roll_dot_des"]); - this->vehicle_info.C_filter_roll_dot_des = extractMatrixFromJSON(j["C_filter_roll_dot_des"]); - this->vehicle_info.D_filter_roll_dot_des = j["D_filter_roll_dot_des"]; - - this->vehicle_info.A_filter_pitch_dot_des = extractMatrixFromJSON(j["A_filter_pitch_dot_des"]); - this->vehicle_info.B_filter_pitch_dot_des = extractMatrixFromJSON(j["B_filter_pitch_dot_des"]); - this->vehicle_info.C_filter_pitch_dot_des = extractMatrixFromJSON(j["C_filter_pitch_dot_des"]); - this->vehicle_info.D_filter_pitch_dot_des = j["D_filter_pitch_dot_des"]; + json j = readJsonFile(paramsFile); + vehicle_info = ParamsVehicle(j); } // Getter function that returns a reference to the current MultiThreadedNode object @@ -134,8 +100,8 @@ MultiThreadedNode& Control::getNode() const { return node_; } -// Getter function that returns a reference to the current VehicleInfo object -const VehicleInfo& Control::getVehicleInfo() const +// Getter function that returns a reference to the current ParamsVehicle object +const ParamsVehicle& Control::getParamsVehicle() const { return vehicle_info; } @@ -271,36 +237,44 @@ Eigen::Matrix3d Control::rotationMatrix321GlobalToLocal(const double& roll, cons thrust/motor curve obtained experimentally using the thrust stand. For QUADCOPTER */ -void Control::computeNormalizedThrustQuadcopterMode(ControlInternalMembers& cim, VehicleInfo& vehicle_info) +void Control::computeNormalizedThrustQuadcopterMode(ControlInternalMembers& cim, ParamsVehicle& vehicle_info) { /* Compute the thrust in Newton that each motor needs to produce */ - cim.thrust_vector_quadcopter = vehicle_info.mixer_matrix_quadcopter * cim.U_control_inputs; + cim.thrust_vector_quadcopter = vehicle_info.mixer_matrix_quadcopter *cim.U_control_inputs; + // std::cout << "U control Inputs: " << cim.U_control_inputs << std::endl; /* Apply upper saturation at UPPER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON and lower saturation at LOWER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON */ Eigen::Vector4d thrust_vector_quadcopter_saturated = (cim.thrust_vector_quadcopter. - cwiseMin(UPPER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON). - cwiseMax(LOWER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON)); + cwiseMin(vehicle_info.motorSaturationUpper). + cwiseMax(vehicle_info.motorSaturationLower)); + + // std::cout << "Thrust of each Motor (T1 T2 T3 T4)" << std::endl; + // std::cout << "T1: " << cim.thrust_vector_quadcopter[0] << std::endl; + // std::cout << "T2: " << cim.thrust_vector_quadcopter[1] << std::endl; + // std::cout << "T3: " << cim.thrust_vector_quadcopter[2] << std::endl; + // std::cout << "T4: " << cim.thrust_vector_quadcopter[3] << std::endl; + // std::cout << "Saturated Thrust of each Motor (T1 T2 T3 T4)" << std::endl; + // std::cout << "T1: " << thrust_vector_quadcopter_saturated[0] << std::endl; + // std::cout << "T2: " << thrust_vector_quadcopter_saturated[1] << std::endl; + // std::cout << "T3: " << thrust_vector_quadcopter_saturated[2] << std::endl; + // std::cout << "T4: " << thrust_vector_quadcopter_saturated[3] << std::endl; cim.thrust_vector_quadcopter_normalized[0] = - evaluatePolynomial(vehicle_info.thrust_polynomial_coefficients_quadcopter, - thrust_vector_quadcopter_saturated[0]); + thrust_vector_quadcopter_saturated[0]/vehicle_info.motorSaturationUpper; cim.thrust_vector_quadcopter_normalized[1] = - evaluatePolynomial(vehicle_info.thrust_polynomial_coefficients_quadcopter, - thrust_vector_quadcopter_saturated[1]); + thrust_vector_quadcopter_saturated[1]/vehicle_info.motorSaturationUpper; cim.thrust_vector_quadcopter_normalized[2] = - evaluatePolynomial(vehicle_info.thrust_polynomial_coefficients_quadcopter, - thrust_vector_quadcopter_saturated[2]); + thrust_vector_quadcopter_saturated[2]/vehicle_info.motorSaturationUpper; cim.thrust_vector_quadcopter_normalized[3] = - evaluatePolynomial(vehicle_info.thrust_polynomial_coefficients_quadcopter, - thrust_vector_quadcopter_saturated[3]); + thrust_vector_quadcopter_saturated[3]/vehicle_info.motorSaturationUpper; } @@ -310,7 +284,7 @@ void Control::computeNormalizedThrustQuadcopterMode(ControlInternalMembers& cim, thrust/motor curve obtained experimentally using the thrust stand. For X8-COPTER */ -void Control::computeNormalizedThrust(ControlInternalMembers& cim, VehicleInfo& vehicle_info) +void Control::computeNormalizedThrust(ControlInternalMembers& cim, ParamsVehicle& vehicle_info) { /* Compute the thrust in Newton that each motor needs to produce @@ -322,8 +296,8 @@ void Control::computeNormalizedThrust(ControlInternalMembers& cim, VehicleInfo& lower saturation at LOWER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON */ Eigen::Matrix thrust_vector_saturated = (cim.thrust_vector. - cwiseMin(UPPER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON). - cwiseMax(LOWER_MOTOR_THRUST_SATURATION_LIMIT_IN_NEWTON)); + cwiseMin(vehicle_info.motorSaturationUpper). + cwiseMax(vehicle_info.motorSaturationLower)); cim.thrust_vector_normalized[0] = evaluatePolynomial(vehicle_info.thrust_polynomial_coefficients_quadcopter, diff --git a/src/control/control_callback.cpp b/src/control/control_callback.cpp index 82f44e0..497cd98 100644 --- a/src/control/control_callback.cpp +++ b/src/control/control_callback.cpp @@ -38,7 +38,6 @@ #include "control.hpp" #include "multi_threaded_node.hpp" - /* Callback function that is executed at a fixed specified rate defined by the "timer_controller_" */ @@ -93,14 +92,14 @@ void MultiThreadedNode::controller_callback() // std::cout << "K_hat_r_translational " << control_->getStateController().K_hat_r_translational << std::endl; // std::cout << "thrust_vector_quadcopter " << control_->getControlInternalMembers().thrust_vector_quadcopter << std::endl; // std::cout << "thrust_vector_quadcopter_normalized " << control_->getControlInternalMembers().thrust_vector_quadcopter_normalized << std::endl; - // std::cout << "mass " << control_->getVehicleInfo().mass << std::endl; - // std::cout << "inertia_matrix " << control_->getVehicleInfo().inertia_matrix << std::endl; - // std::cout << "drag_coefficient_matrix " << control_->getVehicleInfo().drag_coefficient_matrix << std::endl; + // std::cout << "mass " << control_->getParamsVehicle().mass << std::endl; + // std::cout << "inertia_matrix " << control_->getParamsVehicle().inertia_matrix << std::endl; + // std::cout << "drag_coefficient_matrix " << control_->getParamsVehicle().drag_coefficient_matrix << std::endl; // std::cout << "r_cmd_translational " << control_->getControllerSpecificInternalMembers().r_cmd_translational << std::endl; - // std::cout << "A_filter_roll_des " << control_->getVehicleInfo().A_filter_roll_des << std::endl; - // std::cout << "B_filter_roll_des " << control_->getVehicleInfo().B_filter_roll_des << std::endl; - // std::cout << "C_filter_roll_des " << control_->getVehicleInfo().C_filter_roll_des << std::endl; - // std::cout << "D_filter_roll_des " << control_->getVehicleInfo().D_filter_roll_des << std::endl; + // std::cout << "A_filter_roll_des " << control_->getParamsVehicle().A_filter_roll_des << std::endl; + // std::cout << "B_filter_roll_des " << control_->getParamsVehicle().B_filter_roll_des << std::endl; + // std::cout << "C_filter_roll_des " << control_->getParamsVehicle().C_filter_roll_des << std::endl; + // std::cout << "D_filter_roll_des " << control_->getParamsVehicle().D_filter_roll_des << std::endl; // std::cout << "state_roll_des_filter " << control_->getStateController().state_roll_des_filter << std::endl; // std::cout << "KP_translational " << control_->getGains().KP_translational << std::endl; diff --git a/src/control/mrac/logging_mrac.cpp b/src/control/mrac/logging_mrac.cpp index 7fc7b90..760588e 100644 --- a/src/control/mrac/logging_mrac.cpp +++ b/src/control/mrac/logging_mrac.cpp @@ -33,6 +33,9 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" + +#if SELECTED_CONTROLLER == __MRAC__ #include "mrac.hpp" #include "logging_mrac.hpp" @@ -289,20 +292,20 @@ void LogData_MRAC::logInitializeLogging() std::string log_base_directory = "./src/flightstack/log/" + current_date + "/" + ControlType::getControllerName(); // Check if the directory exists, and create it if it doesn't - if (!std::filesystem::exists(log_base_directory)) { - std::filesystem::create_directories(log_base_directory); + if (!fs::exists(log_base_directory)) { + fs::create_directories(log_base_directory); } // Check if the "logs" subdirectory exists within the log_base_directory, create it if needed std::string logs_directory = log_base_directory + "/logs"; - if (!std::filesystem::exists(logs_directory)) { - std::filesystem::create_directories(logs_directory); + if (!fs::exists(logs_directory)) { + fs::create_directories(logs_directory); } // Check if the "gains" subdirectory exists within the log_base_directory, create it if needed std::string gains_directory = log_base_directory + "/gains"; - if (!std::filesystem::exists(gains_directory)) { - std::filesystem::create_directories(gains_directory); + if (!fs::exists(gains_directory)) { + fs::create_directories(gains_directory); } // Generate the log file name with a timestamp @@ -345,9 +348,9 @@ void LogData_MRAC::logInitializeLogging() target_ss << gains_directory << "/gains_mrac_" << std::put_time(std::localtime(&now_c), "%Y%m%d_%H%M%S") << ".json"; std::string target_file = target_ss.str(); - if (std::filesystem::exists(source_file)) { + if (fs::exists(source_file)) { std::cout << "GAINS FILE PRESENT" << std::endl; - std::filesystem::copy(source_file, target_file); + fs::copy(source_file, target_file); } else { std::cout << "GAINS FILE NOT PRESENT" << std::endl; } @@ -580,3 +583,4 @@ void LogData_MRAC::logLogData() BOOST_LOG(LogData_MRAC::logger_logdata) << oss.str(); } +#endif //SELECTED_CONTROLLER == __MRAC__ \ No newline at end of file diff --git a/src/control/mrac/mrac.cpp b/src/control/mrac/mrac.cpp index dabc621..c537b64 100644 --- a/src/control/mrac/mrac.cpp +++ b/src/control/mrac/mrac.cpp @@ -33,18 +33,24 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" + +#if SELECTED_CONTROLLER == __MRAC__ #include "multi_threaded_node.hpp" #include "mrac.hpp" #include "logging_mrac.hpp" #include "json_parser.hpp" +using namespace nlohmann; + // Constructor MRAC::MRAC(MultiThreadedNode& node) : Control(node) { // Initialize controller gains - this->readJSONfile("gains_mrac.json"); + json j = readJsonFile("./src/flightstack/params/control/mrac/gains_mrac.json"); + this->loadGains(j); this->initializeControllerParameters(this->vehicle_info, this->gains_); log_data_ = std::make_shared(node, *this); @@ -148,22 +154,9 @@ const std::string& MRAC::getControllerName() Function to read the tuning gains coming from the .json file and assign it to the gains_ struct instantiated in the MRAC class */ -void MRAC::readJSONfile(const std::string& fileName) -{ - // Define the path where the MRAC gains JSON files are located - const std::string path = "./src/flightstack/params/control/mrac/"; - - // Concatenate the path with the file name - std::string jsonFile = path + fileName; - - std::ifstream file(jsonFile); - if (!file.is_open()) { - throw std::runtime_error("Could not open file: " + jsonFile); - } - nlohmann::json j; - file >> j; - +void MRAC::loadGains(json j){ + gains_.KP_refmod_translational = extractMatrixFromJSON(j["BASELINE"]["KP_refmod_translational"]); gains_.KD_refmod_translational = extractMatrixFromJSON(j["BASELINE"]["KD_refmod_translational"]); gains_.KI_refmod_translational = extractMatrixFromJSON(j["BASELINE"]["KI_refmod_translational"]); @@ -213,13 +206,12 @@ void MRAC::readJSONfile(const std::string& fileName) gains_.x_e_Theta_rotational = extractMatrixFromJSON(j["ADAPTIVE"]["x_e_Theta_rotational"]); gains_.S_diagonal_Theta_rotational = extractMatrixFromJSON(j["ADAPTIVE"]["S_diagonal_Theta_rotational"]); gains_.alpha_Theta_rotational = j["ADAPTIVE"]["alpha_Theta_rotational"]; - } /* Function that given the gains read from the JSON file, initializes the rest of the parameters accordingly */ -void MRAC::initializeControllerParameters(VehicleInfo& vehicle_info, GainsMRAC& gains_) +void MRAC::initializeControllerParameters(ParamsVehicle& vehicle_info, GainsMRAC& gains_) { // Initialize to zero the 6x6 matrix and set the top-right 3x3 block as follows gains_.A_translational.setZero(); @@ -277,7 +269,6 @@ void MRAC::initializeControllerParameters(VehicleInfo& vehicle_info, GainsMRAC& gains_.epsilon_x_rotational = projection_operator::computeEpsilonFromAlpha(gains_.alpha_x_rotational); gains_.epsilon_r_rotational = projection_operator::computeEpsilonFromAlpha(gains_.alpha_r_rotational); gains_.epsilon_Theta_rotational = projection_operator::computeEpsilonFromAlpha(gains_.alpha_Theta_rotational); - } /* @@ -327,7 +318,7 @@ void MRAC::assignSystemToDxdt(state_type /* &x */, state_type &dxdt, const doubl desired angles using the provided filter coefficients and the current state variables. */ void MRAC::computeFilterDifferentiatorVariables(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_) { cim.internal_state_roll_des_filter = vehicle_info.A_filter_roll_des * state_.state_roll_des_filter @@ -354,7 +345,7 @@ void MRAC::computeFilterDifferentiatorVariables(ControlInternalMembers& cim, OUTER LOOP CONTROLLER */ void MRAC::computeOuterLoop(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_, ControlReferences& cr, GainsMRAC& gains_, @@ -482,7 +473,7 @@ void MRAC::computeOuterLoop(ControlInternalMembers& cim, OUTER LOOP CONTROLLER USED FOR DEBUGGING, using 'user' instead of 'reference-model' in the baseline */ void MRAC::computeOuterLoopDEBUGGING(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_, ControlReferences& cr, GainsMRAC& gains_, @@ -611,7 +602,7 @@ void MRAC::computeOuterLoopDEBUGGING(ControlInternalMembers& cim, INNER LOOP CONTROLLER */ void MRAC::computeInnerLoop(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_, ControlReferences& cr, GainsMRAC& gains_, @@ -764,7 +755,7 @@ void MRAC::computeInnerLoop(ControlInternalMembers& cim, INNER LOOP CONTROLLER USED FOR DEBUGGING, using 'desired' instead of 'reference-model' in the baseline */ void MRAC::computeInnerLoopDEBUGGING(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_, ControlReferences& cr, GainsMRAC& gains_, @@ -967,3 +958,5 @@ void MRAC::computeControlAlgorithm() // this->computeNormalizedThrustQuadcopterMode(cim, vehicle_info); } + +#endif //SELECTED_CONTROLLER == __MRAC__ \ No newline at end of file diff --git a/src/control/pid/logging_pid.cpp b/src/control/pid/logging_pid.cpp index a060a80..7af595c 100644 --- a/src/control/pid/logging_pid.cpp +++ b/src/control/pid/logging_pid.cpp @@ -33,11 +33,53 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" + +#if SELECTED_CONTROLLER == __PID__ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "pid.hpp" #include "logging_pid.hpp" #include "multi_threaded_node.hpp" +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; +namespace fs = std::experimental::filesystem; + // Define the logger for LogData src::logger LogData_PID::logger_logdata; @@ -176,20 +218,20 @@ void LogData_PID::logInitializeLogging() std::string log_base_directory = "./src/flightstack/log/" + current_date + "/" + ControlType::getControllerName(); // Check if the directory exists, and create it if it doesn't - if (!std::filesystem::exists(log_base_directory)) { - std::filesystem::create_directories(log_base_directory); + if (!fs::exists(log_base_directory)) { + fs::create_directories(log_base_directory); } // Check if the "logs" subdirectory exists within the log_base_directory, create it if needed std::string logs_directory = log_base_directory + "/logs"; - if (!std::filesystem::exists(logs_directory)) { - std::filesystem::create_directories(logs_directory); + if (!fs::exists(logs_directory)) { + fs::create_directories(logs_directory); } // Check if the "gains" subdirectory exists within the log_base_directory, create it if needed std::string gains_directory = log_base_directory + "/gains"; - if (!std::filesystem::exists(gains_directory)) { - std::filesystem::create_directories(gains_directory); + if (!fs::exists(gains_directory)) { + fs::create_directories(gains_directory); } // Generate the log file name with a timestamp @@ -227,14 +269,14 @@ void LogData_PID::logInitializeLogging() logging::add_common_attributes(); // Add attributes like timestamp // Copy the gains file to the gains_directory with a new name containing the time info - std::string source_file = "./src/flightstack/params/control/pid/gains_pid.json"; + std::string source_file = "./src/flightstack/params/control/pid/pid_gains.json"; std::stringstream target_ss; target_ss << gains_directory << "/gains_pid_" << std::put_time(std::localtime(&now_c), "%Y%m%d_%H%M%S") << ".json"; std::string target_file = target_ss.str(); - if (std::filesystem::exists(source_file)) { + if (fs::exists(source_file)) { std::cout << "GAINS FILE PRESENT" << std::endl; - std::filesystem::copy(source_file, target_file); + fs::copy(source_file, target_file); } else { std::cout << "GAINS FILE NOT PRESENT" << std::endl; } @@ -354,3 +396,4 @@ void LogData_PID::logLogData() BOOST_LOG(LogData_PID::logger_logdata) << oss.str(); } +#endif //SELECTED_CONTROLLER == __PID__ \ No newline at end of file diff --git a/src/control/pid/pid.cpp b/src/control/pid/pid.cpp index 047c3e1..d91ab4f 100644 --- a/src/control/pid/pid.cpp +++ b/src/control/pid/pid.cpp @@ -33,11 +33,14 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" +#if SELECTED_CONTROLLER == __PID__ #include "multi_threaded_node.hpp" #include "pid.hpp" #include "logging_pid.hpp" #include "json_parser.hpp" +using namespace nlohmann; // Constructor PID::PID(MultiThreadedNode& node) : @@ -126,7 +129,7 @@ void PID::readJSONfile(const std::string& fileName) throw std::runtime_error("Could not open file: " + jsonFile); } - nlohmann::json j; + json j; file >> j; @@ -172,7 +175,7 @@ void PID::assignSystemToDxdt(state_type /* &x */, state_type &dxdt, const double angles using the provided filter coefficients and the current state variables. */ void PID::computeFilterDifferentiatorVariables(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_) { cim.internal_state_roll_des_filter = vehicle_info.A_filter_roll_des * state_.state_roll_des_filter @@ -199,7 +202,7 @@ void PID::computeFilterDifferentiatorVariables(ControlInternalMembers& cim, OUTER LOOP CONTROLLER */ void PID::computeOuterLoop(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_, ControlReferences& cr, GainsPID& gains_, @@ -217,13 +220,15 @@ void PID::computeOuterLoop(ControlInternalMembers& cim, + csim_.outer_loop_I + cr.user_defined_acceleration ); + + } /* INNER LOOP CONTROLLER */ void PID::computeInnerLoop(ControlInternalMembers& cim, - VehicleInfo& vehicle_info, + ParamsVehicle& vehicle_info, StateController& state_, ControlReferences& cr, GainsPID& gains_, @@ -293,10 +298,32 @@ void PID::computeControlAlgorithm() Compute the thrust in Newton that each motor needs to produce and convert it from Newton to the normalized value comprised between 0 and 1 to be sent to Pixhawk */ - // FOR X8COPTER - this->computeNormalizedThrust(cim, vehicle_info); +#if VEH_ARCH == ARCH_X8 + // FOR X8 + this->computeNormalizedThrust(cim, vehicle_info); +#elif VEH_ARCH == ARCH_QUAD // FOR QUADCOPTER - // this->computeNormalizedThrustQuadcopterMode(cim, vehicle_info); + this->computeNormalizedThrustQuadcopterMode(cim, vehicle_info); +#else + // Invalid architecture selection + #error "Invalid architecture selection" +#endif + + // Debug output: Print position error, motor input, and velocity for each axis + + std::cout << "Vector Inputs (U1, U2, U3, U4):" << std::endl; + std::cout << " U1 (Total Thrust): " << cim.U_control_inputs[0] << std::endl; + std::cout << " U2 (Roll Moment): " << cim.U_control_inputs[1] << std::endl; + std::cout << " U3 (Pitch Moment): " << cim.U_control_inputs[2] << std::endl; + std::cout << " U4 (Yaw Moment): " << cim.U_control_inputs[3] << std::endl; + + std::cout << "Motor Inputs (U1, U2, U3, U4):" << std::endl; + std::cout << " M1 (Motor 1): " << cim.thrust_vector_quadcopter_normalized[0] << std::endl; + std::cout << " M2 (Motor 2): " << cim.thrust_vector_quadcopter_normalized[1] << std::endl; + std::cout << " M3 (Motor 3): " << cim.thrust_vector_quadcopter_normalized[2] << std::endl; + std::cout << " M4 (Motor 4): " << cim.thrust_vector_quadcopter_normalized[3] << std::endl; + } +#endif //SELECTED_CONTROLLER == __PID__ \ No newline at end of file diff --git a/src/control/vehicleParams.cpp b/src/control/vehicleParams.cpp new file mode 100644 index 0000000..e839d07 --- /dev/null +++ b/src/control/vehicleParams.cpp @@ -0,0 +1,112 @@ + +#include +#include + +#include +#include +#include "vehicleParams.hpp" + +using namespace Eigen; +using namespace std; + +ParamsVehicle::ParamsVehicle(){} //default constructor +ParamsVehicle::ParamsVehicle(json j) { + + motorSaturationUpper = j.at("motorSaturationUpper").get(); + motorSaturationLower = j.at("motorSaturationLower").get(); + minValuePublishMotors = j.at("minValuePublishMotors").get(); + mass = j.at("mass").get(); + inertia_matrix = extractMatrixFromJSON(j["inertia_matrix"]); + air_density = j.at("air_density").get(); + surface_area = j.at("surface_area").get();\ + drag_coefficient_matrix = extractMatrixFromJSON(j["drag_coefficient_matrix"]); + + A_filter_roll_des = extractMatrixFromJSON(j["A_filter_roll_des"]); + B_filter_roll_des = extractMatrixFromJSON(j["B_filter_roll_des"]); + C_filter_roll_des = extractMatrixFromJSON(j["C_filter_roll_des"]); + D_filter_roll_des = j["D_filter_roll_des"]; + + A_filter_pitch_des = extractMatrixFromJSON(j["A_filter_pitch_des"]); + B_filter_pitch_des = extractMatrixFromJSON(j["B_filter_pitch_des"]); + C_filter_pitch_des = extractMatrixFromJSON(j["C_filter_pitch_des"]); + D_filter_pitch_des = j["D_filter_pitch_des"]; + + A_filter_roll_dot_des = extractMatrixFromJSON(j["A_filter_roll_dot_des"]); + B_filter_roll_dot_des = extractMatrixFromJSON(j["B_filter_roll_dot_des"]); + C_filter_roll_dot_des = extractMatrixFromJSON(j["C_filter_roll_dot_des"]); + D_filter_roll_dot_des = j["D_filter_roll_dot_des"]; + + A_filter_pitch_dot_des = extractMatrixFromJSON(j["A_filter_pitch_dot_des"]); + B_filter_pitch_dot_des = extractMatrixFromJSON(j["B_filter_pitch_dot_des"]); + C_filter_pitch_dot_des = extractMatrixFromJSON(j["C_filter_pitch_dot_des"]); + + distance_motors_centerline_x_dir = j.at("distance_motors_centerline_x_dir").get(); + distance_motors_centerline_y_dir = j.at("distance_motors_centerline_y_dir").get(); + propeller_drag_coefficient = j.at("propeller_drag_coefficient").get(); + + // Assign values element-by-element + mixer_matrix(0, 0) = 1.0 / 8.0; + mixer_matrix(0, 1) = -1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(0, 2) = 1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(0, 3) = 1.0 / (8.0 * propeller_drag_coefficient); + + mixer_matrix(1, 0) = 1.0 / 8.0; + mixer_matrix(1, 1) = 1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(1, 2) = 1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(1, 3) = -1.0 / (8.0 * propeller_drag_coefficient); + + mixer_matrix(2, 0) = 1.0 / 8.0; + mixer_matrix(2, 1) = 1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(2, 2) = -1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(2, 3) = 1.0 / (8.0 * propeller_drag_coefficient); + + mixer_matrix(3, 0) = 1.0 / 8.0; + mixer_matrix(3, 1) = -1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(3, 2) = -1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(3, 3) = -1.0 / (8.0 * propeller_drag_coefficient); + + mixer_matrix(4, 0) = 1.0 / 8.0; + mixer_matrix(4, 1) = 1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(4, 2) = 1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(4, 3) = 1.0 / (8.0 * propeller_drag_coefficient); + + mixer_matrix(5, 0) = 1.0 / 8.0; + mixer_matrix(5, 1) = -1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(5, 2) = 1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(5, 3) = -1.0 / (8.0 * propeller_drag_coefficient); + + mixer_matrix(6, 0) = 1.0 / 8.0; + mixer_matrix(6, 1) = -1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(6, 2) = -1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(6, 3) = 1.0 / (8.0 * propeller_drag_coefficient); + + mixer_matrix(7, 0) = 1.0 / 8.0; + mixer_matrix(7, 1) = 1.0 / (8.0 * distance_motors_centerline_y_dir); + mixer_matrix(7, 2) = -1.0 / (8.0 * distance_motors_centerline_x_dir); + mixer_matrix(7, 3) = -1.0 / (8.0 * propeller_drag_coefficient); + + // Assign values element-by-element + mixer_matrix_quadcopter(0, 0) = 1.0 / 4.0; + mixer_matrix_quadcopter(0, 1) = -1.0 / (4.0 * distance_motors_centerline_y_dir); + mixer_matrix_quadcopter(0, 2) = 1.0 / (4.0 * distance_motors_centerline_x_dir); + mixer_matrix_quadcopter(0, 3) = -1.0 / (4.0 * propeller_drag_coefficient); + + mixer_matrix_quadcopter(1, 0) = 1.0 / 4.0; + mixer_matrix_quadcopter(1, 1) = 1.0 / (4.0 * distance_motors_centerline_y_dir); + mixer_matrix_quadcopter(1, 2) = -1.0 / (4.0 * distance_motors_centerline_x_dir); + mixer_matrix_quadcopter(1, 3) = -1.0 / (4.0 * propeller_drag_coefficient); + + mixer_matrix_quadcopter(2, 0) = 1.0 / 4.0; + mixer_matrix_quadcopter(2, 1) = 1.0 / (4.0 * distance_motors_centerline_y_dir); + mixer_matrix_quadcopter(2, 2) = 1.0 / (4.0 * distance_motors_centerline_x_dir); + mixer_matrix_quadcopter(2, 3) = 1.0 / (4.0 * propeller_drag_coefficient); + + mixer_matrix_quadcopter(3, 0) = 1.0 / 4.0; + mixer_matrix_quadcopter(3, 1) = -1.0 / (4.0 * distance_motors_centerline_y_dir); + mixer_matrix_quadcopter(3, 2) = -1.0 / (4.0 * distance_motors_centerline_x_dir); + mixer_matrix_quadcopter(3, 3) = 1.0 / (4.0 * propeller_drag_coefficient); + + // Polynomial coefficients vector to evaluate the Commanded Thrust [-] based on the Thrust in Newton + // S500 with Holybro props + thrust_polynomial_coefficients_quadcopter = extractMatrixFromJSON(j["thrust_polynomial_coefficients_quadcopter"]); +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index b4b3751..9df38f2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -35,43 +35,42 @@ **********************************************************************************************************************/ #include "main.hpp" +#include "config.hpp" +#include "multi_threaded_node.hpp" int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - if constexpr (CURRENT_EKF2_FUSION_MODE == EKF2FusionMode::GPS) - { - // You MUST use the MultiThreadedExecutor to use, well, multiple threads - rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::NodeOptions options; - auto multi_threaded_node = std::make_shared(options); - executor.add_node(multi_threaded_node); +#if EKF2_FUSION_MODE == FUSION_MODE_GPS + // You MUST use the MultiThreadedExecutor to use, well, multiple threads + rclcpp::executors::MultiThreadedExecutor executor; + rclcpp::NodeOptions options; + auto multi_threaded_node = std::make_shared(options); + executor.add_node(multi_threaded_node); - std::cout << "CURRENT_EKF2_FUSION_MODE = GPS" << std::endl; + std::cout << "CURRENT_EKF2_FUSION_MODE = GPS" << std::endl; - executor.spin(); - rclcpp::shutdown(); + executor.spin(); + rclcpp::shutdown(); - } else if constexpr (CURRENT_EKF2_FUSION_MODE == EKF2FusionMode::MOCAP) - { - // You MUST use the MultiThreadedExecutor to use, well, multiple threads - rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::NodeOptions options; - auto multi_threaded_node = std::make_shared(options); - executor.add_node(multi_threaded_node); +#elif EKF2_FUSION_MODE == FUSION_MODE_MOCAP + // You MUST use the MultiThreadedExecutor to use, well, multiple threads + rclcpp::executors::MultiThreadedExecutor executor; + rclcpp::NodeOptions options; + auto multi_threaded_node = std::make_shared(options); + executor.add_node(multi_threaded_node); - std::cout << "CURRENT_EKF2_FUSION_MODE = MOCAP" << std::endl; - _drivers_::_common_::IoContext ctx{1}; - auto mocap_node = std::make_shared<_drivers_::_udp_driver_::UdpReceiverNode>( - ctx, - multi_threaded_node->getInitialTimestamp()); - executor.add_node(mocap_node->get_node_base_interface()); - mocap_node->checkLifecycleNodeStarted(); - - executor.spin(); - rclcpp::shutdown(); - } + std::cout << "CURRENT_EKF2_FUSION_MODE = MOCAP" << std::endl; + _drivers_::_common_::IoContext ctx{1}; + auto mocap_node = std::make_shared<_drivers_::_udp_driver_::UdpReceiverNode>( + ctx, + multi_threaded_node->getInitialTimestamp()); + executor.add_node(mocap_node->get_node_base_interface()); + mocap_node->checkLifecycleNodeStarted(); + executor.spin(); + rclcpp::shutdown(); +#endif //ENABLE_MOCAP == true return 0; } diff --git a/src/pixhawk_interface/pixhawk_actuator_motors.cpp b/src/pixhawk_interface/pixhawk_actuator_motors.cpp index 14fd742..764fd09 100644 --- a/src/pixhawk_interface/pixhawk_actuator_motors.cpp +++ b/src/pixhawk_interface/pixhawk_actuator_motors.cpp @@ -34,7 +34,8 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ - +#include "config.hpp" +#include "control.hpp" #include "multi_threaded_node.hpp" /* @@ -128,28 +129,17 @@ void MultiThreadedNode::publish_actuator_motors() if (time_current_ < global_params_.TAKEOFF_START_TIME_SECONDS || time_current_ > global_params_.DISARM_START_TIME_SECONDS) { - msg.control[0] = MINIMUM_VALUE_PUBLISH_MOTORS; - msg.control[1] = MINIMUM_VALUE_PUBLISH_MOTORS; - msg.control[2] = MINIMUM_VALUE_PUBLISH_MOTORS; - msg.control[3] = MINIMUM_VALUE_PUBLISH_MOTORS; - msg.control[4] = MINIMUM_VALUE_PUBLISH_MOTORS; - msg.control[5] = MINIMUM_VALUE_PUBLISH_MOTORS; - msg.control[6] = MINIMUM_VALUE_PUBLISH_MOTORS; - msg.control[7] = MINIMUM_VALUE_PUBLISH_MOTORS; + msg.control[0] = this->getControl()->getParamsVehicle().minValuePublishMotors; + msg.control[1] = this->getControl()->getParamsVehicle().minValuePublishMotors; + msg.control[2] = this->getControl()->getParamsVehicle().minValuePublishMotors; + msg.control[3] = this->getControl()->getParamsVehicle().minValuePublishMotors; + msg.control[4] = this->getControl()->getParamsVehicle().minValuePublishMotors; + msg.control[5] = this->getControl()->getParamsVehicle().minValuePublishMotors; + msg.control[6] = this->getControl()->getParamsVehicle().minValuePublishMotors; + msg.control[7] = this->getControl()->getParamsVehicle().minValuePublishMotors; } else { - /* // QUADCOPTER - msg.control[0] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[0]; - msg.control[1] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[1]; - msg.control[2] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[2]; - msg.control[3] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[3]; - msg.control[4] = 0.0; - msg.control[5] = 0.0; - msg.control[6] = 0.0; - msg.control[7] = 0.0; - */ - - // X8-COPTER +#if VEH_ARCH == ARCH_X8 msg.control[0] = this->getControl()->getControlInternalMembers().thrust_vector_normalized[0]; msg.control[1] = this->getControl()->getControlInternalMembers().thrust_vector_normalized[1]; msg.control[2] = this->getControl()->getControlInternalMembers().thrust_vector_normalized[2]; @@ -158,6 +148,20 @@ void MultiThreadedNode::publish_actuator_motors() msg.control[5] = this->getControl()->getControlInternalMembers().thrust_vector_normalized[5]; msg.control[6] = this->getControl()->getControlInternalMembers().thrust_vector_normalized[6]; msg.control[7] = this->getControl()->getControlInternalMembers().thrust_vector_normalized[7]; + +#elif VEH_ARCH == ARCH_QUAD + msg.control[0] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[0]; + msg.control[1] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[1]; + msg.control[2] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[2]; + msg.control[3] = this->getControl()->getControlInternalMembers().thrust_vector_quadcopter_normalized[3]; + + msg.control[4] = 0.0; + msg.control[5] = 0.0; + msg.control[6] = 0.0; + msg.control[7] = 0.0; +#else + #error "Invalid architecture selection" +#endif } msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; diff --git a/src/user_defined_trajectory/piecewise_polynomial_trajectory.cpp b/src/user_defined_trajectory/piecewise_polynomial_trajectory.cpp index a9c9bca..b8a3928 100644 --- a/src/user_defined_trajectory/piecewise_polynomial_trajectory.cpp +++ b/src/user_defined_trajectory/piecewise_polynomial_trajectory.cpp @@ -35,6 +35,7 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" #include #include diff --git a/src/user_defined_trajectory/user_defined_trajectory.cpp b/src/user_defined_trajectory/user_defined_trajectory.cpp index 868f166..c3a6d14 100644 --- a/src/user_defined_trajectory/user_defined_trajectory.cpp +++ b/src/user_defined_trajectory/user_defined_trajectory.cpp @@ -33,6 +33,7 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" #include "user_defined_trajectory.hpp" #include "multi_threaded_node.hpp" diff --git a/src/utils/continuous_lyapunov_equation.cpp b/src/utils/continuous_lyapunov_equation.cpp index afbf33d..4a301a7 100644 --- a/src/utils/continuous_lyapunov_equation.cpp +++ b/src/utils/continuous_lyapunov_equation.cpp @@ -77,6 +77,9 @@ * * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ +#include "config.hpp" + +#if SELECTED_CONTROLLER == __MRAC__ #include #include @@ -312,3 +315,5 @@ MatrixXd SolveReducedRealContinuousLyapunovEquation( } } // namespace internal + +#endif //SELECTED_CONTROLLER == __MRAC__ \ No newline at end of file diff --git a/src/utils/json_parser.cpp b/src/utils/json_parser.cpp index 4f52764..010af50 100644 --- a/src/utils/json_parser.cpp +++ b/src/utils/json_parser.cpp @@ -34,14 +34,12 @@ * GitHub: https://github.com/andrealaffly/ACSL-flightstack.git **********************************************************************************************************************/ -#include -#include - #include "json_parser.hpp" +using namespace nlohmann; void MotorsCommands::readJSONConfig(const std::string& jsonFile) { std::ifstream file(jsonFile); - nlohmann::json j; + json j; file >> j; this->motor_1 = j["MotorsCommands"]["motor_1"]; @@ -55,4 +53,15 @@ void MotorsCommands::readJSONConfig(const std::string& jsonFile) { } +json readJsonFile(const std::string& jsonFile) +{ + json j; + + std::ifstream file(jsonFile); + if (!file.is_open()) { + throw std::runtime_error("Could not open file: " + jsonFile); + } + file >> j; + return j; +} \ No newline at end of file