diff --git a/include/aero_parameters.h b/include/aero_parameters.h index 46150d92..bda1d2db 100644 --- a/include/aero_parameters.h +++ b/include/aero_parameters.h @@ -129,11 +129,6 @@ struct RotorParameters { double vertical_rot_drag_lin{0.07444735702448266}; double vertical_rot_thrust_lin{-0.0017229667485354344}; double vertical_rot_thrust_quad{4.0095427586089745}; - double c_m_leaver_quad{0.0}; - double c_m_leaver_lin{0.0}; - double c_m_drag_z_quad{0.0}; - double c_m_drag_z_lin{0.0}; - double c_m_rolling{0.0}; }; struct FWAerodynamicParameters { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -166,11 +161,6 @@ struct FWAerodynamicParameters { double vertical_rot_drag_lin{0.07444735702448266}; double vertical_rot_thrust_lin{-0.0017229667485354344}; double vertical_rot_thrust_quad{4.0095427586089745}; - double vertical_c_m_drag_z_lin{-10.324851252250626}; - double vertical_c_m_drag_z_quad{6.0213939854338685}; - double vertical_c_m_leaver_lin{-8.33722923229799}; - double vertical_c_m_leaver_quad{32.623014913712176}; - double vertical_c_m_rolling{-1.467193735480539}; double alpha_max; double alpha_min; @@ -218,12 +208,6 @@ struct FWAerodynamicParameters { READ_PARAM(coefficients, vertical_rot_drag_lin); READ_PARAM(coefficients, vertical_rot_thrust_lin); READ_PARAM(coefficients, vertical_rot_thrust_quad); - READ_PARAM(coefficients, vertical_c_m_leaver_lin); - READ_PARAM(coefficients, vertical_c_m_drag_z_lin); - READ_PARAM(coefficients, vertical_c_m_drag_z_quad); - READ_PARAM(coefficients, vertical_c_m_leaver_lin); - READ_PARAM(coefficients, vertical_c_m_leaver_quad); - READ_PARAM(coefficients, vertical_c_m_rolling); const YAML::Node configs = node["model"]; /// TODO: iterate through yaml files and append rotor elements to model diff --git a/include/parametric_dynamics_model.h b/include/parametric_dynamics_model.h index d53968fd..7d4c5494 100644 --- a/include/parametric_dynamics_model.h +++ b/include/parametric_dynamics_model.h @@ -50,7 +50,7 @@ class ParametricDynamicsModel { }; private: - void computeRotorFeatures(const Eigen::Vector3d airspeed, const Eigen::VectorXd &actuator_inputs, Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment); Eigen::Vector3d computeRotorForce(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params); + void computeTotalRotorWrench(const Eigen::Vector3d airspeed, const Eigen::VectorXd &actuator_inputs, Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment); Eigen::Vector3d computeRotorForce(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params); Eigen::Vector3d computeRotorMoment(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params, Eigen::Vector3d rotor_force); Eigen::Vector3d force_{Eigen::Vector3d::Zero()}; Eigen::Vector3d moment_{Eigen::Vector3d::Zero()}; diff --git a/src/parametric_dynamics_model.cpp b/src/parametric_dynamics_model.cpp index 01b7796e..6d5d9d36 100644 --- a/src/parametric_dynamics_model.cpp +++ b/src/parametric_dynamics_model.cpp @@ -42,14 +42,14 @@ void ParametricDynamicsModel::setState(const ignition::math::Vector3d &B_air_spe Eigen::Vector3d force_rotor_B{Eigen::Vector3d::Zero()}; Eigen::Vector3d moment_rotor_B{Eigen::Vector3d::Zero()}; - computeRotorFeatures(ignition2eigen(B_air_speed_W_B), actuator_inputs, force_rotor_B, moment_rotor_B); + computeTotalRotorWrench(ignition2eigen(B_air_speed_W_B), actuator_inputs, force_rotor_B, moment_rotor_B); // Transform all the forces and moments into the body frame force_ = force_rotor_B; moment_ = moment_rotor_B; } -void ParametricDynamicsModel::computeRotorFeatures(const Eigen::Vector3d airspeed, const Eigen::VectorXd &actuator_inputs, Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment) { +void ParametricDynamicsModel::computeTotalRotorWrench(const Eigen::Vector3d airspeed, const Eigen::VectorXd &actuator_inputs, Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment) { rotor_force = Eigen::Vector3d::Zero(); rotor_moment = Eigen::Vector3d::Zero(); @@ -86,12 +86,12 @@ Eigen::Vector3d ParametricDynamicsModel::computeRotorForce(const Eigen::Vector3d Eigen::Vector3d ParametricDynamicsModel::computeRotorMoment(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params, Eigen::Vector3d rotor_force) { // Thrust force computation - const double prop_diameter = rotor_params.diameter; - const double c_m_leaver_quad = rotor_params.c_m_leaver_quad; - const double c_m_leaver_lin = rotor_params.c_m_leaver_lin; - const double c_m_drag_z_quad = rotor_params.c_m_drag_z_quad; - const double c_m_drag_z_lin = rotor_params.c_m_drag_z_lin; - const double c_m_rolling = rotor_params.c_m_rolling; + // const double prop_diameter = rotor_params.diameter; + // const double c_m_leaver_quad = rotor_params.c_m_leaver_quad; + // const double c_m_leaver_lin = rotor_params.c_m_leaver_lin; + // const double c_m_drag_z_quad = rotor_params.c_m_drag_z_quad; + // const double c_m_drag_z_lin = rotor_params.c_m_drag_z_lin; + // const double c_m_rolling = rotor_params.c_m_rolling; Eigen::Vector3d rotor_axis = (rotor_params.rotor_axis).normalized(); Eigen::Vector3d rotor_position = rotor_params.position;